// Copyright 2022, Algoryx Simulation AB.

#include "RobotSequence.h"

namespace RobotSequence_helpers
{
	constexpr double GripperClosePos = 0.0;
	constexpr double GripperOpenPos = 1.7;
	static TArray<double> GripperOpen = {GripperOpenPos, GripperOpenPos};
	static TArray<double> GripperClose = {GripperClosePos, GripperClosePos};

	TArray<FRobotTarget> GetLeftArmTargets()
	{
		static TArray<double> InitPos {-42.61, -129.03, 14.12, -3.17, 42.49, -130.50, 82.08};
		static TArray<double> DispenserHighApproaching {-70.94, -65.08, -22.19, 107.09, 94.5, -171.63, 29.62};
		static TArray<double> DispenserHigh {-73.14, -59.58, -39.0, 107.08, 92.17, -160.76, 30.14};
		static TArray<double> DispenserLow {-76.94, -59.04, -45.29, 105.44, 85.88, -158.40, 37.14};
		static TArray<double> DominoLow0 {-78.65, -77.69, -1.91, 118.82, 70.77, -93.06, 61.69};
		static TArray<double> DominoHigh0 {-71.46, -78.10, 12.11, 115.07, 82.61, -95.96, 50.81};
		static TArray<double> DominoLow1 {-72.99, -81.10, 5.16, 119.83, 69.39, -99.86, 62.83};
		static TArray<double> DominoHigh1 {-66.06, -82.24, 18.95, 115.46, 81.32, -101.99, 51.40};
		static TArray<double> DominoLow2 {-67.41, -84.28, 11.67, 121.12, 67.40, -108.30, 64.60};
		static TArray<double> DominoHigh2 {-60.51, -86.24, 25.45, 116.11, 79.40, -109.58, 52.88};
		static TArray<double> PushLow {-65.53, -93.02, -7.85, -55.27, 51.01, -104.17, 93.57};
		static TArray<double> PushHigh {-59.18, -94.75, -3.95, -55.51, 58.91, -100.73, 101.9};

		static TArray<FRobotTarget> Targets = {
			FRobotTarget {InitPos, GripperOpen, 2.0},
			FRobotTarget {InitPos, GripperClose, 0.4},
			FRobotTarget {InitPos, GripperClose, 1.0},

			// Domino 0.
			FRobotTarget {DispenserHighApproaching, GripperClose, 1.3},
			FRobotTarget {DispenserHigh, GripperClose, 0.5},
			FRobotTarget {DispenserHigh, GripperOpen, 0.4},
			FRobotTarget {DispenserLow, GripperOpen, 0.5},
			FRobotTarget {DispenserLow, GripperClose, 0.4},
			FRobotTarget {DispenserHigh, GripperClose, 0.5},
			FRobotTarget {DispenserHighApproaching, GripperClose, 0.5},
			FRobotTarget {DominoHigh0, GripperClose, 1.3},
			FRobotTarget {DominoLow0, GripperClose, 0.5},
			FRobotTarget {DominoLow0, GripperOpen, 0.4},
			FRobotTarget {DominoHigh0, GripperOpen, 0.5},

			// Domino 1.
			FRobotTarget {DispenserHighApproaching, GripperOpen, 1.3},
			FRobotTarget {DispenserHigh, GripperOpen, 0.5},
			FRobotTarget {DispenserLow, GripperOpen, 0.5},
			FRobotTarget {DispenserLow, GripperClose, 0.4},
			FRobotTarget {DispenserHigh, GripperClose, 0.5},
			FRobotTarget {DispenserHighApproaching, GripperClose, 0.5},
			FRobotTarget {DominoHigh1, GripperClose, 1.3},
			FRobotTarget {DominoLow1, GripperClose, 0.5},
			FRobotTarget {DominoLow1, GripperOpen, 0.4},
			FRobotTarget {DominoHigh1, GripperOpen, 0.5},

			// Domino 2.
			FRobotTarget {DispenserHighApproaching, GripperOpen, 1.3},
			FRobotTarget {DispenserHigh, GripperOpen, 0.5},
			FRobotTarget {DispenserLow, GripperOpen, 0.5},
			FRobotTarget {DispenserLow, GripperClose, 0.4},
			FRobotTarget {DispenserHigh, GripperClose, 0.5},
			FRobotTarget {DispenserHighApproaching, GripperClose, 0.5},
			FRobotTarget {DominoHigh2, GripperClose, 1.3},
			FRobotTarget {DominoLow2, GripperClose, 0.5},
			FRobotTarget {DominoLow2, GripperOpen, 0.4},
			FRobotTarget {DominoHigh2, GripperOpen, 0.5},

			// Push dominos.
			FRobotTarget {PushHigh, GripperOpen, 1.0},
			FRobotTarget {PushHigh, GripperClose, 0.4},
			FRobotTarget {PushHigh, GripperClose, 1.5},
			FRobotTarget {PushLow, GripperClose, 0.6}
		};

		return Targets;
	}

