#include "Handler.h"

void Handler::AutoRunnerCallback(String point, AutoPathRunner::CarHeadPosition pos)
{
	//Serial.print("Callback(");
	//Serial.print((point == "") ? "EMPTY" : point);
	//Serial.println(String(",") + (int)pos + ")");
	AccurateMotor.ForceStop();
	OpenMVOpt::OpenMVTrack_Disc_CloseUp();
	AccurateMotor.ForceStop();
	if ((point == "D2")/* && (pos == AutoPathRunner::CarHeadPosition::Y_Negitive)*/)
	{
		TrafficLightCommand cmd;
		ZigBeeOperator.SendCommand(cmd.CMD_EnterIdentifyMode());
		delay(500);
		ZigBeeOperator.SendCommand(cmd.CMD_CheckRandomResult());
		delay(500);
		AccurateMotor.RunBackwardSlow(3);
		AccurateMotor.DelayUntilCarStop();
		OpenMVOpt::OpenMV_TrackInPlace();
		AccurateMotor.RunForwardSlow(9);
		AccurateMotor.DelayUntilCarStop();
	}
	else if (point == "B4")
	{
		_3DDisplayCommand cmd;
		uint8_t data[128] = { 0 };
		AccurateMotor.TurnLeft(45);
		AccurateMotor.DelayUntilCarStop();
		cmd.CMD_IR_CarLicenseMode_1to4('A', 'B', 'C', 'D');
		delay(200);
		cmd.CMD_IR_CarLicenseMode_5to6_XY('E', 'F', 'E', '2');
		AccurateMotor.TurnRight(45);
		AccurateMotor.DelayUntilCarStop();
		/*AccurateMotor.TurnRight(90);
		AccurateMotor.DelayUntilCarStop();
		OpenMVOpt::Servo_Control(0);
		delay(500);
		OpenMVOpt::OpenMVQr_Disc_StartUp();
		bool ret = OpenMVOpt::OpenMV_WaitQRData(data, 128, 2000);
		OpenMVOpt::OpenMVQr_Disc_CloseUp();
		AccurateMotor.TurnLeft(135);
		AccurateMotor.DelayUntilCarStop();
		if (!ret)
		{
			cmd.CMD_IR_CarLicenseMode_1to4('A', 'B', 'C', 'D');
			delay(200);
			cmd.CMD_IR_CarLicenseMode_5to6_XY('E', 'F', 'E', '2');
		}
		else
		{
			String str = OpenMVOpt::DecodeQRData(data, 128);
			_3DDisplayCommand cmd;
			cmd.CMD_IR_CarLicenseMode_1to4(str[0], str[1], str[2], str[3]);
			delay(200);
			cmd.CMD_IR_CarLicenseMode_5to6_XY(str[4], str[5], 'E', '2');
		}
		OpenMVOpt::Servo_Control(-60);
		delay(500);
		AccurateMotor.TurnRight(135);
		AccurateMotor.DelayUntilCarStop();*/
	}
}

void Handler::ZigBeeRx_Handler(uint8_t* mar)
{
	CommandDecoder decoder(mar);
	switch (decoder.GetDevice())
	{
	case CommandData::Devices::MainCar:        //主车
		MainCarRx_Handler(mar);       //主车命令相应函数
		break;
	case CommandData::Devices::Barrier:        //道闸标志物
		DataTool::PrintDataArray(mar, 8);
		break;
	case CommandData::Devices::LED_Display:        //LED显示标志物（暂无返回）
		break;
	case CommandData::Devices::Garage_B:        //立体车库标志物B
	{
		GarageCommand cmd;
		if (cmd.IsPositionData(mar))
		{
			//Serial.print("Garage at: ");
			//Serial.println(cmd.CurrentPos(mar));
		}
		if (cmd.IsIRData(mar))
		{
			//Serial.print("Front IR: ");
			//Serial.println(cmd.FrontIRTiggered(mar) ? "Triggered" : "Not Triggered");
			//Serial.print("Back IR: ");
			//Serial.println(cmd.BackIRTiggered(mar) ? "Triggered" : "Not Triggered");
		}
		break;
	}
	case CommandData::Devices::VoiceReport:        //语音播报标志物
	{
		VoiceReportCommand cmd;
		uint16_t y;
		byte m, d, h, mm, s;
		if (cmd.ReadRTCDate(mar, y, m, d))
		{
			//Serial.print(y);
			//Serial.print("-");
			//Serial.print(m);
			//Serial.print("-");
			//Serial.println(d);
		}
		else
			//Serial.println("Invalidate Date");
		if (cmd.ReadRTCTime(mar, h, mm, s))
		{
			//Serial.print(h);
			//Serial.print(":");
			//Serial.print(mm);
			//Serial.print(":");
			//Serial.println(s);
		}
		else
			//Serial.println("Invalidate Time");
		break;
	}
	case CommandData::Devices::AlarmTower:        //红外报警标志物
	{
		AlarmTowerCommand cmd;
		if (cmd.IsAlarmTowerCommand(mar))
		{
			//Serial.print("Rescue Position: ");
			//Serial.println(cmd.ReadRescuePosition(mar), HEX);
		}
		break;
	}
	case CommandData::Devices::TFT_B:        //TFT显示标志物B
		break;
	case CommandData::Devices::WirelessCharger:        //磁悬浮无线充电标志物
		break;
	case CommandData::Devices::TFT_A:        //TFT显示标志物A
		break;
	case CommandData::Devices::ETC:        //ETC系统标志物
		break;
	case CommandData::Devices::Garage_A:        //立体车库标志物A
		break;
	case CommandData::Devices::TrafficLight_A:        //交通灯标注物A 
	{
		TrafficLightCommand cmd;
		if (cmd.ModeChangeSuccess(mar))
			Serial.println("Enter Identify Mode");
		else
			Serial.println("Mode Change Failure");
		break;
	}
	case CommandData::Devices::TrafficLight_B:        //交通灯标志物B
		break;
	case CommandData::Devices::SpecTerrain:
	{
		SpecTerrainCommand cmd;
		if(cmd.IsSpecTerrainCommand(mar))
		{
			switch (cmd.ReadStatus(mar))
			{
			case SpecTerrainCommand::Status::Passed_AtoB:
			case SpecTerrainCommand::Status::Passed_BtoA:
				Serial.println("Passed");
				break;
			case SpecTerrainCommand::Status::NotPass:
				Serial.println("Not Pass");
				break;
			default:
				Serial.println("Data Invalidate");
			}
		}
		else
			Serial.println("Command Invalidate");
		break;
	}
	default:
		break;
	}
}

