// 
// 自动路径运行程序 - 修改自原MCR，去除了对STL库的依赖
// 

#include "AutoPathRunner.h"

AutoPathRunner::MapPoint::MapPoint()
{
	x = 0;
	y = 0;
}

AutoPathRunner::MapPoint::MapPoint(int x, int y)
{
	this->x = x;
	this->y = y;
}

int AutoPathRunner::MapPoint::GetX()
{
	return x;
}

int AutoPathRunner::MapPoint::GetY()
{
	return y;
}

void AutoPathRunner::MapPoint::SetX(int x)
{
	this->x = x;
}

void AutoPathRunner::MapPoint::SetY(int y)
{
	this->y = y;
}

bool AutoPathRunner::MapPoint::NeedTurn(MapPoint next, CarHeadPosition pos)
{
	if ((pos == CarHeadPosition::X_Positive) || (pos == CarHeadPosition::X_Negitive))
		return (x != next.GetX());
	else
		return (y != next.GetY());
}

AutoPathRunner::TurnDirection AutoPathRunner::MapPoint::CalcTurnDirection(MapPoint next, CarHeadPosition pos)
{
	TurnDirection direction;
	if (x == next.GetX())
	{
		if (y < next.GetY())
			direction = TurnDirection::Right;
		else if (y > next.GetY())
			direction = TurnDirection::Left;
		else
			direction = TurnDirection::Null;
	}
	else if (y == next.GetY())
	{
		if (x < next.GetX())
			direction = TurnDirection::Right;
		else if (x > next.GetX())
			direction = TurnDirection::Left;
		else
			direction = TurnDirection::Null;
	}
	else
		direction = TurnDirection::Null;
	if ((pos == CarHeadPosition::X_Negitive) || (pos == CarHeadPosition::Y_Negitive))
	{
		if (direction == TurnDirection::Left)
			direction = TurnDirection::Right;
		else if (direction == TurnDirection::Right)
			direction = TurnDirection::Left;
	}
	return direction;
}

AutoPathRunner::MapPoint AutoPathRunner::MapPoint::operator=(const MapPoint& obj)
{
	this->x = obj.x;
	this->y = obj.y;
	return (*this);
}

bool AutoPathRunner::MapPoint::operator==(const MapPoint& obj)
{
	return ((x == obj.x) && (y == obj.y));
}

AutoPathRunner::PointMapper::PointMapper()
{
}

AutoPathRunner::PointMapper::PointMapper(const char* n, const MapPoint& p)
{
	strncpy(name, n, 3);
	point = p;
}

AutoPathRunner::PointMapper std_points[49] =
{
	{"A1", AutoPathRunner::MapPoint(0,0)},
	{"A2", AutoPathRunner::MapPoint(0,1)},
	{"A3", AutoPathRunner::MapPoint(0,2)},
	{"A4", AutoPathRunner::MapPoint(0,3)},
	{"A5", AutoPathRunner::MapPoint(0,4)},
	{"A6", AutoPathRunner::MapPoint(0,5)},
	{"A7", AutoPathRunner::MapPoint(0,6)},
	{"B1", AutoPathRunner::MapPoint(1,0)},
	{"B2", AutoPathRunner::MapPoint(1,1)},
	{"B3", AutoPathRunner::MapPoint(1,2)},
	{"B4", AutoPathRunner::MapPoint(1,3)},
	{"B5", AutoPathRunner::MapPoint(1,4)},
	{"B6", AutoPathRunner::MapPoint(1,5)},
	{"B7", AutoPathRunner::MapPoint(1,6)},
	{"C1", AutoPathRunner::MapPoint(2,0)},
	{"C2", AutoPathRunner::MapPoint(2,1)},
	{"C3", AutoPathRunner::MapPoint(2,2)},
	{"C4", AutoPathRunner::MapPoint(2,3)},
	{"C5", AutoPathRunner::MapPoint(2,4)},
	{"C6", AutoPathRunner::MapPoint(2,5)},
	{"C7", AutoPathRunner::MapPoint(2,6)},
	{"D1", AutoPathRunner::MapPoint(3,0)},
	{"D2", AutoPathRunner::MapPoint(3,1)},
	{"D3", AutoPathRunner::MapPoint(3,2)},
	{"D4", AutoPathRunner::MapPoint(3,3)},
	{"D5", AutoPathRunner::MapPoint(3,4)},
	{"D6", AutoPathRunner::MapPoint(3,5)},
	{"D7", AutoPathRunner::MapPoint(3,6)},
	{"E1", AutoPathRunner::MapPoint(4,0)},
	{"E2", AutoPathRunner::MapPoint(4,1)},
	{"E3", AutoPathRunner::MapPoint(4,2)},
	{"E4", AutoPathRunner::MapPoint(4,3)},
	{"E5", AutoPathRunner::MapPoint(4,4)},
	{"E6", AutoPathRunner::MapPoint(4,5)},
	{"E7", AutoPathRunner::MapPoint(4,6)},
	{"F1", AutoPathRunner::MapPoint(5,0)},
	{"F2", AutoPathRunner::MapPoint(5,1)},
	{"F3", AutoPathRunner::MapPoint(5,2)},
	{"F4", AutoPathRunner::MapPoint(5,3)},
	{"F5", AutoPathRunner::MapPoint(5,4)},
	{"F6", AutoPathRunner::MapPoint(5,5)},
	{"F7", AutoPathRunner::MapPoint(5,6)},
	{"G1", AutoPathRunner::MapPoint(6,0)},
	{"G2", AutoPathRunner::MapPoint(6,1)},
	{"G3", AutoPathRunner::MapPoint(6,2)},
	{"G4", AutoPathRunner::MapPoint(6,3)},
	{"G5", AutoPathRunner::MapPoint(6,4)},
	{"G6", AutoPathRunner::MapPoint(6,5)},
	{"G7", AutoPathRunner::MapPoint(6,6)}
};

