#include <AccelStepper.h>
#include <Servo.h>
#include <math.h>

// Definizione dei LED
int led = 13;
int led2 = 12;
int led3 = 8;




// Define stepper motor connections and motor interface type. Motor interface type must be set to 1 when using a driver:
const int DIR_X = 2; // X axis
const int STEP_X = 3; // X axis
const int DIR_Y = 8; // Y axis
const int STEP_Y = 9; // Y axis

const int MICRO_STEP = 16;
const int MOTORE_STEP_PER_GIRO = 200;
const int CURRENT_STEP_PER_GIRO = MOTORE_STEP_PER_GIRO * MICRO_STEP;

// Crea un nuovo oggetto Servo per controllare il servo
Servo myservo;
int servoPos = 0;


// Definizione lunghezza bracci scara
int FIRST_ARM_LENGTH = 110;
int SECOND_ARM_LENGTH = 110;
int TOTAL_ARMS_LENGTH = FIRST_ARM_LENGTH + SECOND_ARM_LENGTH;

// parametri che rispecchiano il prg di gestione da cancellare quando
// le coordinate sarranno inviate già modificate
const float OFFSET_EFFECTOR_X = TOTAL_ARMS_LENGTH * 0.707 - 40;
const int OFFSET_EFFECTOR_Y = 20;

// Array di 10 stringhe, ognuna lunga fino a 19 caratteri + terminatore nullo
const int BUFFER_SIZE = 10;
String bufferGcode[BUFFER_SIZE];
int datiInseritiIndex = 0;
int datiPrelevatiIndex = 0;

// Create a new instance of the AccelStepper class:
AccelStepper stepper_X(AccelStepper::DRIVER, STEP_X, DIR_X);
AccelStepper stepper_Y(AccelStepper::DRIVER, STEP_Y, DIR_Y);

// Struttura per contenere gli angoli dei motori
struct MotorAngles {
  float shoulderMotorAngle;
  float elbowMotorAngle;
};

String letturaSeriale;
String comandoGenerale = "cmd";
String parseGcodeLine;
bool  isProcessingCommand = false;
float targetPosition;

bool receiveGcode = false;

void setup() {
  pinMode(led, OUTPUT);
  pinMode(led2, OUTPUT);
  pinMode(led3, OUTPUT);

  Serial.begin(115200);
  Serial.print("Pronti");

  // inizializza il buffer
  for (int i = 0; i < BUFFER_SIZE; i++) {
    bufferGcode[i] = "";
  }

  myservo.attach(10);
  myservo.write(0);

  stepper_X.setMaxSpeed(7000);
  stepper_X.setAcceleration(3000);
  stepper_Y.setMaxSpeed(7000);
  stepper_Y.setAcceleration(3000);
}

void loop() {
  if (Serial.available() > 0) {
    //Leggiamo con uno dei comandi di lettura
    letturaSeriale = Serial.readStringUntil('\n');
    if (letturaSeriale.startsWith(comandoGenerale)) {
      // è un comando generico
      Serial.print("comando generico: ");
      Serial.println(letturaSeriale);
    } else {
      Serial.print("comando G-code: ");
      Serial.println(letturaSeriale);
      bufferGcode[datiInseritiIndex] = letturaSeriale;
      if (datiInseritiIndex == 9) {
        datiInseritiIndex = 0;
      } else {
        datiInseritiIndex++;
      }
      Serial.println();
      Serial.print("Puntatore dati inseriti: ");
      Serial.println(datiInseritiIndex);
      Serial.print("Puntatore dati prelevati: ");
      Serial.println(datiPrelevatiIndex);
      Serial.println();

    }

  }
  processCommands(parseGcodeLine);
}


// Funzione che processa i comandi g-code
void processCommands(String cmd) {

  // Primo step: controlla se deve iniziare il parsing di un nuovo comando gcode
  // prende il gcode e lo imposta come targetPosition
  if (!isProcessingCommand && datiPrelevatiIndex != datiInseritiIndex) {
    // Inizia un nuovo comando
    isProcessingCommand = true;
    // preleva nuova comando g-code dal buffer es. "G1 X100"
    parseGcodeLine = bufferGcode[datiPrelevatiIndex];
    if (parseGcodeLine.startsWith("G1")) {
      G1CommandProcessing(parseGcodeLine);
    }
    if (parseGcodeLine.startsWith("G28")) {
      G28CommandProcessing(parseGcodeLine);
    }
  }

  // secondo step: verifica se deve completare l'esecuzione di un comando gcode
  if (isProcessingCommand) {
    // Esegui un passo del motore
    stepper_X.run();
    stepper_Y.run();

    // Verifica se il motore ha raggiunto la posizione target
    if (stepper_X.distanceToGo() == 0 && stepper_Y.distanceToGo() == 0) {
      // Completamento dell'elaborazione del comando
      isProcessingCommand = false;
      datiPrelevatiIndex = (datiPrelevatiIndex + 1) % BUFFER_SIZE;
      Serial.println();
      Serial.print("Finished processing: ");
      Serial.println(bufferGcode[datiPrelevatiIndex]);
      Serial.print("Puntatore dati inseriti: ");
      Serial.println(datiInseritiIndex);
      Serial.print("Puntatore dati prelevati: ");
      Serial.println(datiPrelevatiIndex);
      Serial.println();

    }

  }

}