void Handler::OpenMVRx_Handler(uint8_t* mac)
{
	switch (mac[2])
	{
	case 0x91:        //保留

		break;
	case 0x92:        //二维码识别

		break;
	default:
		break;
	}
}

void Handler::MainCarRx_Handler(uint8_t* com)
{
	CommandDecoder decoder(com);
	if (!decoder.CommandValidate())
		return;
	switch(decoder.GetMainCommand())
	{
	case CommandData::MainCarCommand::StartRun:
		int point_cnt = 0;
		CommandEncoder encoder;
		encoder.SetDevice(0xAA);	//主车
		encoder.SetCommand(0xAA, 0xAA, 0xAA, 0xAA);
		AutoPathRunner::MapPoint* points = AutoPathRunner::DecodeRoute("E2-D2-B2-B4-B6-B7", point_cnt);
		AutoPathRunner::DoRun(points, point_cnt, AutoPathRunner::CarHeadPosition::Y_Negitive, AutoRunnerCallback);
		ZigBeeOperator.SendCommand(encoder.GetCommandArray());
		delay(200);
		ZigBeeOperator.SendCommand(encoder.GetCommandArray());
		OpenMVOpt::Servo_Control(-60);
		delay(1500);
		AutoPathRunner::ReversingIntoGarage(true, 3);
		break;
	case CommandData::MainCarCommand::RunningRoute:
		dynamic_route_recv = "";
		dynamic_route_recv.concat((char)decoder.GetDataBit(1));
		dynamic_route_recv.concat((char)decoder.GetDataBit(2));
		dynamic_route_recv.concat('-');
		//Serial.print("Route Command: ");
		//Serial.println(dynamic_route_recv);
		break;
	case CommandData::MainCarCommand::RunningRouteData:
		dynamic_route_recv.concat((char)decoder.GetDataBit(1));
		dynamic_route_recv.concat((char)decoder.GetDataBit(2));
		if (decoder.GetDataBit(3) == CommandData::MainCarCommand::RouteDataStop)
		{
			dynamic_route_recv += "-B7";
			//Serial.println(dynamic_route_recv);
			//Serial.println("Run!");
			int point_cnt = 0;
			AutoPathRunner::MapPoint* points = AutoPathRunner::DecodeRoute(dynamic_route_recv.c_str(), point_cnt);
			AutoPathRunner::DoRun(points, point_cnt, AutoPathRunner::CarHeadPosition::Y_Negitive, AutoRunnerCallback);
			delay(1500);
			AutoPathRunner::ReversingIntoGarage(true, 3);
		}
		else
		{
			dynamic_route_recv.concat('-');
			//Serial.println(dynamic_route_recv);
		}
		break;
	}
	switch (com[2])
	{
	case 0x01:                                         //停止
		//DCMotor.Stop();
		AccurateMotor.ForceStop();
		break;
	case 0x02:                                        //前进
		//DCMotor.Go(com[3], (com[4] + (com[5] << 8)));
		AccurateMotor.RunForward((com[4] + (com[5] << 8)));
		ZigBee_back[2] = 0x03;
		break;
	case 0x03:                                        //后退
		//DCMotor.Back(com[3], (com[4] + (com[5] << 8)));
		AccurateMotor.RunBackward((com[4] + (com[5] << 8)));
		ZigBee_back[2] = 0x03;
		break;
	case 0x04:                                        //左转
		//DCMotor.TurnLeft(com[3]);
		AccurateMotor.TurnLeft(com[3]);
		ZigBee_back[2] = 0x02;
		break;
	case 0x05:                                        //右转
		//DCMotor.TurnRight(com[3]);
		AccurateMotor.TurnRight(com[3]);
		ZigBee_back[2] = 0x02;
		break;
	case 0x06:                                        //循迹
		//DCMotor.CarTrack(com[3]);
		ZigBee_back[2] = 0x00;
		break;
	case 0x07:                                        //码盘清零
		Command.Judgment(Command.command01);                        //计算校验和                
		ExtSRAMInterface.ExMem_Write_Bytes(Command.command01, 8);   //码盘清零
		break;
	case 0x10:                                       //保存红外数据
		infrare_com[0] = com[3];
		infrare_com[1] = com[4];
		infrare_com[2] = com[5];
		break;
	case 0x11:                                       //保存红外数据
		infrare_com[3] = com[3];
		infrare_com[4] = com[4];
		infrare_com[5] = com[5];
		break;
	case 0x12:                                        //发送红外数据
		Infrare.Transmition(infrare_com, 6);
		break;
	case 0x20:                                        //左右转向灯
		if (com[3] == 0x01)
			LED.LeftTurnOn();
		else
			LED.LeftTurnOff();
		if (com[4] == 0x01)
			LED.RightTurnOn();
		else
			LED.RightTurnOff();
		break;
	case 0x30:                                        //蜂鸣器
		if (com[3] == 0x01)
			BEEP.TurnOn();
		else
			BEEP.TurnOff();
		break;
	case 0x40:                                        //保留
		break;
	case 0x50:                                        //LCD图片上翻页
		Command.Judgment(Command.command13);
		ExtSRAMInterface.ExMem_Write_Bytes(Command.command13, 8);
		break;
	case 0x51:                                        //LCD图片下翻页
		Command.Judgment(Command.command14);
		ExtSRAMInterface.ExMem_Write_Bytes(Command.command14, 8);
		break;
	case 0x61:                                        //光源加一档
		Infrare.Transmition(Command.HW_Dimming1, 4);
		break;
	case 0x62:                                        //光源加二挡
		Infrare.Transmition(Command.HW_Dimming2, 4);
		break;
	case 0x63:                                        //光源加三挡
		Infrare.Transmition(Command.HW_Dimming3, 4);
		break;
	case 0x80:                                        //从车返回数据切换                                       
		if (com[3] == 0x01)
		{
			sendflag = 0x01;                             //上传从车数据
		}
		else
		{
			sendflag = 0x00;                              //关闭上从车数据
		}
	case 0x91:
		if (com[3] == 0x03)                               //舵机控制
		{
			//Servoangle_control(com[4]);
		}
		break;
	default:
		break;
	}
}

