#include "Laser.h"
#include "../tool/MeasureTopicPublisher.h"

CBinaryData::CBinaryData()
	:m_buf(0)
	,m_length(0)
{
}
CBinaryData::CBinaryData(const char* buf, int len, DATA_type type)
    :m_buf(0),m_length(0)
{
	if (len > 0)
	{
		m_buf = (char*)malloc(len);
		memcpy(m_buf, buf, len);
		m_length = len;
	}
}
CBinaryData::CBinaryData(const CBinaryData& data)
    :m_buf(0),m_length(0)
{
	if (m_buf)
	{
		free(m_buf);
		m_buf = 0;
		m_length = 0;
	}
	if (data.Length() > 0)
	{
		m_buf = (char*)malloc(data.Length());
		memcpy(m_buf, data.Data(), data.Length());
		m_length = data.Length();
	}
}
CBinaryData::~CBinaryData()
{
    if (m_buf && m_length)
	{
		free(m_buf);
		m_length = 0;
	}
}
char* CBinaryData::Data()const {
	return m_buf;
}
int CBinaryData::Length()const {
	return m_length;
}
CBinaryData& CBinaryData::operator=(const CBinaryData& data)
{
	if (m_buf)
	{
		free(m_buf);
		m_buf = 0;
		m_length = 0;
	}
	if (data.Length() > 0)
	{
		m_buf = (char*)malloc(data.Length());
		memcpy(m_buf, data.Data(), data.Length());
		m_length = data.Length();
	}
	return *this;
}
bool CBinaryData::operator==(const char* str)
{
	int length = strlen(str);
	return  (strncmp((const char*)m_buf, str, length) == 0);
}
const char* CBinaryData::operator+(int n)
{
	if (n < m_length)
		return m_buf + n;
	else
		return NULL;
}
CBinaryData& CBinaryData::operator+(CBinaryData& data)
{
	if (data.Length() > 0)
	{
		int length = m_length + data.Length();
		char* buf = (char*)malloc(length);
		memcpy(buf, m_buf, m_length);
		memcpy(buf + m_length, data.Data(), data.Length());
		free(m_buf);
		m_buf = buf;
		m_length = length;
	}
	return *this;
}
char& CBinaryData::operator[](int n)
{
	if (n >= 0 && n < m_length)
		return m_buf[n];
}
//////////////

Laser_base::Laser_base(int id, Laser_proto::laser_parameter laser_param)
	:m_ThreadRcv(0)
	,m_ThreadPro(0)
	,m_ThreadPub(0)
	,m_id(id)
    ,m_points_count(0)
	,m_statu(eLaser_disconnect)
	,m_dMatrix(0)
	, m_point_callback_fnc(0)
	, m_point_callback_pointer(0)
	,m_laser_param(laser_param)
	,m_bSave_file(false)
	,m_bscan_start(false)
{
    mp_laser_task = NULL;
}


Laser_base::~Laser_base()
{
	if (m_dMatrix)
	{
		free(m_dMatrix);
		m_dMatrix = 0;
	}
}

Error_manager Laser_base::check_error()
{
    return SUCCESS;
}

bool Laser_base::Start()
{

    m_bscan_start=true;
	m_statu = eLaser_busy;
	m_bStart_capture.Notify(true);
    m_points_count=0;
	return true;
}

bool Laser_base::Stop()
{
    m_bscan_start=false;
	m_bStart_capture.Notify(false);
	m_binary_log_tool.close();
	m_pts_log_tool.close();
	m_statu=eLaser_ready;

	//在停止雷达时，将雷达任务状态改为 TASK_OVER 已结束
	if(mp_laser_task !=NULL)
    {
        mp_laser_task->set_task_statu(TASK_OVER);
	    /*if(mp_laser_task->get_statu() == TASK_WORKING)
        {

        }*/
    }

	return true;
}

void Laser_base::SetMetrix(double* data)
{
	if (m_dMatrix == 0)
	{
		m_dMatrix = (double*)malloc(12 * sizeof(double));
	}
	memcpy(m_dMatrix, data, 12 * sizeof(double));
}
void Laser_base::SetPointCallBack(PointCallBack fnc, void* pointer)
{
	m_point_callback_fnc = fnc;
	m_point_callback_pointer = pointer;
}

bool Laser_base::Connect()
{
	m_bThreadRcvRun.Notify(true);
	if(m_ThreadRcv==0)
		m_ThreadRcv = new std::thread(&Laser_base::thread_recv, this);
	m_ThreadRcv->detach();

	m_bThreadProRun.Notify(true);
	if(m_ThreadPro==0)
		m_ThreadPro = new std::thread(&Laser_base::thread_toXYZ, this);
	m_ThreadPro->detach();

    if(m_ThreadPub==0)
        m_ThreadPub = new std::thread(threadPublish, this);
    m_ThreadPub->detach();

	return true;
}
void Laser_base::Disconnect()
{
	if (m_ThreadRcv)
	{
		delete m_ThreadRcv;
		m_ThreadRcv = 0;
	}

    if (m_ThreadPub)
    {
        delete m_ThreadPub;
        m_ThreadPub = 0;
    }

	if (m_ThreadPro)
	{
		delete m_ThreadPro;
		m_ThreadPro = NULL;
	}
	m_statu = eLaser_disconnect;
}


