#include "AccelStepper.h"
#include "PololuMaestro.h"
#include "Configuration.h"
#include <SoftwareSerial.h>

SoftwareSerial maestroSerial(10, 11);
AccelStepper stepper(X_INTERFACE_TYPE, X_STEP_PIN, X_DIR_PIN); // Define a stepper and the pins it will use
MicroMaestro maestro(maestroSerial); // Define a servo controller

int cafe = -6687;
int limao = -4000;
int groselha = -5033;
int aguaCoco = -4197;
int laranja = -3000;
int aguaGas = -2528;
int ananas = -2000;
int cola = -858;
int sevenUp = -30;

void setup() {
  Serial.begin(9600);
  Serial.println("A iniciar...");
  maestroSerial.begin(9600);
  Serial2.begin(9600);
  
  stepper.setMaxSpeed(X_MAX_SPEED); // Sets the maximum speed the X axis accelerate up to
  pinMode(X_ENDSTOP_PIN, INPUT_PULLUP); // Initialize endstop pin with the internal pull-up resistor enabled
  homeXAxis(); // Return the X axis to it's home position at the startup
}

void homeXAxis() {
  int endStopState = digitalRead(X_ENDSTOP_PIN);
  
  while (endStopState == HIGH) {
    stepper.moveTo(100);
    stepper.setSpeed(X_HOME_SPEED);
    stepper.runSpeed();
    endStopState = digitalRead(X_ENDSTOP_PIN);
  }

  stepper.moveTo(stepper.currentPosition() - 50);
  while (stepper.distanceToGo() != 0) {
    stepper.setSpeed(X_PARK_SPEED * -1);
    stepper.runSpeed();
  }

  endStopState = digitalRead(X_ENDSTOP_PIN);

  while (endStopState == HIGH) {
    stepper.moveTo(100);
    stepper.setSpeed(X_PARK_SPEED);
    stepper.runSpeed();
    endStopState = digitalRead(X_ENDSTOP_PIN);
  }
  stepper.setCurrentPosition(0);
}

void moveXTo(int pos) {
  Serial.print("X goes to: ");
  Serial.println(pos);
  
  if(pos < 0 && pos >= X_MAX_POS) {
    stepper.setAcceleration(X_ACCELERATION);
    stepper.moveTo(pos);
      if(pos < stepper.currentPosition()) {
        stepper.setSpeed(-100);
      } else {
        stepper.setSpeed(100);
      }
    while(stepper.distanceToGo() != 0) {
      stepper.run(); 
    }
  } else {
    Serial.println("Position should be between -4995 and 0");
  }
}

void pour(int times, int holdDuration, int waitDuration) {
  int count = 0;
  if(holdDuration > 0 && waitDuration > 0) {
    for(int i=0; i<times; i++) {
      maestro.setSpeed(0, SERVO_RAISE_SPEED);
      maestro.setTarget(0, SERVO_MAX_POS);
      delay(holdDuration);
      maestro.setSpeed(0, SERVO_RELEASE_SPEED);
      maestro.setTarget(0, SERVO_MIN_POS);
      if(times - 1 > count) {
        delay(waitDuration);
      } else {
        delay(DELAY_BETWEEN_INGREDIENTS);
      }
      count++;
    }
  } else {
    Serial.println("Hold and wait duration parameters cannot be lower than or equal to 0");
  }
}


void loop() {
  String comando = "";
  
  while(Serial2.available()){
    char valorLido =(char) Serial2.read();
    comando += valorLido;
    Serial.println("O valor recebido foi: " + comando);

    if(comando.equals("1")){
      cosmopolitan();
    }

    if(comando.equals("2")){
      cocktailTropical();
    }

    if(comando.equals("3")){
      cinderela();
    }

    if(comando.equals("4")){
      sanFrancisco();
    }

    if(comando.equals("5")){
      uglyKid();
    }

    if(comando.equals("6")){
      belaRagazza();
    }

    if(comando.equals("7")){
      tango();
    }

    if(comando.equals("8")){
      pneu();
    }
  }
}

void teste(){
    moveXTo(-20);
}

void cosmopolitan(){
  Serial.println("A sair um cosmopolitan...");
  int posicao [] = {limao,laranja,aguaCoco,groselha};
  int times [] = {1,1,1,1};
  int holdDuration [] = {3300,3300,2000,2000};
  int waitDuration [] = {2500,2500,2500,2500};
  Serial.println(sizeof(times));
  for(int i = 0; i < sizeof(posicao) / 2; i++){
    Serial.print("Posição: ");
    Serial.println(posicao[i]);
    Serial.print("Vezes: ");
    Serial.println(times[i]);
    Serial.print("Duração: ");
    Serial.println(holdDuration[i]);
    Serial.print("Espera: ");
    Serial.println(waitDuration[i]);
    moveXTo(posicao[i]);
    pour(times[i],holdDuration[i],waitDuration[i]);
  }
  homeXAxis();
}

