#define WIN32_LEAN_AND_MEAN
#include <WinSock2.h>
#include "PCANBasic.h"
#include "./include/EgoInterface.h"
#include "EgoClient.h"
#include "EgoWindow.h"
#include "protocol.pb.h"

#include "../common/iobuffer.h"
#include "protocol.pb.h"
#include "control_sensor.h"
#include <algorithm>
#include <string>

#define MAX_DGREE (int32_t)(4096/6)
inline int16_t make_int16(int8_t h, int8_t l)
{
	int16_t hi = (int16_t)(h & 0x00FF);
	int16_t low = (int16_t)(l & 0x00FF);

	return (hi << 8) | low;
}
inline int8_t hi_byte(int16_t value)
{
	int8_t hi =(int8_t)((value & 0xFF00) >> 8);
	return hi;
}
inline int8_t lo_byte(int16_t value)
{
	int8_t lo = (int8_t)(value & 0xff);
	return lo;
}
CControlSensor::CControlSensor(CMessageQueue* q) 
{
	_window = static_cast<CEgoWindow*>(q);
	_collection = Sensor_Collection::Sensor_None;
	
	 
}
void CControlSensor::Notify(TPCANMsg& canmsg,uint64_t timestamp)
{
	 
		//OutputDebugString(str);
		//OutputDebugString(ret.c_str());
	   //发送座椅信号给下位机
	switch (canmsg.ID)
	{

		case 0x181:
		{
			//接收181msg
			frames[0].canid = canmsg.ID;
			frames[0].dlc = canmsg.LEN;
			memcpy(&frames[0].data, canmsg.DATA, sizeof(canmsg.DATA));
			//flag收到181
			_collection = (Sensor_Collection)(_collection | Sensor_Collection::Sensor_181);
		}
		break;
		case 0x182:
		{
			//接收182msg
			frames[1].canid = canmsg.ID;
			frames[1].dlc = canmsg.LEN;
			memcpy(&frames[1].data, canmsg.DATA, sizeof(canmsg.DATA));
			//flag收到182
			_collection = (Sensor_Collection)(_collection | Sensor_Collection::Sensor_182);
		}
		break;
		case 0x183:
		{			//接收183msg
			frames[2].canid = canmsg.ID;
			frames[2].dlc = canmsg.LEN;
			memcpy(&frames[2].data, canmsg.DATA, sizeof(canmsg.DATA));
			//flag收到183
			_collection = (Sensor_Collection)(_collection | Sensor_Collection::Sensor_183);
		}
		break;
		case 0x184:
		//case 0x611:
		{

			//接收184msg
			frames[3].canid = canmsg.ID;
			frames[3].dlc = canmsg.LEN;
			memcpy(&frames[3].data, canmsg.DATA, sizeof(canmsg.DATA));
			//flag收到184
			_collection = (Sensor_Collection)(_collection | Sensor_Collection::Sensor_184);

			//
			//解读184信号，发送到Qt显示（灯光与档位状态和声音的改变）
			bool cautionLight = frames[3].data[0] & 0x4;
			bool lightL = frames[3].data[0] & 0x8;
			bool lightR = frames[3].data[0] & 0x10;
			bool gearF = frames[3].data[0] & 0x20;
			bool gearR = frames[3].data[0] & 0x40;
			bool carPark = frames[3].data[1] & 0x1;
			bool buzzerPlay = frames[3].data[1] & 0x2;
			bool lightFront = frames[3].data[2] & 0x10;
			bool lightWork = frames[3].data[2] & 0x20;
			bool emergency = frames[3].data[3] & 0x2;

			_window->GetEgoClient()->OnCautionLight(cautionLight);
			_window->GetEgoClient()->OnLightL(lightL);
			_window->GetEgoClient()->OnLightR(lightR);
			_window->GetEgoClient()->OnGearF(gearF);
			_window->GetEgoClient()->OnGearR(gearR);
			_window->GetEgoClient()->OnCarPark(carPark);
			_window->GetEgoClient()->OnBuzzerPlay(buzzerPlay);
			_window->GetEgoClient()->OnLightFront(lightFront);
			_window->GetEgoClient()->OnLightWork(lightWork);
			_window->GetEgoClient()->OnEmergency(emergency);

		}
		break;
		case 0x185:
		{
			//接收185msg
			frames[4].canid = canmsg.ID;
			frames[4].dlc = canmsg.LEN;
			memcpy(&frames[4].data, canmsg.DATA, sizeof(canmsg.DATA));
			//flag收到185
			_collection = (Sensor_Collection)(_collection | Sensor_Collection::Sensor_185);
		}
		break;

	}
	 
	
	 
	if (_window->GetControlState() == ControlState::Process && _collection == Sensor_Collection::Sensor_All)
	{
		static DWORD tick = GetTickCount();
		RemoNet::CCCanMsg req;
		for (int32_t i = 0; i < _countof(frames); i++)
		{
			if (frames[i].canid != 0)
			{
				RemoNet::can_net_frame* frame = req.add_frams();
				frame->set_canid(frames[i].canid);
				frame->set_dlc(frames[i].dlc);
				frame->set_data(frames[i].data, frames[i].dlc);
			}
		}
		MessageHead Head;
		CIOBuffer pBuffer;
		Head.Command = RemoNet::CC_CANMSG;
		Head.Length = req.ByteSizeLong();
		Head.Serialize(pBuffer.Buffer);
		auto ptr = pBuffer.Buffer + MessageHead::Size();
		req.SerializeToArray(ptr, Head.Length);
		pBuffer.Length = MessageHead::Size() + Head.Length;
		_window->SendData(pBuffer);
		_collection = Sensor_Collection::Sensor_None;
		/*DWORD diff = GetTickCount() - tick;
		tick = GetTickCount();
		std::string str = std::to_string(diff);
		str += "\n";
		OutputDebugStringA(str.c_str());
		*/
	}
}
 
