//
// Created by zx on 2021/12/15.
//

#include "LivoxMid70.h"
namespace livox
{

TimedLivoxExtendRawPoint::TimedLivoxExtendRawPoint()
{

    m_point_num=0;
    m_frame= nullptr;
    m_timeout=0;
    m_time_point=std::chrono::steady_clock::now();
}

void TimedLivoxExtendRawPoint::Reset(LivoxExtendRawPoint* data ,int count,float timeout_time){
    if(m_frame!=nullptr)
    {
        free(m_frame);
    }
    m_frame=(LivoxExtendRawPoint*)malloc(sizeof(LivoxExtendRawPoint)*count);
    m_point_num=count;
    memcpy(m_frame,data,count*sizeof(LivoxExtendRawPoint));
    m_time_point=std::chrono::steady_clock::now();
    m_timeout=timeout_time;
}
TimedLivoxExtendRawPoint::~TimedLivoxExtendRawPoint()
{
    if(m_frame!=nullptr)
    {
        free(m_frame);
        m_frame=nullptr;
        m_point_num=0;
    }
}

void TimedLivoxExtendRawPoint::operator=(const TimedLivoxExtendRawPoint& frame)
{
    if(m_frame!=nullptr)
    {
        free(m_frame);
    }
    m_point_num=frame.m_point_num;
    m_frame=(LivoxExtendRawPoint*)malloc(sizeof(LivoxExtendRawPoint)*frame.m_point_num);
    memcpy(m_frame,frame.m_frame,m_point_num*sizeof(LivoxExtendRawPoint));
    m_time_point=frame.m_time_point;
    m_timeout=frame.m_timeout;
}

bool TimedLivoxExtendRawPoint::is_timeout(){
    auto tic=std::chrono::steady_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(tic - m_time_point);

    double tm=double(duration.count()) * std::chrono::microseconds::period::num /
            std::chrono::microseconds::period::den;

    return tm>m_timeout;
}


LivoxMid70::LivoxMid70(){

    m_idx=0;
    m_fps=2.0;
    m_transform=Eigen::Affine3f::Identity();
}
LivoxMid70::~LivoxMid70(){}


bool LivoxMid70::is_connected()
{
    auto tic=std::chrono::steady_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(tic - m_last_data_tp);

    double tm=double(duration.count()) * std::chrono::microseconds::period::num /
            std::chrono::microseconds::period::den;
    return tm<1.0;
}

void LivoxMid70::SetTransformParam(Eigen::Vector3f rpy,Eigen::Vector3f transpose)
{
    Eigen::AngleAxisf rollAngle(Eigen::AngleAxisf(rpy(0),Eigen::Vector3f::UnitX()));
    Eigen::AngleAxisf pitchAngle(Eigen::AngleAxisf(rpy(1),Eigen::Vector3f::UnitY()));
    Eigen::AngleAxisf yawAngle(Eigen::AngleAxisf(rpy(2),Eigen::Vector3f::UnitZ()));

    Eigen::Matrix3f rotation_matrix;
    rotation_matrix=yawAngle*pitchAngle*rollAngle;
    m_transform.rotate(rotation_matrix);
    m_transform.translation()<<transpose;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr LivoxMid70::GetCloud()
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    /*printf("idx:%d\n",m_idx);
    m_frames[m_idx].is_timeout();
    int idc=m_idx-1;
    idc=(idc>=0)?idc:MAX_FRAME+idc;
    printf("idx-1:%d\n",idc);
    m_frames[idc].is_timeout();*/
    TimedLivoxExtendRawPoint     frames[MAX_FRAME];
    m_mutex.lock();
    int id=m_idx-1;
    for(int i=0;i<MAX_FRAME;++i)
    {
        frames[i]=m_frames[i];
    }
    m_mutex.unlock();


    for(int i=0;i<MAX_FRAME;++i)
    {
        int idc=id-i;
        idc=(idc>=0)?idc:MAX_FRAME+idc;

        if(frames[idc].is_timeout()==false)
        {
            for (int j = 0; j < frames[idc].m_point_num; ++j)
            {
                pcl::PointXYZ point;
                point.x = frames[idc].m_frame[j].x/1000.0;
                point.y = frames[idc].m_frame[j].y/1000.0;
                point.z = frames[idc].m_frame[j].z/1000.0;

                if(fabs(point.x)>8||fabs(point.y)>8||fabs(point.z)>8)
                    continue;
                cloud->push_back(point);
            }

        }
        else
        {
            break;
        }
    }

    //变换点云
    if(cloud->size()>0)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_t(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::transformPointCloud(*cloud,*cloud_t,m_transform);
        return cloud_t;

    }
    return cloud;
}


void LivoxMid70::LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32_t data_num)
{
    m_last_data_tp=std::chrono::steady_clock::now();
    LivoxEthPacket *eth_packet = data;

    if (!data || !data_num || (handle >= kMaxLidarCount))
    {
        return;
    }

    if (eth_packet)
    {
        //uint64_t cur_timestamp = *((uint64_t *) (data->timestamp));

        LivoxExtendRawPoint *p_point_data = (LivoxExtendRawPoint *) data->data;



        //printf("  create point num:%d\n", data_num);
        m_mutex.lock();
        m_frames[m_idx].Reset(p_point_data, data_num,1.0/m_fps);
        m_idx = (m_idx + 1) % MAX_FRAME;
        m_mutex.unlock();

    }
}
}