#include <Arduino.h>
//#define USE_PCA9685_SERVO_EXPANDER
#include "ServoEasing.hpp"
#include "InverseK.h"

// Define servo objects
#if defined(USE_PCA9685_SERVO_EXPANDER)
	ServoEasing Servo1(PCA9685_DEFAULT_ADDRESS);
	ServoEasing Servo2(PCA9685_DEFAULT_ADDRESS);
	ServoEasing Servo3(PCA9685_DEFAULT_ADDRESS);
	ServoEasing Servo4(PCA9685_DEFAULT_ADDRESS);
	ServoEasing Servo5(PCA9685_DEFAULT_ADDRESS);
	#else
	ServoEasing Servo1;
	ServoEasing Servo2;
	ServoEasing Servo3;
	ServoEasing Servo4;
	ServoEasing Servo5;
#endif

// Define PCA9685 servo ports or Arduino pins
const int servo1Pin = 3;
const int servo2Pin = 11;
const int servo3Pin = 10;
const int servo4Pin = 9;
const int servo5Pin = 6;
const int tSpeed = 40; // Set Servo movement speed. ie: 30° per second

int maxPos1 = 180;
int maxPos2 = 360;
int maxPos3 = 360;
int maxPos4 = 180;

#define True 1
#define False 0
unsigned long currentMillis;
unsigned long previousMillis;
bool detection = False;
int stepFlag = 0;

//initial angle position of the arm : 90,120,180 - 0, 180 - 15
#define START_DEGREE_VALUE_1 90
#define START_DEGREE_VALUE_2 120
#define START_DEGREE_VALUE_3 180-0
#define START_DEGREE_VALUE_4 180-15
#define START_DEGREE_GRIPPER_OPEN 0     // 90° gripper close / 0° gripper open
#define START_DEGREE_GRIPPER_CLOSE 40
#define GRIPPER_DELAY 100
#define BACK_DELAY 10000
float angles[5] = {START_DEGREE_VALUE_1, START_DEGREE_VALUE_2, START_DEGREE_VALUE_3, START_DEGREE_VALUE_4, START_DEGREE_GRIPPER_OPEN};
float IKangles[5] = {START_DEGREE_VALUE_1, START_DEGREE_VALUE_2, START_DEGREE_VALUE_3, START_DEGREE_VALUE_4, START_DEGREE_GRIPPER_OPEN};
int grip_angle = START_DEGREE_GRIPPER_OPEN;
uint32_t startTimestamp = 0;

// Define lengths of the arm segments in millimeters
//const float L1 = 105.0; // Length of the first link
//const float L2 = 128.0; // Length of the second link
//const float L3 = 131.0; // Length of the third link

// Define a buffer to store received serial data
char receivedData[32];
char lastCommand = '\0';

// Store the last received coordinates
int lastX = -1, lastY = -1, lastZ = -1;

void setup() {
	// Initialize serial communication
	Serial.begin(115200);
	
	// Print initialization message
	Serial.println("Begin Serial Motor. Send coordinates: x,y,z");
	
	#if defined(USE_PCA9685_SERVO_EXPANDER)
		if (Servo1.InitializeAndCheckI2CConnection(&Serial)) {
			while (true) {
				blinkLED();
			}
		}
		if (Servo2.InitializeAndCheckI2CConnection(&Serial)) {
			while (true) {
				blinkLED();
			}
		}
		if (Servo3.InitializeAndCheckI2CConnection(&Serial)) {
			while (true) {
				blinkLED();
			}
		}
		if (Servo4.InitializeAndCheckI2CConnection(&Serial)) {
			while (true) {
				blinkLED();
			}
		}
	#endif
	Servo1.attach(servo1Pin, START_DEGREE_VALUE_1);
	Servo2.attach(servo2Pin, START_DEGREE_VALUE_2);
	Servo3.attach(servo3Pin, START_DEGREE_VALUE_3);
	Servo4.attach(servo4Pin, START_DEGREE_VALUE_4);
	Servo5.attach(servo5Pin, START_DEGREE_GRIPPER_OPEN);
	
	Link base, upperarm, forearm, hand;
	
	base.init(150.0, b2a(0.0), b2a(180.0));
	upperarm.init(105.0, b2a(0.0), b2a(180.0));
	forearm.init(128.0, b2a(0.0), b2a(180.0));
	hand.init(180.0, b2a(0.0), b2a(180.0));
	
	InverseK.attach(base, upperarm, forearm, hand);
	
}