ControlStatus CControlSensor::CheckStatus()
{
	return ControlStatus::Ok;
}
bool CControlSensor::Start()
{
	int iBuffer;
	TPCANStatus stsResult;
	TPCANHandle _HandlesArray[16];
	_HandlesArray[0] = PCAN_USBBUS1;
	_HandlesArray[1] = PCAN_USBBUS2;
	_HandlesArray[2] = PCAN_USBBUS3;
	_HandlesArray[3] = PCAN_USBBUS4;
	_HandlesArray[4] = PCAN_USBBUS5;
	_HandlesArray[5] = PCAN_USBBUS6;
	_HandlesArray[6] = PCAN_USBBUS7;
	_HandlesArray[7] = PCAN_USBBUS8;
	_HandlesArray[8] = PCAN_USBBUS9;
	_HandlesArray[9] = PCAN_USBBUS10;
	_HandlesArray[10] = PCAN_USBBUS11;
	_HandlesArray[11] = PCAN_USBBUS12;
	_HandlesArray[12] = PCAN_USBBUS13;
	_HandlesArray[13] = PCAN_USBBUS14;
	_HandlesArray[14] = PCAN_USBBUS15;
	_HandlesArray[15] = PCAN_USBBUS16;
	for (int i = 0; i < (sizeof(_HandlesArray) / sizeof(TPCANHandle)); i++)
	{
		stsResult = CAN_GetValue(_HandlesArray[i], PCAN_CHANNEL_CONDITION, &iBuffer, sizeof(iBuffer));
		if (((stsResult) == PCAN_ERROR_OK) && ((iBuffer & PCAN_CHANNEL_AVAILABLE) == PCAN_CHANNEL_AVAILABLE))
		{
			stsResult = CAN_GetValue((TPCANHandle)_HandlesArray[i], PCAN_CHANNEL_FEATURES, (void*)&iBuffer, sizeof(iBuffer));
			_isFD = (stsResult == PCAN_ERROR_OK) && (iBuffer & FEATURE_FD_CAPABLE);
			_handle = _HandlesArray[i];
			break;
		}
	}
	if (_handle != -1)
	{
		TPCANStatus stsResult = CAN_Initialize(_handle, PCAN_BAUD_250K);
		//使用雷达盒子测试用500k
		//TPCANStatus stsResult = CAN_Initialize(_handle, PCAN_BAUD_500K);
		if (stsResult != PCAN_ERROR_OK)
		{
			return false;
		}
		_thread = std::thread(&CControlSensor::Run, this);
	}
	return _handle != -1;
}
void CControlSensor::Run()
{
	_run = true;
	while (_run)
	{
		for (int32_t i = 0; i < sizeof(frames) / sizeof(cannet_frame); i++)
		{
			frames[i].canid = 0;
		}
		Sleep(10);
		ReadMessages();
	}
	CAN_Uninitialize(_handle);

}
void CControlSensor::ReadMessages()
{
	TPCANStatus stsResult;

	// We read at least one time the queue looking for messages. If a message is found, we look again trying to
	// find more. If the queue is empty or an error occurr, we get out from the dowhile statement.
	do
	{
		ReadMessage();
		if (stsResult != PCAN_ERROR_OK && stsResult != PCAN_ERROR_QRCVEMPTY)
		{
			//ShowStatus(stsResult);
			return;
		}
	} while (!(stsResult & PCAN_ERROR_QRCVEMPTY));
}
 
TPCANStatus CControlSensor::ReadMessage()
{
	TPCANMsg CANMsg;
	TPCANTimestamp CANTimeStamp;

	// We execute the "Read" function of the PCANBasic
	TPCANStatus stsResult = CAN_Read(_handle, &CANMsg, &CANTimeStamp);
	if (stsResult != PCAN_ERROR_QRCVEMPTY)
	{	// We process the received message
		uint64_t newTimestamp = (CANTimeStamp.micros + 1000 * CANTimeStamp.millis + 0x100000000 * 1000 * CANTimeStamp.millis_overflow);
		Notify(CANMsg, newTimestamp);
	}

	return stsResult;
}

//将PCAN信号传回座椅PLC中
TPCANStatus CControlSensor::SendCANMessage(TPCANMsg* CANMsg)
{
	// 执行PCANBasic的“写入”功能
	TPCANStatus stsResult = CAN_Write(_handle, CANMsg);
	return stsResult;
}

void CControlSensor::Stop()
{
	_run = false;
	_thread.join();
}
void CControlSensor::SetEngineRPM(int32_t value)
{
	_rpm = value;
}