#include "DebugOpt.h"

uint8_t DebugOpt::led_flow_cnt = 0;
uint8_t DebugOpt::open_road_buf[8] = { 0x55,0x03,0x01,0x01,0x00,0x00,0x00,0xBB };
uint8_t DebugOpt::voive_test_buf[9] = { 0xFD,0x00,0x06,0x01,0x01,0xC4,0xFA,0xBA,0xC3 };
uint8_t DebugOpt::voice_trm_buf[8] = { 0xAF,0x06,0x00,0x02,0x00,0x00,0x01,0xBB };
uint8_t DebugOpt::ir_repo_buf[6] = { 0x03,0x05,0x14,0x45,0xDE,0x92 };

void DebugOpt::DoLEDFlow()
{
	switch (led_flow_cnt)
	{
	case 0:
		CoreLED.TurnOn(LED1);
		CoreLED.TurnOff(LED2);
		CoreLED.TurnOff(LED3);
		CoreLED.TurnOff(LED4);

		LED.LeftTurnOn();
		LED.RightTurnOff();

		digitalWrite(LED_BUILTIN, HIGH);
		break;
	case 1:
		CoreLED.TurnOff(LED1);
		CoreLED.TurnOn(LED2);
		CoreLED.TurnOff(LED3);
		CoreLED.TurnOff(LED4);

		LED.LeftTurnOff();
		LED.RightTurnOn();

		digitalWrite(LED_BUILTIN, LOW);
		break;
	case 2:
		CoreLED.TurnOff(LED1);
		CoreLED.TurnOff(LED2);
		CoreLED.TurnOn(LED3);
		CoreLED.TurnOff(LED4);

		LED.LeftTurnOn();
		LED.RightTurnOff();

		digitalWrite(LED_BUILTIN, LOW);
		break;
	case 3:
		CoreLED.TurnOff(LED1);
		CoreLED.TurnOff(LED2);
		CoreLED.TurnOff(LED3);
		CoreLED.TurnOn(LED4);

		LED.LeftTurnOff();
		LED.RightTurnOn();

		digitalWrite(LED_BUILTIN, LOW);
		break;
	}
	led_flow_cnt++;
	if (led_flow_cnt >= 4)
		led_flow_cnt = 0;
	delay(200);
}

void DebugOpt::DebugRun()
{
	Serial.println("Hello Accurate DCMotor.");
	LED.LeftTurnOn();
	LED.RightTurnOn();
	BEEP.TurnOn();
	delay(100);
	BEEP.TurnOff();
	/*DCMotor.Go(100, 100);
	DCMotor.Stop();
	DCMotor.Back(100, 100);
	DCMotor.Stop();
	DCMotor.TurnLeft_Code(200, 100);
	DCMotor.Stop();
	DCMotor.TurnRight_Code(200, 100);
	DCMotor.Stop();*/
	AccurateMotor.RunForward(10);
	AccurateMotor.DelayUntilCarStop();
	AccurateMotor.RunBackward(10);
	AccurateMotor.DelayUntilCarStop();
	AccurateMotor.TurnLeft(20);
	AccurateMotor.DelayUntilCarStop();
	AccurateMotor.TurnRight(20);
	AccurateMotor.DelayUntilCarStop();
	BEEP.TurnOn();
	delay(100);
	BEEP.TurnOff();
	LED.LeftTurnOff();
	LED.RightTurnOff();
}

void DebugOpt::DebugServo()
{
	Serial.println("Hello Servo.");
	for (int i = -80; i <= 20; i++)
	{
		OpenMVOpt::Servo_Control(i);
		delay(50);
	}
	OpenMVOpt::Servo_Control(0);
}

void DebugOpt::Road_Gate_Test()
{
	Command.Judgment(open_road_buf);         //计算校验和
	ExtSRAMInterface.ExMem_Write_Bytes(0x6008, open_road_buf, 8);
}

void DebugOpt::Speech_Sounds_Ctr()
{
	ExtSRAMInterface.ExMem_Write_Bytes(0x6180, voive_test_buf, 13);
}

void DebugOpt::Speech_Disc()
{
	uint8_t sph_id = 0;
	/*0为控制语音播报标志物随机播报并开启识别，
	2-6为指定词条识别，具体信息可查看通信协议*/
	sph_id = BKRC_Voice.BKRC_Voice_Extern(0);
	delay(200);
	Serial.print("识别到的词条ID为：");
	Serial.println(sph_id, HEX);
	voice_trm_buf[2] = sph_id;
	ExtSRAMInterface.ExMem_Write_Bytes(0x6008, voice_trm_buf, 8);//上传识别结果至评分终端
}

void DebugOpt::Infrared_Test()
{
	Infrare.Transmition(ir_repo_buf, 6);
}