AutoPathRunner::MapPoint AutoPathRunner::Mapping(const char* name)
{
	for (auto& [key, value] : std_points)
		if (strcmp(key, name) == 0)
			return value;
	return MapPoint();
}

const char* AutoPathRunner::ReverseMapping(MapPoint point)
{
	for (auto& [key, value] : std_points)
		if (value == point)
			return key;
	return "";
}

void AutoPathRunner::ChangePositon(CarHeadPosition& pos, TurnDirection turn)
{
	switch (pos)
	{
	case CarHeadPosition::X_Positive:
		if (turn == TurnDirection::Left)
			pos = CarHeadPosition::Y_Negitive;
		else if (turn == TurnDirection::Right)
			pos = CarHeadPosition::Y_Positive;
		break;
	case CarHeadPosition::X_Negitive:
		if (turn == TurnDirection::Left)
			pos = CarHeadPosition::Y_Positive;
		else if (turn == TurnDirection::Right)
			pos = CarHeadPosition::Y_Negitive;
		break;
	case CarHeadPosition::Y_Positive:
		if (turn == TurnDirection::Left)
			pos = CarHeadPosition::X_Positive;
		else if (turn == TurnDirection::Right)
			pos = CarHeadPosition::X_Negitive;
		break;
	case CarHeadPosition::Y_Negitive:
		if (turn == TurnDirection::Left)
			pos = CarHeadPosition::X_Negitive;
		else if (turn == TurnDirection::Right)
			pos = CarHeadPosition::X_Positive;
		break;
	}
}

AutoPathRunner::CarHeadPosition AutoPathRunner::Int2Position(int pos)
{
	switch (pos)
	{
	case 0:
		return CarHeadPosition::X_Positive;
	case 1:
		return CarHeadPosition::X_Negitive;
	case 2:
		return CarHeadPosition::Y_Positive;
	default:
		return CarHeadPosition::Y_Negitive;
	}
}

bool AutoPathRunner::TrackCallback()
{
	AccurateMotor.RunBackwardSlow(6);
	AccurateMotor.DelayUntilCarStop();
	OpenMVOpt::AdjustCarPoseInPlace();
	if (IsSpecTerrainItem())
	{
		OpenMVOpt::AdjustCarPoseInPlace();
		AccurateMotor.RunForwardSlow(30);
	}
	else
	{
		OpenMVOpt::AdjustCarPoseInPlace();
		AccurateMotor.RunForwardSlow(12);
	}
	AccurateMotor.DelayUntilCarStop();
	return true;
}

AutoPathRunner::MapPoint* AutoPathRunner::DecodeRoute(const char* route, int& point_cnt)
{
	char temp[3] = { 0 };
	MapPoint* list = new MapPoint[(strlen(route) + 1) / 3];
	int str_index = 0, point_index = 0;
	char* route_cpy = new char[strlen(route) + 1];
	memset(route_cpy, 0, strlen(route) + 1);
	strncpy(route_cpy, route, strlen(route));
	memset(list, 0, sizeof(list));
	while (sscanf(route_cpy, "%c%c-", &temp[0], &temp[1]) != 0)
	{
		str_index += 3;
		if (str_index >= strlen(route))
			break;
		if (point_index < ((strlen(route) + 1) / 3))
			list[point_index++] = Mapping(temp);
		memset(temp, 0, sizeof(temp));
		memset(route_cpy, 0, strlen(route) + 1);
		strncpy(route_cpy, route + str_index, strlen(route) - str_index);
	}
	point_cnt = point_index;
	delete[] route_cpy;
	return list;
}