/*void cosmopolitan(){
  int posicao [] = {sevenUp,cola,ananas,aguaGas,laranja,aguaCoco,groselha,limao,cafe};
  int times [] = {2,2,2,2,2,2,2,2,2};
  int holdDuration [] = {3300,3300,3300,3300,3300,3300,3300,3300,3300};
  int waitDuration [] = {600,600,600,600,600,600,600,600,600};
  Serial.println(sizeof(times));
  for(int i = 0; i < sizeof(posicao) / 2; i++){
    Serial.print("Posição: ");
    Serial.println(posicao[i]);
    Serial.print("Vezes: ");
    Serial.println(times[i]);
    Serial.print("Duração: ");
    Serial.println(holdDuration[i]);
    Serial.print("Espera: ");
    Serial.println(waitDuration[i]);
    moveXTo(posicao[i]);
    pour(times[i],holdDuration[i],waitDuration[i]);
  }
  homeXAxis();
}*/

void pneu(){
  Serial.println("A sair um pneu...");
  int posicao [] = {limao,aguaGas};
  int times [] = {2,3};
  int holdDuration [] = {3300,3300};
  int waitDuration [] ={2500,2500};

  Serial.println(sizeof(times));

  for(int i = 0; i < sizeof(posicao) / 2; i++){
    moveXTo(posicao[i]);
    pour(times[i],holdDuration[i],waitDuration[i]);
  }
  homeXAxis();
}

void tango(){
  Serial.println("A sair uma Seven Up + Groselha...");
  int posicao [] = {sevenUp,groselha};
  int times [] = {3,1};
  int holdDuration [] = {3300,3300};
  int waitDuration [] ={3800,3800};

  Serial.println(sizeof(times));

  for(int i = 0; i < sizeof(posicao) / 2; i++){
    moveXTo(posicao[i]);
    pour(times[i],holdDuration[i],waitDuration[i]);
  }
  homeXAxis();
}

void cinderela(){
  Serial.println("A sair uma Seven Up...");
  int posicao [] = {laranja,limao,ananas};
  int times [] = {2,2,2};
  int holdDuration [] = {3300,3300,3300};
  int waitDuration [] ={2500,2500,2500};

  Serial.println(sizeof(times));

  for(int i = 0; i < sizeof(posicao) / 2; i++){
    moveXTo(posicao[i]);
    pour(times[i],holdDuration[i],waitDuration[i]);
  }
  homeXAxis();
}

void sanFrancisco(){
  Serial.println("A sair um Sumo de Laranja...");
  int posicao [] = {limao,laranja,ananas,groselha};
  int times [] = {1,1,1,1};
  int holdDuration [] = {3300,3300,3300,2150};
  int waitDuration [] ={600,1000,1000,600};

  Serial.println(sizeof(times));

  for(int i = 0; i < sizeof(posicao) / 2; i++){
    moveXTo(posicao[i]);
    pour(times[i],holdDuration[i],waitDuration[i]);
  }
  homeXAxis();
}

void uglyKid(){
  Serial.println("A sair um Sumo de Laranja...");
  int posicao [] = {groselha,laranja,limao};
  int times [] = {1,2,2};
  int holdDuration [] = {3000,3300,3300};
  int waitDuration [] ={600,2500,2500};

  Serial.println(sizeof(times));

  for(int i = 0; i < sizeof(posicao) / 2; i++){
    moveXTo(posicao[i]);
    pour(times[i],holdDuration[i],waitDuration[i]);
  }
  homeXAxis();
}

void belaRagazza(){
  Serial.println("A sair uma Coca Cola...");
  int posicao [] = {limao,cafe,cola};
  int times [] = {1,1,3};
  int holdDuration [] = {2150,3300,3300};
  int waitDuration [] ={600,2500,3800};

  Serial.println(sizeof(times));

  for(int i = 0; i < sizeof(posicao) / 2; i++){
    moveXTo(posicao[i]);
    pour(times[i],holdDuration[i],waitDuration[i]);
  }
  homeXAxis();
}

void cocktailTropical(){
  Serial.println("A sair um Cocktail Tropical...");
  int posicao [] = {limao,laranja,aguaGas};
  int times [] = {1,1,2};
  int holdDuration [] = {3300,3300,3300};
  int waitDuration [] ={2500,2500,3800};

  Serial.println(sizeof(times));

  for(int i = 0; i < sizeof(posicao) / 2; i++){
    moveXTo(posicao[i]);
    pour(times[i],holdDuration[i],waitDuration[i]);
  }
  homeXAxis();
}