void Handler::Key_0()
{
	int point_cnt = 0;
	AutoPathRunner::MapPoint* points = AutoPathRunner::DecodeRoute("E2-D2-B2-B4-B6-B7", point_cnt);
	AutoPathRunner::DoRun(points, point_cnt, AutoPathRunner::CarHeadPosition::Y_Negitive, AutoRunnerCallback);
	delay(1500);
	AutoPathRunner::ReversingIntoGarage(true, 3);
}

void Handler::Key_1()
{
	/*OpenMVOpt::OpenMVTrack_Disc_CloseUp();
	delay(500);
	OpenMVOpt::Servo_Control(0);*/
	AccurateMotor.TurnLeft(90);
	AccurateMotor.DelayUntilCarStop();
	delay(2000);
	AccurateMotor.TurnLeft(180);
	AccurateMotor.DelayUntilCarStop();
}

void Handler::Key_2()
{
	OpenMVOpt::Servo_Control(-60);
	delay(500);
	OpenMVOpt::OpenMV_Track(8, []() { return false; }, false);
	AutoPathRunner::ReversingIntoGarage(true, 0x03);
}

void Handler::Key_3()
{
	uint8_t data[128] = { 0 };
	OpenMVOpt::OpenMVQr_Disc_StartUp();
	if (!OpenMVOpt::OpenMV_WaitQRData(data, 128, 2000))
	{
		OpenMVOpt::OpenMVQr_Disc_CloseUp();
		LED.LeftTurnOn();
	}
	else
	{
		OpenMVOpt::OpenMVQr_Disc_CloseUp();
		LED.RightTurnOn();
		AccurateMotor.TurnLeft(135);
		AccurateMotor.DelayUntilCarStop();
		String str = OpenMVOpt::DecodeQRData(data, 128);
		_3DDisplayCommand cmd;
		cmd.CMD_IR_CarLicenseMode_1to4(str[0], str[1], str[2], str[3]);
		delay(200);
		cmd.CMD_IR_CarLicenseMode_5to6_XY(str[4], str[5], 'B', '6');
	}
}

void Handler::KEY_Handler(uint8_t k_value)
{
	switch (k_value)
	{
	case 1:
		Key_0();
		break;
	case 2:
		Key_1();
		break;
	case 3:
		Key_2();
		break;
	case 4:
		Key_3();
		break;
	}
}