#include "Sick511FileLaser.h"
#include <unistd.h>

RegisterLaser(Sick511File);
CSick511FileLaser::CSick511FileLaser(int id, Automatic::stLaserCalibParam laser_param)
	:CLaser(id, laser_param)
	,m_start_read(false)
{
}

CSick511FileLaser::~CSick511FileLaser()
{
	if (m_stream_read.is_open())
		m_stream_read.close();
}

bool CSick511FileLaser::Connect()
{
	std::string ip = m_laser_param.laser_ip();
	char file[255] = { 0 };
	sprintf(file, "%s/laser%d.data", ip.c_str(), m_id + 1);
	if (m_stream_read.is_open())
	{
		m_stream_read.close();
	}
	m_stream_read.open(file, ios::in | ios::binary);
	if (!m_stream_read.good())
		return false;
	bool ret= CLaser::Connect();
	m_statu = eLaser_ready;
	m_file = file;
	return ret;
}
void CSick511FileLaser::Disconnect()
{
	if(m_stream_read.is_open())
		m_stream_read.close();
	return CLaser::Disconnect();
}
bool CSick511FileLaser::Start()
{
	std::lock_guard<std::mutex> lk(m_mutex);
	
	if (!this->IsReady())
		return false;
	
	m_start_read = true;
	return CLaser::Start();
	
}

bool CSick511FileLaser::Stop()
{
	std::lock_guard<std::mutex> lk(m_mutex);
	m_start_read = false;
	return true;
}

DATA_type CSick511FileLaser::Data2PointXYZ(CBinaryData* pData, std::vector<CPoint3D>& points)
{
	////拼接上次未打包数据
	
	DATA_type type = eUnknow;
	CBinaryData frame;

	if (m_last_data.Length() > 0)
	{
		if (pData)
			frame = m_last_data + (*pData);
		else
			frame = m_last_data;
		m_last_data = CBinaryData();
	}
	else if(pData)
	{
		frame = (*pData);
	}
	else
	{
		return type;
	}

	int head = FindHead(frame.Data(), frame.Length());
	int tail = FindTail(frame.Data(), frame.Length());
	
	if (tail >= 0)
	{
		if (tail < frame.Length() - 1)		//包尾后还有数据
		{
			m_last_data = CBinaryData(frame + tail + 1, frame.Length() - tail - 1);
		}
		if (head >= 0 && head < tail)
		{
			////解包
			CBinaryData data(frame + head, tail - head + 1);
			if (data == "$SLSSTP")
				type = eStop;
			else if (data == "$SLSSTA")
				type = eStart;
			else if (data == "$SCLRDY")
				type = eReady;
			else if (data == "$SHWERR")
				type = eHerror;
			else if (data == "$N")
				type = eData;

			if (type == eData)
			{
				////解析数据
				points.clear();
				if (data.Length() <= 29)
					return type;

				////解析
				unsigned char angle_1 = 0;
				unsigned char angle_2 = 0;
				memcpy(&angle_1, (data)+26, 1);
				memcpy(&angle_2, (data)+27, 1);
				float beta_a = float(angle_1 * 256 + angle_2)*0.01;
				float start_angle = 0.0;
				float freq = 0.0;

				std::vector<float> distance;
				if (GetData(&data, distance, freq, start_angle))
				{
					float beta = (beta_a)*DEGREES;
					float sin_beta = sin(beta);
					float cos_beta = cos(beta);
					for (int i = 0; i < distance.size(); ++i)
					{
						if (distance[i] < 0.001)
							continue;

						float alpha = (start_angle + i*freq - 90.0) * DEGREES;
						float sin_alpha = sin(alpha);
						float cos_alpha = cos(alpha);

						float x = distance[i] * sin_alpha;
						float y = distance[i] * cos_alpha * sin_beta;
						float z = distance[i] * cos_alpha * cos_beta;

						points.push_back(CPoint3D(x, y, z));
					}
					//points.push_back(CPoint3D(double(distance.size()), start_angle, freq));
				}

			}
			else if (type == eStop)
			{
				m_statu = eLaser_ready;
			}
		}
	}
	else if (head >= 0)
	{
		m_last_data = CBinaryData(frame + head, frame.Length() - head);
	}

	return type;
}