//对外的接口函数，负责接受并处理任务单，
//input：laser_task 雷达任务单，必须是子类，并且任务正确。（传引用）
Error_manager Laser_base::execute_task(Task_Base* p_laser_task)
{
    return SUCCESS;
}


void Laser_base::SetSaveDir(std::string strDir,bool bSaveFile)
{
	m_bSave_file = bSaveFile;
	m_pts_log_tool.close();
	m_binary_log_tool.close();

	if (bSaveFile == false)
		return;

	m_pts_save_path = strDir;
	char pts_file[255] = { 0 };
	sprintf(pts_file, "%s/pts%d.txt", strDir.c_str(), m_id+1);
	m_pts_log_tool.open(pts_file);

	char bin_file[255] = { 0 };
	sprintf(bin_file, "%s/laser%d.data", strDir.c_str(), m_id+1);
	m_binary_log_tool.open(bin_file,true);
}

void Laser_base::thread_recv()
{
	while (m_bThreadRcvRun.WaitFor(1))
	{
		if (m_bStart_capture.WaitFor(1) == false)
			continue;
		CBinaryData* data=new CBinaryData();
		if (this->RecvData(*data))
		{
			m_queue_laser_data.push(data);
		}
		else
		{
			delete data;
            data=0;
		}
		
	}
}

CPoint3D Laser_base::transfor(CPoint3D point)
{
	if (m_dMatrix)
	{
		CPoint3D result;
		double x = point.x;
		double y = point.y;
		double z = point.z;
		result.x = x * m_dMatrix[0] + y*m_dMatrix[1] + z*m_dMatrix[2] + m_dMatrix[3];
		result.y = x * m_dMatrix[4] + y*m_dMatrix[5] + z*m_dMatrix[6] + m_dMatrix[7];
		result.z = x * m_dMatrix[8] + y*m_dMatrix[9] + z*m_dMatrix[10] + m_dMatrix[11];
		return result;
	}
	else
	{
		return point;
	}
}

void Laser_base::thread_toXYZ()
{
	while (m_bThreadProRun.WaitFor(1))
	{
		if (m_bStart_capture.WaitFor(1) == false)
		{
			m_queue_laser_data.clear();
			continue;
		}
        CBinaryData* pData=NULL;
        bool bPop = m_queue_laser_data.try_pop(pData);
        DATA_type type = eUnknow;
        if (bPop || m_last_data.Length() != 0)
        {
            std::vector<CPoint3D> cloud;
            if (bPop)
            {
                if (pData == NULL)
                    continue;
                m_binary_log_tool.write(pData->Data(), pData->Length());
                type= this->Data2PointXYZ(pData, cloud);
            }
            else
            {
                type = this->Data2PointXYZ(NULL, cloud);
            }
            if (type == eUnknow)
            {
                delete pData;
                continue;
            }

            if (type == eData || type == eStart || type == eStop)
            {
                for (int i = 0; i < cloud.size(); ++i)
                {
                    CPoint3D point = transfor(cloud[i]);
                    if(m_bSave_file) {
                        char buf[64] = {0};
                        sprintf(buf, "%f %f %f\n", point.x, point.y, point.z);
                        m_pts_log_tool.write(buf, strlen(buf));
                    }

                    //此时cloud为雷达采集数据的结果，转换后的三维点云。
                    //将每个点存入任务单里面的点云容器。

                    if(mp_laser_task!=NULL)
                    {
                        Error_manager code=mp_laser_task->push_point(pcl::PointXYZ(point.x,point.y,point.z));

                    }
                }
                m_points_count+=cloud.size();
                if (type == eStop)
                {
                    if(m_bSave_file) {
                        m_pts_log_tool.close();
                        m_binary_log_tool.close();
                    }
                    m_statu = eLaser_ready;

                    //在停止雷达时，将雷达任务状态改为 TASK_OVER 已结束
                    if(mp_laser_task !=NULL)
                    {
                        if(mp_laser_task->get_statu() == TASK_WORKING)
                        {
                            mp_laser_task->set_task_statu(TASK_OVER);
                        }
                    }
                }

            }
            else if (type == eReady)
            {
                if(m_bSave_file) {
                    m_pts_log_tool.close();
                    m_binary_log_tool.close();
                }
                m_statu = eLaser_ready;
            }


            delete pData;
        }
		
	}
}


#include "unistd.h"
void Laser_base::threadPublish(Laser_base *laser)
{
    if(laser==0) return ;
    while(laser->m_bThreadRcvRun.WaitFor(1))
    {
        laser->PublishMsg();
        usleep(1000*300);
    }
}


void Laser_base::PublishMsg()
{
    laser_message::laserMsg msg;

    laser_message::laserStatus status;
    if(GetStatu()==eLaser_ready) status=laser_message::eLaserConnected;
    else if(GetStatu()==eLaser_disconnect) status=laser_message::eLaserDisconnected;
    else if(GetStatu()==eLaser_busy) status=laser_message::eLaserBusy;
    else  status=laser_message::eLaserUnknown;

    msg.set_id(m_id);
    msg.set_laser_status(status);
    msg.set_queue_data_count(m_queue_laser_data.size());
    msg.set_cloud_count(m_points_count);
    MeasureTopicPublisher::GetInstance()->Publish(msg.SerializeAsString());

}