// Funzione per calcolare gli angoli dati x, y
MotorAngles XYToAngle(
  float x,
  float y,
  int FIRST_ARM_LENGTH,
  int SECOND_ARM_LENGTH
) {
  MotorAngles angles;
  float hypotenuse = sqrt(x * x + y * y);
  if (hypotenuse > FIRST_ARM_LENGTH + SECOND_ARM_LENGTH) {
    Serial.println("Cannot reach; total arm length is less than hypotenuse.");
    angles.shoulderMotorAngle = 0;
    angles.elbowMotorAngle = 0;
    return angles;  // Ritorna angoli non validi se non raggiungibile
  }

  float hypotenuseAngle = asin(x / hypotenuse);
  float innerAngle = acos((hypotenuse * hypotenuse + FIRST_ARM_LENGTH * FIRST_ARM_LENGTH - SECOND_ARM_LENGTH * SECOND_ARM_LENGTH) / (2 * hypotenuse * FIRST_ARM_LENGTH));
  float outerAngle = acos((FIRST_ARM_LENGTH * FIRST_ARM_LENGTH + SECOND_ARM_LENGTH * SECOND_ARM_LENGTH - hypotenuse * hypotenuse) / (2 * FIRST_ARM_LENGTH * SECOND_ARM_LENGTH));

  angles.shoulderMotorAngle = radiansToDegrees(hypotenuseAngle - innerAngle);
  angles.elbowMotorAngle = radiansToDegrees(M_PI - outerAngle);

  Serial.print("XYToAngle X: ");
  Serial.println(x);
  Serial.print("XYToAngle Y: ");
  Serial.println(x);
  Serial.print("angles.shoulderMotorAngle: ");
  Serial.println(angles.shoulderMotorAngle, 6);
  Serial.print("angles.elbowMotorAngle: ");
  Serial.println(angles.elbowMotorAngle, 6);

  return angles;
}

// Funzione per convertire radianti in gradi
float radiansToDegrees(float radians) {
  return radians * (180.0 / M_PI);
}

// Funzione per convertire gli angoli in gradi in step
float angleToStep(float angle) {
  return (CURRENT_STEP_PER_GIRO / 360.0) * angle;
}


// Funzione per impostare la velocità dei motori
void setMaxCurrentSpeed(int motorSpeedF) {
  stepper_X.setMaxSpeed(motorSpeedF);
  stepper_Y.setMaxSpeed(motorSpeedF);
}

// Funzione elaborazione comando G1
void G1CommandProcessing(String command) {
  int xIndex = command.indexOf('X');
  int yIndex = command.indexOf('Y');
  int eIndex = command.indexOf('E');
  int fIndex = command.indexOf('F');

  const float targetPositionX = command.substring(xIndex + 1).toFloat(); // Prende la substring dal valore di X fino alla fine, float prende solo il primo valore e scarta il restante
  const float targetPositionY = command.substring(yIndex + 1).toFloat(); // Prende la substring dal valore di Y fino alla fine, float prende solo il primo valore e scarta il restante
  const float targetPositionE = command.substring(eIndex + 1).toFloat(); // Prende la substring dal valore di E fino alla fine, float prende solo il primo valore e scarta il restante
  const int motorSpeedF = command.substring(fIndex + 1).toInt(); // Prende la substring dal valore di F fino alla fine, int prende solo il primo valore e scarta il restante
  /*      Serial.print("targetPositionX ");
       Serial.println(targetPositionX);
       Serial.print("targetPositionY ");
       Serial.println(targetPositionY);
       Serial.print("targetPositionE ");
       Serial.println(eIndex);
       Serial.println(targetPositionE); */

  if (fIndex && motorSpeedF > 0) {
    setMaxCurrentSpeed( motorSpeedF);
  }

  if (eIndex && targetPositionE > 0) {
    for (servoPos = 0; servoPos <= 180; servoPos += 1) {
      // in steps of 1 degree
      myservo.write(servoPos);
    }
  } else {
    for (servoPos = 180; servoPos >= 0; servoPos -= 1) {
      myservo.write(servoPos);
    }
  }

  MotorAngles angles = XYToAngle(targetPositionX - OFFSET_EFFECTOR_X, targetPositionY + OFFSET_EFFECTOR_Y, 110, 110);
  float primo_angolo = angles.shoulderMotorAngle;
  stepper_X.moveTo(angleToStep(primo_angolo)); // Imposta la posizione target
  Serial.print("targetPosition parsed X ");
  Serial.println(primo_angolo);
  stepper_Y.moveTo(angles.elbowMotorAngle); // Imposta la posizione target
  Serial.print("targetPosition parsed Y ");
  Serial.println(angleToStep(angles.elbowMotorAngle));
}

void G28CommandProcessing(String command) {
  // HOMMING
  Serial.print("HOMMING G28");
  Serial.println();
  // Imposta velocità e accellerazioni relativamente basse
  stepper_X.setMaxSpeed(300);
  stepper_X.setAcceleration(500);
  stepper_Y.setMaxSpeed(300);
  stepper_Y.setAcceleration(500);

  // Sposta in avanti il braccio di poco in modalità relativa alla posizione corrente
  stepper_X.move(10);
  stepper_Y.move(10);
  while (stepper_X.distanceToGo() != 0 || stepper_Y.distanceToGo() != 0) {
    stepper_X.run();
    stepper_Y.run();
  }

  // Muove il braccio verso la home di una grande distanza in modo da raggiungerlo
  stepper_X.move(-10000);
  stepper_Y.move(-10000);
}