#include "OpenMVOpt.h"

uint8_t OpenMVOpt::Data_OpenMVBuf[8] = { 0 };
uint8_t OpenMVOpt::ROI_Query_CMD[8] = { 0x55,0x02,0x91,0xAA,0x00,0x00,0x00,0xBB };

void OpenMVOpt::AdjustCarPosition(uint8_t Car_Speed)
{
	if (Data_OpenMVBuf[6] <= 5)     // 车身正无需校准
	{
		if (Data_OpenMVBuf[5] == 43)     // +  向左调
		{
			DCMotor.SpeedCtr(Car_Speed, Car_Speed);
		}
		else if (Data_OpenMVBuf[5] == 45)     // -  向右调
		{
			DCMotor.SpeedCtr(Car_Speed, Car_Speed);
		}
	}
	else if (Data_OpenMVBuf[6] <= 15)    // 车身微偏
	{
		if (Data_OpenMVBuf[5] == 43)     // +  向左调
		{
			DCMotor.SpeedCtr(20, 50);
		}
		else if (Data_OpenMVBuf[5] == 45)     // -  向右调
		{
			DCMotor.SpeedCtr(50, 20);
		}
	}
	else if (Data_OpenMVBuf[6] <= 25)
	{

		if (Data_OpenMVBuf[5] == 43)     // +  向左调
		{
			DCMotor.SpeedCtr(10, 50);
		}
		else if (Data_OpenMVBuf[5] == 45)     // -  向右调
		{
			DCMotor.SpeedCtr(50, 10);
		}
	}
	else if (Data_OpenMVBuf[6] <= 35)
	{

		if (Data_OpenMVBuf[5] == 43)     // +  向左调
		{
			DCMotor.SpeedCtr(5, 50);
		}
		else if (Data_OpenMVBuf[5] == 45)     // -  向右调
		{
			DCMotor.SpeedCtr(50, 5);
		}
	}
	else
	{
		AccurateMotor.ForceStop();
		if (Data_OpenMVBuf[5] == 43)     // +  向左调
		{
			AccurateMotor.TurnLeft(Data_OpenMVBuf[6] / 6.5);
		}
		else if (Data_OpenMVBuf[5] == 45)     // -  向右调
		{
			AccurateMotor.TurnRight(Data_OpenMVBuf[6] / 6.5);
		}
		AccurateMotor.DelayUntilCarStop();
		DCMotor.SpeedCtr(Car_Speed, Car_Speed);
	}
}

bool OpenMVOpt::AdjustCarPoseInPlace()
{
	if ((Data_OpenMVBuf[3] != 0x02) || (Data_OpenMVBuf[4] <= 10))
		return false;
	AccurateMotor.ForceStop();
	if (Data_OpenMVBuf[5] == 43)     // +  向左调
	{
		AccurateMotor.TurnLeft(Data_OpenMVBuf[4] / 6.5);
	}
	else if (Data_OpenMVBuf[5] == 45)     // -  向右调
	{
		AccurateMotor.TurnRight(Data_OpenMVBuf[4] / 6.5);
	}
	AccurateMotor.DelayUntilCarStop();
	return true;
}

OpenMVOpt::ImageROIData OpenMVOpt::GetROIData()
{
	ImageROIData data = { 0 };
	ExtSRAMInterface.ExMem_Write_Bytes(0x6008, ROI_Query_CMD, 8);
read_data:
	while (ExtSRAMInterface.ExMem_Read(0x6038) == 0x00)
		delay(1);
	ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Data_OpenMVBuf, 8);
	if ((Data_OpenMVBuf[0] == 0x55) && (Data_OpenMVBuf[1] == 0x02))
	{
		if (Data_OpenMVBuf[2] == 0x81)
		{
			data.left = Data_OpenMVBuf[3];
			data.right = Data_OpenMVBuf[4];
			data.up = Data_OpenMVBuf[5];
			goto read_data;
		}
		else if (Data_OpenMVBuf[2] == 0x82)
		{
			data.middle_up = Data_OpenMVBuf[3];
			data.middle_down = Data_OpenMVBuf[3];
			data.down = Data_OpenMVBuf[3];
			data.data_validate = true;
			return data;
		}
	}
	return { 0 };
}