void AutoPathRunner::DoRun(MapPoint* points, int point_cnt, CarHeadPosition pos)
{
	//调整K210舵机至循迹角度
	OpenMVOpt::Servo_Control(-60);
	MapPoint last_point = points[0];
	for (int i = 1; i < point_cnt; i++)
	{
		if (i != 1)
		{
			//向前一定距离
			AccurateMotor.RunForward(3);
			AccurateMotor.DelayUntilCarStop();
		}
		if (last_point.NeedTurn(points[i], pos))
		{
			//TurnFlag = true;
			switch (last_point.CalcTurnDirection(points[i], pos))
			{
			case TurnDirection::Left:
				//左转
				AccurateMotor.TurnLeft(90);
				AccurateMotor.DelayUntilCarStop();
				ChangePositon(pos, TurnDirection::Left);
				break;
			case TurnDirection::Right:
				//右转
				AccurateMotor.TurnRight(90);
				AccurateMotor.DelayUntilCarStop();
				ChangePositon(pos, TurnDirection::Right);
				break;
			default:
				break;
			}
			//向前循迹
			OpenMVOpt::OpenMV_Track(8, TrackCallback);
		}
		else
		{
			//向前循迹
			OpenMVOpt::OpenMV_Track(8, TrackCallback);
		}
		last_point = points[i];
	}
	delete[] points;
	//恢复K210舵机角度
	delay(1000);
	OpenMVOpt::OpenMVTrack_Disc_CloseUp();
	delay(500);
	OpenMVOpt::Servo_Control(0);
}

bool AutoPathRunner::IsSpecTerrainItem()
{
	double dis = Ultrasonic.Ranging(CM);
	AccurateMotor.TurnLeft(90);
	AccurateMotor.DelayUntilCarStop();
	AccurateMotor.RunForward(5);
	AccurateMotor.DelayUntilCarStop();
	AccurateMotor.TurnRight(90);
	AccurateMotor.DelayUntilCarStop();
	delay(200);
	double dis_left = Ultrasonic.Ranging(CM);
	AccurateMotor.TurnRight(90);
	AccurateMotor.DelayUntilCarStop();
	AccurateMotor.RunForward(10);
	AccurateMotor.DelayUntilCarStop();
	AccurateMotor.TurnLeft(90);
	AccurateMotor.DelayUntilCarStop();
	delay(200);
	double dis_right = Ultrasonic.Ranging(CM);
	AccurateMotor.TurnRight(90);
	AccurateMotor.DelayUntilCarStop();
	AccurateMotor.RunBackward(5);
	AccurateMotor.DelayUntilCarStop();
	AccurateMotor.TurnLeft(90);
	AccurateMotor.DelayUntilCarStop();
	delay(200);
	return (((dis - dis_left) >= 10) || ((dis - dis_right) >= 10));
}

void AutoPathRunner::ReversingIntoGarage(bool a_b_select, uint8_t target_floor)
{
	GarageCommand cmd;
	uint8_t recv[8] = { 0 };
	AccurateMotor.RunForwardSlow(1);
	AccurateMotor.DelayUntilCarStop();
	if (a_b_select)
		cmd.UsingA();
	else
		cmd.UsingB();
	ZigBeeOperator.SendCommand(cmd.CMD_PositionReset());
	do
	{
		delay(500);
		ZigBeeOperator.SendCommand(cmd.CMD_QueryCurrentPos());
		if (ZigBeeOperator.ReciveCommand(recv, 8, 1000))
		{
			if (cmd.IsPositionData(recv))
			{
				if (cmd.CurrentPos(recv) == 0x01)
					break;
			}
		}
	} while (true);
	AccurateMotor.TurnLeft(90);
	AccurateMotor.DelayUntilCarStop();
	OpenMVOpt::OpenMV_TrackInPlace();
	do
	{
		delay(500);
		ZigBeeOperator.SendCommand(cmd.CMD_QueryIRStatus());
		if (ZigBeeOperator.ReciveCommand(recv, 8, 1000))
		{
			if (cmd.IsIRData(recv))
			{
				if (!cmd.BackIRTiggered(recv))
				{
					AccurateMotor.ForceStop();
					break;
				}
				else
				{
					AccurateMotor.RunBackwardSlow(1);
					AccurateMotor.DelayUntilCarStop();
				}
			}
		}
	} while (true);
	AccurateMotor.RunForwardSlow(1);
	AccurateMotor.DelayUntilCarStop();
	ZigBeeOperator.SendCommand(cmd.CMD_SetToFloor(target_floor));
}