	TArray<FRobotTarget> GetRightArmTargets()
	{
		static TArray<double> InitPos {39.50, -121.25, 21.37, 22.94, 42.33, -107.18, -99.52};
		static TArray<double> DispenserHighApproaching {85.89, -72.7, -17.78, -130.91, 109.12, -13.98, -12.59};
		static TArray<double> DispenserHigh {85.29, -66.03, -34.17, -132.25, 103.87, -23.04, -11.25};
		static TArray<double> DispenserLow {86.74, -66.42, -39.60, -130.72, 96.80, -24.13, -16.54};
		static TArray<double> DominoLow0 {108.08, -85.56, -7.63, -143.48, 91.43, -110.5, -48.55};
		static TArray<double> DominoHigh0 {104.89, -83.35, 5.36, -139.26, 104.22, -111.88, -43.14};
		static TArray<double> DominoLow1 {102.19, -89.73, 0.38, -142.69, 92.44, -99.74, -47.79};
		static TArray<double> DominoHigh1 {99.47, -87.66, 12.76, -138.10, 105.16, -96.39, -41.76};
		static TArray<double> DominoLow2 {96.67, -94.04, 7.98, -142.16, 92.97, -88.62, -47.87};
		static TArray<double> DominoHigh2 {94.35, -92.19, 20.04, -137.11, 105.74, -85.49, -41.17};

		static TArray<FRobotTarget> Targets = {
			FRobotTarget {InitPos, GripperOpen, 2.0},
			FRobotTarget {InitPos, GripperClose, 0.4},
			FRobotTarget {InitPos, GripperClose, 1.0},

			// Domino 0.
			FRobotTarget {DispenserHigh, GripperClose, 1.1},
			FRobotTarget {DispenserHigh, GripperOpen, 0.4},
			FRobotTarget {DispenserLow, GripperOpen, 0.5},
			FRobotTarget {DispenserLow, GripperClose, 0.4},
			FRobotTarget {DispenserHigh, GripperClose, 0.5},
			FRobotTarget {DispenserHighApproaching, GripperClose, 0.5},
			FRobotTarget {DominoHigh0, GripperClose, 1.1},
			FRobotTarget {DominoLow0, GripperClose, 0.5},
			FRobotTarget {DominoLow0, GripperOpen, 0.4},
			FRobotTarget {DominoHigh0, GripperOpen, 0.5},

			// Domino 1.
			FRobotTarget {DispenserHighApproaching, GripperOpen, 1.1},
			FRobotTarget {DispenserHigh, GripperOpen, 0.5},
			FRobotTarget {DispenserLow, GripperOpen, 0.5},
			FRobotTarget {DispenserLow, GripperClose, 0.4},
			FRobotTarget {DispenserHigh, GripperClose, 0.5},
			FRobotTarget {DispenserHighApproaching, GripperClose, 0.5},
			FRobotTarget {DominoHigh1, GripperClose, 1.1},
			FRobotTarget {DominoLow1, GripperClose, 0.5},
			FRobotTarget {DominoLow1, GripperOpen, 0.4},
			FRobotTarget {DominoHigh1, GripperOpen, 0.5},

			// Domino 2.
			FRobotTarget {DispenserHighApproaching, GripperOpen, 1.1},
			FRobotTarget {DispenserHigh, GripperOpen, 0.5},
			FRobotTarget {DispenserLow, GripperOpen, 0.5},
			FRobotTarget {DispenserLow, GripperClose, 0.4},
			FRobotTarget {DispenserHigh, GripperClose, 0.5},
			FRobotTarget {DispenserHighApproaching, GripperClose, 0.5},
			FRobotTarget {DominoHigh2, GripperClose, 1.1},
			FRobotTarget {DominoLow2, GripperClose, 0.5},
			FRobotTarget {DominoLow2, GripperOpen, 0.4},
			FRobotTarget {DominoHigh2, GripperOpen, 0.5},

			FRobotTarget {InitPos, GripperClose, 1.5}
		};

		return Targets;
	}
}

TArray<FRobotTarget>& FRobotSequence::GetTargets()
{
	return Targets;
}

void FRobotSequence::SetType(TEnumAsByte<enum ESequenceType> InSequenceType)
{
	SequenceType = InSequenceType;
	if (SequenceType == ESequenceType::DominoLeft)
	{
		Targets = RobotSequence_helpers::GetLeftArmTargets();
	}
	else
	{
		Targets = RobotSequence_helpers::GetRightArmTargets();
	}
}