void loop() {
	
	/////////////////////////////////////////////////////////
	/////////////////////////////////////////////////////////
	
	ServoEasing::ServoEasingNextPositionArray[0] = angles[0];
	ServoEasing::ServoEasingNextPositionArray[1] = angles[1];
	ServoEasing::ServoEasingNextPositionArray[2] = 180 - angles[2];
	ServoEasing::ServoEasingNextPositionArray[3] = angles[3];
	ServoEasing::ServoEasingNextPositionArray[4] = angles[4];
	setEaseToForAllServosSynchronizeAndStartInterrupt(tSpeed);
	
	while (ServoEasing::areInterruptsActive()) {
		currentMillis = millis();
		// Check if serial data is available
		if (Serial.available() > 0) {
			// Read the data into the receivedData buffer until a newline character is received
			Serial.readBytesUntil('\n', receivedData, sizeof(receivedData));
			
			// Variables to store the parsed coordinates
			int x, y, z, g;
			
			// Parse the received data
			if (sscanf(receivedData, "%d,%d,%d,%d", &x, &y, &z, &g) == 4) {
				// Check if the new coordinates are the same as the last received coordinates
				memset(receivedData, 0, sizeof(receivedData)); // Clear the array after parsing receivedData
				
					if (lastX != x || lastY != y || lastZ != z) {
					// Update the last received coordinates
					lastX = x;
					lastY = y;
					lastZ = z;

					// Perform inverse kinematics calculation
					calculateIK(x, y, z, IKangles);
					Serial.println("New coordinates found");
				} else {
					detection = False;
					Serial.println("Ignoring repeated coordinates.");
				}
				
				// Copy gripper angle to another variable
				if (grip_angle != g){ // Check for grip_angle that is not the same as received command
					grip_angle = g; // Update grip angle to same as received command
				}
			}
		}
		// Insert servo sequence here
		if (detection == True && stepFlag == 0){
			stepFlag = 1;
		}
		if (stepFlag == 1 && currentMillis - previousMillis >= 8000){
			angles[0] = IKangles[0];
			angles[1] = IKangles[1];
			angles[2] = IKangles[2];
			angles[3] = IKangles[3];

			Serial.println("1. Moving to obtained object position");
			printAngles();
			stepFlag = 2;
			previousMillis = currentMillis;
		}
		
		if (stepFlag == 2 && currentMillis - previousMillis > 8000){
			angles[4] = grip_angle;
			
			Serial.println("2. Gripping objects");
			printAngles();
			stepFlag = 3;
			previousMillis = currentMillis;                
		}
		
		if (stepFlag == 3 && currentMillis - previousMillis > 8000){
			angles[0] = START_DEGREE_VALUE_1;
			angles[1] = START_DEGREE_VALUE_2;
			angles[2] = START_DEGREE_VALUE_3;
			angles[3] = START_DEGREE_VALUE_4;
			
			Serial.println("3. Back to home pos");
			printAngles();
			stepFlag = 4;
			previousMillis = currentMillis;
		}
		/////..
		if (stepFlag == 4 && currentMillis - previousMillis > 8000){
			angles[0] = 0;
			angles[1] = 110;
			angles[2] = 180-0;
			angles[3] = 180-0;
			
			Serial.println("4. Placing objects");
			printAngles();
			stepFlag = 5;
			previousMillis = currentMillis;
		}
		
		if (stepFlag == 5 && currentMillis - previousMillis > 8000){
			angles[4] = START_DEGREE_GRIPPER_OPEN;
			
			Serial.println("5. Open gripper");
			printAngles();
			stepFlag = 6;
			previousMillis = currentMillis;
		}
		
		if (stepFlag == 6 && currentMillis - previousMillis > 8000){
			angles[0] = START_DEGREE_VALUE_1;
			angles[1] = START_DEGREE_VALUE_2;
			angles[2] = START_DEGREE_VALUE_3;
			angles[3] = START_DEGREE_VALUE_4;
			
			Serial.println("6. Back to home pos");
			printAngles();
			stepFlag = 0;
			detection = False;
			previousMillis = currentMillis;
		}
	}
}

// Function to perform inverse kinematics calculation
void calculateIK(int &x, int &y, int &z, float vector []) {
	float a0, a1, a2, a3;
	if (InverseK.solve(x, y, z, a0, a1, a2, a3)) {
		detection = True;
		Serial.println("IK solution found!");
		} else {
		Serial.println("No solution found!");
		return;
	}
	
	// Convert angles to degrees
	IKangles[0] = a2b(a0);
	IKangles[1] = a2b(a1);
	IKangles[2] = a2b(a2);
	IKangles[3] = a2b(a3);
}

void printAngles()
{
	// Print servo angles in degrees and radians
	Serial.println("Servo Angles:");
	for (int i = 0; i < 5; i++)
	{
		Serial.print("Angle ");
		Serial.print(i + 1);
		Serial.print(" (degrees): ");
		Serial.print(angles[i]);
		Serial.print("\t");
		Serial.print("Angle ");
		Serial.print(i + 1);
		Serial.print(" (radians): ");
		Serial.println(degreesToRadians(angles[i]));
	}
}

void blinkLED() {
	digitalWrite(LED_BUILTIN, HIGH);
	delay(100);
	digitalWrite(LED_BUILTIN, LOW);
	delay(100);
}

// Function to convert degrees to radians
float degreesToRadians(float degrees) {
	return degrees * PI / 180.0;
}

// Quick conversion from the Braccio angle system to radians
float b2a(float b) {
	return b / 180.0 * PI - HALF_PI;
}

// Quick conversion from radians to the Braccio angle system
float a2b(float a) {
	return (a + HALF_PI) * 180 / PI;
}