/*
*  RSLIDAR System
*  Driver Interface
*
*  Copyright 2015 RS Team
*  All rights reserved.
*
*	Author: ruishi, Data:2015-12-25
*
*/
#include <time.h>
#include "sdkcommon.h"
#include "hal/abs_rxtx.h"
#include "hal/thread.h"
#include "hal/locker.h"
#include "hal/event.h"
#include "rslidar_driver_serial.h"


//#include <ioctl.h>
using namespace std;
#ifndef min
#define min(a,b)            (((a) < (b)) ? (a) : (b))
#endif

namespace rs { namespace standalone{ namespace rslidar {


// Factory Impl
RSlidarDriver * RSlidarDriver::CreateDriver()
{
    return new RSlidarDriverSerialImpl();
}


void RSlidarDriver::DisposeDriver(RSlidarDriver * drv)
{
    delete drv;
}

// Serial Driver Impl

RSlidarDriverSerialImpl::RSlidarDriverSerialImpl(): _isConnected(false), _isScanning(false)
{
    _rxtx = rs::hal::serial_rxtx::CreateRxTx();
	_flag_cached_scan_node_ping_pong = NOTE_BUFFER_PING;
	memset(_cached_scan_note_bufferPing, 0, sizeof(_cached_scan_note_bufferPing));
	memset(_cached_scan_note_bufferPong, 0, sizeof(_cached_scan_note_bufferPong));
    _cached_scan_node_count = 0;
	RunningStates = PORT_RDY;
}

RSlidarDriverSerialImpl::~RSlidarDriverSerialImpl()
{
    disconnect();
    rs::hal::serial_rxtx::ReleaseRxTx(_rxtx);
}

u_result RSlidarDriverSerialImpl::connect(const char * port_path, _u32 baudrate, _u32 flag)
{
    rs::hal::AutoLocker l(_lock);
    if (isConnected()) return RESULT_ALREADY_DONE;

    if (!_rxtx) return RESULT_INSUFFICIENT_MEMORY;

    // establish the serial connection...
    if (!_rxtx->bind(port_path, baudrate)  ||  !_rxtx->open()) {
        return RESULT_INVALID_DATA;
    }

    _rxtx->flush(0);
    _isConnected = true;

	_isGetData = true;
    return RESULT_OK;
}

void RSlidarDriverSerialImpl::disconnect()
{
    if (!_isConnected) return ;
		stopScan();
    _rxtx->close();
}

bool RSlidarDriverSerialImpl::isConnected()
{
    return _isConnected;
}

u_result RSlidarDriverSerialImpl::_cacheSerialData()
{
	return RESULT_OK;
}


u_result RSlidarDriverSerialImpl::resetlidar(_u32 timeout)
{
	_u8 param = 0x08;
    u_result ans;
	COMM_FRAME_T *response_header = reinterpret_cast<COMM_FRAME_T *>(_paramReceiveBuf);

	if (!isConnected()) return RESULT_OPERATION_FAIL;

	_disableDataGrabbing();

	{
		rs::hal::AutoLocker l(_lock);
		param = RESET_MODE;
		if (IS_FAIL(ans = _sendCommand(CMD_WORK_MODE, &param, 1))) {
			return ans;
		}

		if (IS_FAIL(ans = _waitResponseHeader(response_header, CMD_WORK_MODE, NULL,1, timeout))) {
			return ans;
		}

		if ((response_header->command&COMM_FRAME_ERROR) > 0)
		{
			return *(response_header->paramBuf);
		}
	}

	return RESULT_OK;
}

u_result RSlidarDriverSerialImpl::setMotorRpm(_u16 speed, _u32 timeout)
{
	_u8 Data[] = { 0x23,0x01,0x67,0x45,0xAB,0x89,0xEF,0xCD,0x00,0x00};
	u_result ans;
	COMM_FRAME_T *response_header = reinterpret_cast<COMM_FRAME_T *>(_paramReceiveBuf);

	Data[8] = speed;
	Data[9] = speed >> 8;

	if (!isConnected()) return RESULT_OPERATION_FAIL;

	//_disableDataGrabbing();

	{
		//rs::hal::AutoLocker l(_lock);

		if (IS_FAIL(ans = _sendCommand(CMD_SET_MOTOR_SPEED, &Data, 10))) {
			return ans;
		}

		if (IS_FAIL(ans = _waitResponseHeader(response_header, CMD_SET_MOTOR_SPEED, NULL,1, timeout))) {
			return ans;
		}

		if ((response_header->command&COMM_FRAME_ERROR) > 0)
		{
			return (*(response_header->paramBuf));
		}
	}

	return RESULT_OK;
}

u_result RSlidarDriverSerialImpl::getDeviceInfo(LIDAR_RESPONSE_DEV_INFO_T * info, _u32 timeout)
{
	u_result  ans;
	COMM_FRAME_T *response_header = reinterpret_cast<COMM_FRAME_T *>(_paramReceiveBuf);

	if (!isConnected()) return RESULT_OPERATION_FAIL;

	_disableDataGrabbing();

	{
		rs::hal::AutoLocker l(_lock);
		if (IS_FAIL(ans = _sendCommand(CMD_READ_DEV_VER, NULL, 0))) {
			return ans;
		}

		if (IS_FAIL(ans = _waitResponseHeader(response_header, CMD_READ_DEV_VER, NULL,45, timeout))) {
			return ans;
		}

		if ((response_header->command&COMM_FRAME_ERROR) > 0)
		{
			return (*(response_header->paramBuf));
		}

		memcpy(info, response_header->paramBuf, response_header->paramLen);
	}
	return RESULT_OK;
}

u_result RSlidarDriverSerialImpl::startScan(_u8 scanmode,_u32 timeout)
{
	_u8 param = scanmode;
	u_result ans;
	COMM_FRAME_T *response_header = reinterpret_cast<COMM_FRAME_T *>(_paramReceiveBuf);

	if (!isConnected()) return RESULT_OPERATION_FAIL;
	if (_isScanning) return RESULT_ALREADY_DONE;

	{
		rs::hal::AutoLocker l(_lock);
		_rxtx->flush(0);
		if (IS_FAIL(ans = _sendCommand(CMD_WORK_MODE, &param, 1))) {
			return ans;
		}
		
		/*if (IS_FAIL(ans = _waitResponseHeader(response_header, CMD_WORK_MODE, NULL, 1, timeout))) {
			return ans;
		}
	
		if ((response_header->command&COMM_FRAME_ERROR) > 0)
		{
			return (*(response_header->paramBuf));
		}
		*/
		_isScanning = true;
		_cachethread = CLASS_THREAD(RSlidarDriverSerialImpl, _cacheScanData);
	}

	return RESULT_OK;
}


u_result RSlidarDriverSerialImpl::stopScan(_u32 timeout)
{
	_u8 param = 0x00;
	u_result ans;
	COMM_FRAME_T *response_header = reinterpret_cast<COMM_FRAME_T *>(_paramReceiveBuf);

	if (!isConnected()) return RESULT_OPERATION_FAIL;

	_disableDataGrabbing();

	{
		rs::hal::AutoLocker l(_lock);

		if (IS_FAIL(ans = _sendCommand(CMD_WORK_MODE,&param,1))) {
			return ans;
		}

		if (IS_FAIL(ans = _waitResponseHeader(response_header, CMD_WORK_MODE, NULL,1, timeout))) {
			return ans;
		}

		if ((response_header->command&COMM_FRAME_ERROR) > 0)
		{
			return (*(response_header->paramBuf));
		}
	}

	return RESULT_OK;
}

u_result RSlidarDriverSerialImpl::_cacheScanData()
{
	LIDAR_MEASURE_INFO_T				*local_buf = NULL;
	size_t                              count = NUMBER_OF_TEETH;
	size_t                              scan_count = 0;
	u_result                            ans;

	_flag_cached_scan_node_ping_pong = NOTE_BUFFER_PING;
	local_buf = _cached_scan_note_bufferPong;
	_waitScanData(local_buf, count); // // always discard the first data since it may be incomplete
	count = NUMBER_OF_TEETH;

	while (_isScanning)
	{
		if (IS_FAIL(ans = _waitScanData(local_buf, count))) {
			if (!((ans == RESULT_OPERATION_TIMEOUT) || (ans == RESULT_RECEIVE_NODE_ERROR))) {
				_isScanning = false;
				return RESULT_OPERATION_FAIL;
			}
		}

		_lock.lock();
		_flag_cached_scan_node_ping_pong = _flag_cached_scan_node_ping_pong ^ 0x01;
		if (NOTE_BUFFER_PING == _flag_cached_scan_node_ping_pong)
		{
			local_buf = _cached_scan_note_bufferPong;
		}
		else if (NOTE_BUFFER_PONG == _flag_cached_scan_node_ping_pong)
		{
			local_buf = _cached_scan_note_bufferPing;
		}
		else{}
		_cached_scan_node_count = count;
		_dataEvt.set();
		_lock.unlock();
		count = NUMBER_OF_TEETH;
	}
	_isScanning = false;
	return RESULT_OK;
}

u_result RSlidarDriverSerialImpl::_waitScanData(LIDAR_MEASURE_INFO_T * nodebuffer, size_t & count, _u32 timeout)
{
	_u8		recvBuffer[1024];
	_u16	frameLens = 0;
	_u16	paramLens = 0;
	_u16	angleRange = 0;
	float	angleStep = 0;
	float  speed;
	float  angleoffset;
	bool	flagOfNewNodes = false;
	_u16	crcCheckNum = 0,calcchecksum;
	_u16	sampleNumber = 0;
	_u16    sampleNumberCount = 0;

	if (!_isConnected) {
		count = 0;
		return RESULT_OPERATION_FAIL;
	}

	size_t   recvNodeCount = 0;
	_u32     startTs = getms();
	_u32     waitTime;
	u_result ans;


	while ((waitTime = getms() - startTs) <= timeout && recvNodeCount < count) 
	{
		if (IS_FAIL(ans = _waitNode(recvBuffer, timeout - waitTime))) 
		{
			return ans;
		}

		frameLens = (recvBuffer[2] << 8) | recvBuffer[1];
		crcCheckNum = *(_u16 *)(recvBuffer + frameLens);
		calcchecksum = _checksum(recvBuffer, frameLens);
		angleRange = (recvBuffer[10] << 8) | recvBuffer[9];

		if (crcCheckNum == calcchecksum)
		{
			paramLens = (recvBuffer[6] << 8) | recvBuffer[5];
			sampleNumber = (paramLens - 4) / 2;
			if ((angleRange >= 35000) || (angleRange <= 100))  //find original point
			{
				flagOfNewNodes = true;
				recvNodeCount = 0;
				sampleNumberCount = 0;
				angleStep = 36000.0 / sampleNumber / NUMBER_OF_TEETH;
			}

			if (flagOfNewNodes)
			{
				speed = ((recvBuffer[8] << 8) | recvBuffer[7])*0.01;
				angleoffset = 0;

				for (int i = 0; i < sampleNumber; i++)
				{
					(nodebuffer + sampleNumberCount)->motorspeed = speed;
					(nodebuffer + sampleNumberCount)->angle = angleRange + i * angleStep;
					(nodebuffer + sampleNumberCount)->angleoffset = angleoffset;
					(nodebuffer + sampleNumberCount)->distance = (recvBuffer[12 + i * 2] << 8) | recvBuffer[11 + i * 2];
				//	printf("this is my shared data somehow");
					printf("test \n");
					sampleNumberCount++;
				}
				recvNodeCount++;
			}			
		}
		else
		{
			RunningStates = CHKSUM_ERROR;
			continue;
		}

		if (recvNodeCount == count)
		{
			flagOfNewNodes = false;
			count = sampleNumberCount;
			return RESULT_OK;
		}
	}

	//CLogout("_waitScanData002:%d \r\n", sampleNumberCount);
	count = sampleNumberCount;
	return RESULT_OPERATION_TIMEOUT;
}

u_result RSlidarDriverSerialImpl::_waitNode(_u8 * nodeBuffer, _u32 timeout)
{
	int		recvPos = 0;
	_u32	startTs = getms();
	_u8		recvBuffer[512];
	_u32	waitTime;
	_u16	frameLens = 0;
	size_t  recvSize;
	size_t  remainSize;
	size_t  pos;
	bool    frameDataRcvFlag = false;
	
	
	char tmp_message[100];
	memset(tmp_message, 0, 100);

	while ((waitTime = getms() - startTs) <= timeout) {

		if (false == frameDataRcvFlag)
		{
			//receive head info
			remainSize = sizeof(COMM_FRAME_T) - recvPos;
		}
		else
		{
			//receive data
			remainSize = frameLens + 2 - recvPos;
		}
		recvSize = 0;
		//printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
		int ans = _rxtx->waitfordata(remainSize, timeout, &recvSize);

		if (ans == rs::hal::serial_rxtx::ANS_DEV_ERR)
		{
			return RESULT_OPERATION_FAIL;
		}

		if ((ans == rs::hal::serial_rxtx::ANS_TIMEOUT) && (0 == recvSize))
		{
			GetLocalTime(&tm_time_out);
			int tm_cha = ((tm_time_out.wMinute - tm_error_frame.wMinute) * 60 + (tm_time_out.wSecond - tm_error_frame.wSecond)) * 1000 + (tm_time_out.wMilliseconds - tm_error_frame.wMilliseconds);
			if (tm_cha > 500)
			{
				_cprintf("[Eror]: Receive data timeout!\r\n");
				RunningStates = RX_DATA_TIMOUT;
				continue;
			}
		}
		if (recvSize > remainSize) recvSize = remainSize;
		_rxtx->recvdata(recvBuffer, recvSize);
		for (pos = 0; pos < recvSize; ++pos)
		{
			_u8 currentByte = recvBuffer[pos];

			if (0 == recvPos)
			{
				if (COMM_HEAD_FLAGE != currentByte)
				{
					frameDataRcvFlag = false;
					continue;
				}
			}
			else if (4 == recvPos)
			{
				frameLens = (nodeBuffer[2] << 8) | nodeBuffer[1];
				//if ((frameLens > 300) || (recvPos < (sizeof(COMM_FRAME_T) - 1)))
				if (frameLens > 300)
				{
					recvPos = 0;
					frameLens = 0;
					frameDataRcvFlag = false;
					continue;
				}
				else
					frameDataRcvFlag = true;

				if (CMD_REPORT_DEV_ERROR == (currentByte&COMM_MASK))
				{
					RunningStates = MOTOR_STALL;
					continue;					
				}

				if (CMD_REPORT_DIST != (currentByte&COMM_MASK))
					continue;
			}

			nodeBuffer[recvPos++] = currentByte;

			if ((recvPos >= (frameLens + 2))&&(recvPos > sizeof(COMM_FRAME_T)))
			{
				RunningStates = WORK_WELL;
				return RESULT_OK;
			}
		}
	}
	return RESULT_OPERATION_TIMEOUT;
}

u_result RSlidarDriverSerialImpl::grabScanData(LIDAR_MEASURE_INFO_T * nodebuffer, size_t & count, _u32 timeout)
{
    switch (_dataEvt.wait(timeout))
    {
		case rs::hal::Event::EVENT_TIMEOUT:
			count = 0;
			return RESULT_OPERATION_TIMEOUT;
			break;

		case rs::hal::Event::EVENT_OK:
			{
				rs::hal::AutoLocker l(_lock);

				size_t size_to_copy = min(count, _cached_scan_node_count);

				if (NOTE_BUFFER_PING == _flag_cached_scan_node_ping_pong)
				{
					memcpy(nodebuffer, _cached_scan_note_bufferPing, size_to_copy*sizeof(LIDAR_MEASURE_INFO_T));
				}
				else if (NOTE_BUFFER_PONG == _flag_cached_scan_node_ping_pong)
				{
					memcpy(nodebuffer, _cached_scan_note_bufferPong, size_to_copy*sizeof(LIDAR_MEASURE_INFO_T));
				}

				count = size_to_copy;
				_cached_scan_node_count = 0;
			}
			return RESULT_OK;
			break;

		default:
			count = 0;
			return RESULT_OPERATION_FAIL;
			break;
    }

}

u_result RSlidarDriverSerialImpl::ascendScanData(LIDAR_MEASURE_INFO_T * nodebuffer, size_t count)
{
    float inc_origin_angle = 360.0/count;
    int i = 0;

    //Tune head
    for (i = 0; i < count; i++) {
        if(nodebuffer[i].distance == 0) {
            continue;
        } else {
            while(i != 0) {
                i--;
                float expect_angle = (nodebuffer[i+1].angle)/100.0f - inc_origin_angle;
                if (expect_angle < 0.0f) expect_angle = 0.0f;
				nodebuffer[i].angle = (_u16)(expect_angle * 100.0f);
            }
            break;
        }
    }

    // all the data is invalid
    if (i == count) return RESULT_OPERATION_FAIL;

    //Tune tail
    for (i = count - 1; i >= 0; i--) {
		if (nodebuffer[i].distance == 0) {
            continue;
        } else {
            while(i != (count - 1)) {
                i++;
				float expect_angle = (nodebuffer[i - 1].angle) / 100.0f + inc_origin_angle;
                if (expect_angle > 360.0f) expect_angle -= 360.0f;
				nodebuffer[i].angle = (_u16)(expect_angle * 100.0f);
            }
            break;
        }
    }

    //Fill invalid angle in the scan
	float frontAngle = (nodebuffer[0].angle) / 100.0f;
    for (i = 1; i < count; i++) {
		if (nodebuffer[i].distance == 0) {
            float expect_angle =  frontAngle + i * inc_origin_angle;
            if (expect_angle > 360.0f) expect_angle -= 360.0f;
			nodebuffer[i].angle = (_u16)(expect_angle * 100.0f);
        }
    }

    // Reorder the scan according to the angle value
    for (i = 0; i < (count-1); i++){
        for (int j = (i+1); j < count; j++){
			if (nodebuffer[i].angle > nodebuffer[j].angle){
				LIDAR_MEASURE_INFO_T temp = nodebuffer[i];
                nodebuffer[i] = nodebuffer[j];
                nodebuffer[j] = temp;
            }
        }
    }

    return RESULT_OK;
}

u_result RSlidarDriverSerialImpl::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize)
{
	_u16 checksum = 0;
	_u8 commBuf[1024] = {0};
	COMM_FRAME_T *commFrameTmp = reinterpret_cast<COMM_FRAME_T *>(commBuf);
	const _u8 *pPayload = reinterpret_cast<const _u8 *>(payload);

	if (!_isConnected) return RESULT_OPERATION_FAIL;

	commFrameTmp->framehead = COMM_HEAD_FLAGE;
	commFrameTmp->frameLen = sizeof(COMM_FRAME_T)+payloadsize;
	commFrameTmp->protocolver = 4;
	commFrameTmp->command = cmd;
	if (payloadsize && payload) {
		commFrameTmp->paramLen = payloadsize;
	}
	else
	{
		commFrameTmp->paramLen = 0;
	}
	if (payloadsize > 0)
	{
		for (int i = payloadsize-1; i >= 0; i--)
			*(commFrameTmp->paramBuf+i) = *(pPayload+i);
	}

	checksum = _checksum(commBuf, sizeof(COMM_FRAME_T) + payloadsize);
	
	commBuf[sizeof(COMM_FRAME_T)+payloadsize] = checksum;
	commBuf[sizeof(COMM_FRAME_T)+payloadsize + 1] = checksum >> 8;

	// send header first
	for (int i = 0; i < (sizeof(COMM_FRAME_T)+payloadsize + sizeof(_u16));i++)
		printf("%02x ", commBuf[i]);
	printf("\n");
	_rxtx->senddata(commBuf, sizeof(COMM_FRAME_T)+payloadsize + sizeof(_u16));
	return RESULT_OK;
}


u_result RSlidarDriverSerialImpl::_waitResponseHeader(COMM_FRAME_T * header, _u8 Cmmd,const void * payload, size_t payloadsize, _u32 timeout)
{
	_u8 Data[512];
	int  recvPos = 0,readPos = 0,i;
	_u32 startTs = getms();
	_u8  *headerBuffer = reinterpret_cast<_u8 *>(header);
	_u32 waitTime;
	_u16 framelen,checksumCac, checksumRcv;
	size_t remainSize = sizeof(COMM_FRAME_T) + payloadsize + sizeof(_u16);
	size_t recvSize;

	while ((waitTime = getms() - startTs) <= timeout) 
	{
		int ans = _rxtx->waitfordata(remainSize, timeout - waitTime, &recvSize);
		if (ans == rs::hal::serial_rxtx::ANS_DEV_ERR)
		{
			return RESULT_OPERATION_FAIL;
		}
		else if (ans == rs::hal::serial_rxtx::ANS_TIMEOUT)
		{
			return RESULT_OPERATION_TIMEOUT;
		}

		if (recvSize > (sizeof(Data) - (recvPos & 0x1FF)))
		{
			_rxtx->recvdata(&Data[recvPos & 0x1FF], sizeof(Data) - (recvPos & 0x1FF));
			_rxtx->recvdata(Data,recvSize - sizeof(Data) + (recvPos & 0x1FF));
		}
		else
			_rxtx->recvdata(&Data[recvPos & 0x1FF], recvSize);

		recvPos += recvSize;
		if ((readPos + remainSize) > recvPos)
			continue;
		else
		{
			for (; readPos < recvPos; readPos++)
			{
				if (Data[readPos & 0x1FF] != COMM_HEAD_FLAGE)
					continue;
				else
				{
					framelen = (Data[(readPos + 2) & 0x1FF] << 8) | Data[(readPos + 1) & 0x1FF];
					if (framelen > 100)
						continue;

					if ((framelen + 2) > (recvPos - readPos))
						break;

					if ((Data[(readPos + 4) & 0x1FF]&COMM_MASK) != Cmmd)
						continue;

					checksumCac = _Cyclechecksum(Data,readPos,framelen,sizeof(Data));
					checksumRcv = (Data[(readPos + framelen + 1) & 0x1FF] << 8) | Data[(readPos + framelen) & 0x1FF];
					if (checksumCac == checksumRcv)
					{
						for (i = 0; i < (payloadsize + sizeof(COMM_FRAME_T)); i++)
						{
							*headerBuffer++ = Data[(readPos + i) & 0x1FF];
						}
						return RESULT_OK;
					}
					else
						return RESULT_INVALID_DATA;
				}
			}
		}
	}
	return RESULT_OPERATION_TIMEOUT;
}

uint16_t RSlidarDriverSerialImpl::_Cyclechecksum(uint8_t *startByte, _u32 pos,uint16_t numBytes,_u32 buffersize)
{
	uint16_t checksum = 0;                           // CRC���ұ��ָ��
	while (numBytes--)
	{
		checksum += *(startByte + (pos%buffersize));      // ����CRC
		pos++;
	}
	return checksum;
}

uint16_t RSlidarDriverSerialImpl::_checksum(uint8_t *startByte, uint16_t numBytes)
{
	uint16_t checksum = 0;                           // CRC���ұ��ָ��
	while (numBytes--)
	{
		checksum += *startByte++;      // ����CRC
	}
	return checksum;
}

void RSlidarDriverSerialImpl::_disableDataGrabbing()
{
    _isScanning = false;
    _cachethread.join();
	_rxtx->flush(0);
}

}}}