#include "Laser.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];
}
//////////////

CLaser::CLaser(int id, Automatic::stLaserCalibParam laser_param)
	:m_ThreadRcv(0)
	,m_ThreadPro(0)
	,m_id(id)
	,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)
{
}


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

bool CLaser::Start()
{
	m_statu = eLaser_busy;
	m_bStart_capture.Notify(true);
	return true;
}

bool CLaser::Stop()
{
	m_bStart_capture.Notify(false);
	m_binary_log_tool.close();
	m_pts_log_tool.close();
	return true;
}

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

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

	m_bThreadProRun.Notify(true);
	if(m_ThreadPro==0)
		m_ThreadPro = new std::thread(&CLaser::thread_toXYZ, this);
	m_ThreadPro->detach();
	m_statu = eLaser_connect;
	return true;
}
void CLaser::Disconnect()
{
	if (m_ThreadRcv)
	{
		delete m_ThreadRcv;
		m_ThreadRcv = 0;
	}
	if (m_ThreadPro)
	{
		delete m_ThreadPro;
		m_ThreadPro = NULL;
	}
	m_statu = eLaser_disconnect;
}

void CLaser::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 CLaser::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 CLaser::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 CLaser::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.front(pData);
		DATA_type type = eUnknow;
		if (bPop || m_last_data.Length() != 0)
		{
            m_queue_laser_data.try_pop();

			std::vector<CPoint3D> cloud;
			if (bPop)
			{
				if (pData == NULL)
					continue;
				if (m_bSave_file)
					m_binary_log_tool.write(pData->Data(), pData->Length());
				type= this->Data2PointXYZ(pData, cloud);
			}
			else
			{
				type = this->Data2PointXYZ(NULL, cloud);
			}
			if (type == eUnknow)
			{
                if(pData)
                 {

                    delete pData;
                    pData=0;
                }
				continue;
			}

			if (type == eData || type == eStart || type == eStop)
			{
				for (int i = 0; i < cloud.size(); ++i)
				{
					CPoint3D point = transfor(cloud[i]);
					if (m_point_callback_fnc)
						m_point_callback_fnc(point, m_point_callback_pointer);
					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));
					}
				}
				if (type == eStop)
				{
					if (m_bSave_file)
					{
						m_pts_log_tool.close();
						m_binary_log_tool.close();
					}
					m_statu = eLaser_ready;
				}

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

			delete pData;
            pData=0;
		}
		
	}
}