bool CSick511FileLaser::RecvData(CBinaryData& data)
{
	if (m_start_read == false)
		return false;
	if (!m_stream_read.is_open())
		return false;
	
	if (m_stream_read.eof())
	{
		Stop();
		m_stream_read.close();
        usleep(100*1000);
		m_stream_read.open(m_file.c_str(), ios::in | ios::binary);
	}

    char buf[512] = { 0 };
    m_stream_read.read(buf, 512);
	int count = m_stream_read.gcount();
	if (count > 0)
	{
		CBinaryData bin_data(buf, count);
		data = bin_data;
		return true;
	}
	return false;
}

int CSick511FileLaser::FindHead(char* buf, int b_len)
{
	int i = 0;
	if (b_len < 2)
		return NULL;
	for (i = 0; i <= b_len; i++)
	{
		if (b_len > 10)
		{
			if (buf[i] == '$'&& buf[i + 1] == 'N' && buf[i + 7] == 'S'
				&&buf[i + 8] == '5'&&buf[i + 9] == '1'&&buf[i + 10] == '1')
				return i;
		}
		if (b_len > 7)
		{
			if (buf[i] == '$'&& buf[i + 1] == 'S'&&buf[i + 7] == '*')
				return i;
		}
	}
	return -1;
}

int CSick511FileLaser::FindTail(char* buf, int b_len)
{
	int i = 0;

	for (i = 0; i <= b_len; i++)
	{
		if (b_len >= i + 5)
		{
			if (buf[i] == '*' && buf[i + 3] == '\r'&&buf[i + 4] == '\n')
				return i + 4;
		}
	}
	return -9999999;
}

bool CSick511FileLaser::GetData(CBinaryData* pData, std::vector<float>& distance,
	float& freq, float& start_angle)
{
	struct stData
	{
		const char* data;
		int length;
	};
	std::vector<struct stData> strDatas;
	int start = 0;
	int end = 0;
	int LMDscandata_index = -1;
	for (int i = 0; i < pData->Length(); ++i)
	{
		if ((*pData)[i] == ' ')
		{
			end = i;
			if (end > start)
			{
				struct stData strData;
				strData.data = (*pData + start);
				strData.length = end - start;

				strDatas.push_back(strData);
				if (strncmp(strData.data, "LMDscandata", 11) == 0)
					LMDscandata_index = strDatas.size() - 1;
			}
			end = i + 1;
			start = end;
		}
	}

	if (strDatas.size() > 26 + LMDscandata_index - 1)
	{
		struct stData start_angle_str = strDatas[23 + LMDscandata_index - 1];
		long start_angle_l = Str0x2Long(start_angle_str.data, start_angle_str.length);
		start_angle = float(start_angle_l)*0.01;

		struct stData freq_str = strDatas[24 + LMDscandata_index - 1];
		long freq_l = Str0x2Long(freq_str.data, freq_str.length);
		freq = float(freq_l)*0.0001;

		struct stData count_str = strDatas[25 + LMDscandata_index - 1];
		long count = Str0x2Long(count_str.data, count_str.length);
		if (strDatas.size() >= 26 + LMDscandata_index - 1 + count)
		{
			for (int i = 26 + LMDscandata_index - 1; i < 26 + LMDscandata_index - 1 + count; ++i)
			{
				float dis = float(Str0x2Long(strDatas[i].data, strDatas[i].length));
				distance.push_back(dis);
			}
			return true;
		}
	}
	return false;
}
long CSick511FileLaser::Str0x2Long(const char* data, int len)
{
	long sum = 0;
	for (int i = 0; i < len; ++i)
	{
		char c = data[i];
		int n = 0;
		if (c >= 48 && c <= 57)
			n = c - 48;
		else if (c >= 65 && c <= 70)
			n = c - 65 + 10;
		sum += n*pow(16, len - i - 1);
	}
	return sum;
}