bool OpenMVOpt::ROIFoundWhite(const OpenMVOpt::ImageROIData& data)
{
	return ((!data.up) || (!data.middle_up) || (!data.middle_down) || (!data.down));
}

void OpenMVOpt::OpenMVTrack_Disc_StartUp()
{
	trackdi_buf[3] = 0x01;                 //开始识别
	Command.Judgment(trackdi_buf);         //计算校验和
	ExtSRAMInterface.ExMem_Write_Bytes(0x6008, trackdi_buf, 8);
}

void OpenMVOpt::OpenMVTrack_Disc_CloseUp()
{
	trackdi_buf[3] = 0x02;                 //关闭识别
	Command.Judgment(trackdi_buf);         //计算校验和
	ExtSRAMInterface.ExMem_Write_Bytes(0x6008, trackdi_buf, 8);
}

void OpenMVOpt::OpenMV_Track(uint8_t Car_Speed, TrackCallback cb, bool enable_white_detect)
{
	uint32_t num = 0;
	// 清空串口缓存
	if (cb == nullptr)
		return;
	while (ExtSRAMInterface.ExMem_Read(0x6038) != 0x00)
		ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Data_OpenMVBuf, 1);
	delay(800);
	OpenMVTrack_Disc_StartUp();
	DCMotor.SpeedCtr(Car_Speed, Car_Speed);
	while (1)
	{
		if (ExtSRAMInterface.ExMem_Read(0x6038) != 0x00)               //检测OpenMV识别结果
		{
			ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Data_OpenMVBuf, 8);
			if ((Data_OpenMVBuf[0] == 0x55) && (Data_OpenMVBuf[1] == 0x02))
			{
				if(Data_OpenMVBuf[2] == 0x91)
				{
					num++;
					Serial.println(num);
					if (Data_OpenMVBuf[4] == 1)  // 路口
					{
						DCMotor.Stop();
						OpenMVTrack_Disc_CloseUp();
						break;
					}
					else  // 调整
						AdjustCarPosition(Car_Speed);
				}
				else if ((Data_OpenMVBuf[2] == 0x80) && enable_white_detect)
				{
					DCMotor.Stop();
					OpenMVTrack_Disc_CloseUp();
					if (cb())
					{
						AccurateMotor.RunBackward(5);
						AccurateMotor.DelayUntilCarStop();
						OpenMVTrack_Disc_StartUp();
					}
				}
			}
		}
	}
}

void OpenMVOpt::OpenMV_TrackInPlace()
{
	uint32_t num = 0;
	// 清空串口缓存
	while (ExtSRAMInterface.ExMem_Read(0x6038) != 0x00)
		ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Data_OpenMVBuf, 1);
	do
	{
		Command.Judgment(ROI_Query_CMD);
		ExtSRAMInterface.ExMem_Write_Bytes(0x6008, ROI_Query_CMD, 8);
		while (ExtSRAMInterface.ExMem_Read(0x6038) == 0x00)
			delay(1);
		ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Data_OpenMVBuf, 8);
		if ((Data_OpenMVBuf[0] != 0x55) || (Data_OpenMVBuf[1] != 0x02) || (Data_OpenMVBuf[2] != 0x80))
			continue;
	} while (AdjustCarPoseInPlace());
}

void OpenMVOpt::OpenMVQr_Disc_StartUp()
{
	qrdi_buf[3] = 0x01;                 //开始识别
	Command.Judgment(qrdi_buf);         //计算校验和
	ExtSRAMInterface.ExMem_Write_Bytes(0x6008, qrdi_buf, 8);
}

void OpenMVOpt::OpenMVQr_Disc_CloseUp()
{
	qrdi_buf[3] = 0x02;                 //关闭识别
	Command.Judgment(qrdi_buf);         //计算校验和
	ExtSRAMInterface.ExMem_Write_Bytes(0x6008, qrdi_buf, 8);
}

void OpenMVOpt::Servo_Control(int8_t angle)
{
	if (angle >= 0)
	{
		servo_buf[4] = 0x2B;
	}
	else
	{
		servo_buf[4] = 0x2D;
	}
	servo_buf[5] = abs(angle);                 //开始识别
	Command.Judgment(servo_buf);         //计算校验和
	ExtSRAMInterface.ExMem_Write_Bytes(0x6008, servo_buf, 8);
	delay(1000);
}