#include <AccelStepper.h>                 // https://www.airspayce.com/mikem/arduino/AccelStepper/index.html
#include <Servo.h>                        // https://www.arduino.cc/reference/en/libraries/servo/
#include <Adafruit_PWMServoDriver.h>      //https://learn.adafruit.com/16-channel-pwm-servo-driver/overview
                                          // https://adafruit.github.io/Adafruit-PWM-Servo-Driver-Library/html/index.html      
#include <Wire.h>

// Input Pins to define Home switches
#define Carousel_A_Home 3		 // J3_3.
#define Carousel_B_Home 14		 // J5_3.
#define Carousel_C_Home 18       // J7_3.
#define X_Home 19     		     // J8_3.
#define Y_Home 2                 // J4-3.

#define En_Carousel_A 38
#define En_Carousel_B A2
#define En_Carousel_C A8
#define En_Y 24
#define En_X 30

int Homebutton = 12;

int ServoPinA = 4;
int ServoPinB = 5;
int ServoPinC = 43;

int SolValve;

float OzToMlConver = 29.5735;                        // One US oz is 29.5735ml
                                  
int ElementCount = 2;

//String DataString = "a2,1.5,b5,0.75,c4,1";
String DataString = "";
int RotationPos[8] = {0,0,33,66,100,133,167};       // map the carousel rotation positions

String State = "Idle";

int i, c, k, j=0, count;
long pos1, old;
String val1;
String amt1;
String CurrentLoc;

unsigned long PreviousMillis = 0;                         // For non-blocking timing

// Adjust the following integers to move the truck under each carousel and mixer positions.
int a_pos = 100;
long b_pos = 200;
long c_pos = 300;
long m1_pos = 50;
long m8_pos = 250;
long DeliveryPosX = 0;
long DeliveryPosY = 250;

float PourVolume = 0;
int PourTime;
float SolPourVolume = 0;
int SolPourTime;
int SolCalFactor = 2000;                      // Solenoid time calibration factor in ms

bool CarouselADone = false;
bool X_AxisDone = false;

String Elements[14]="";

bool ParseFlag = true;
bool Continueflag = true;
bool DeliverDrinkFlag = false;
bool StartDrinkPour = false;
bool CupPresent = false;
bool IsRunning = false;
bool MotorsHome = false;
bool DrinkComplete = false;

// Instantiate Stepper motors
AccelStepper MotCarousel_A(1, A0, A1);  
AccelStepper MotCarousel_B(1, A6, A7);
AccelStepper MotCarousel_C(1, 46, 48);
AccelStepper MotX_Axis(1, 36, 34);
AccelStepper MotY_Axis(1, 26, 28);   
   
// Instantiate solenoid drivers
Adafruit_PWMServoDriver SolDriver_0_7 = Adafruit_PWMServoDriver(0x40);
Adafruit_PWMServoDriver SolDriver_8_14 = Adafruit_PWMServoDriver(0x41);

// Instantiate Servos
Servo PourServoA;
Servo PourServoB;
Servo PourServoC;

long TravelX;  // Used to store the X value entered in the Serial Monitor
int move_finished=1;  // Used to check if move is completed


bool A_Homed = false;
bool B_Homed = false;
bool C_Homed = false;
bool X_Homed = false;
bool Y_Homed = false;

void setup() {
  Serial.begin(9600); 
  pinMode(Y_Home, INPUT_PULLUP);
  pinMode(Homebutton, INPUT_PULLUP);

  SolDriver_0_7.begin();
  SolDriver_0_7.setPWMFreq(1600);  // This is the maximum PWM frequency
  SolDriver_8_14.begin();
  SolDriver_8_14.setPWMFreq(1600);  // This is the maximum PWM frequency

  MotCarousel_A.setMaxSpeed(100);
	MotCarousel_A.setAcceleration(100);
  MotCarousel_B.setMaxSpeed(100);
	MotCarousel_B.setAcceleration(100);
  MotCarousel_C.setMaxSpeed(100);
	MotCarousel_C.setAcceleration(100);
  MotX_Axis.setMaxSpeed(100);
	MotX_Axis.setAcceleration(100);   
  MotY_Axis.setMaxSpeed(100);
	MotY_Axis.setAcceleration(50); 

  // Disable the motor coils
  StepperCoils("Disabled");

  
  PourServoA.attach(ServoPinA);                      // Servo will move to a standard pos on startup (90). 
  //PourServoA.write(0);                               // Make sure to write the startup positions
  PourServoB.attach(ServoPinB);
  //PourServoB.write(0);
  PourServoC.attach(ServoPinC);
  //PourServoC.write(0);


  
  /*MotCarousel_A.moveTo(150);
  MotCarousel_B.moveTo(300);
  MotCarousel_C.moveTo(100);
  MotX_Axis.moveTo(150);
  MotY_Axis.moveTo(250);
*/
Serial.println();
}
bool HomingDone = false;

void HomeMotors(){
  long InitialHomingA = -1;
  long InitialHomingB = -1;
  long InitialHomingC = -1;
  long InitialHomingX = -1;
  long InitialHomingY = -1;

  PourServoA.write(90);
  PourServoB.write(90);
  PourServoC.write(90);

  while(digitalRead(Y_Home) == HIGH){   // If the switch isn't made AND the flag is false
			MotY_Axis.moveTo(InitialHomingY);                     // Continue to move to home direction
			InitialHomingY--;
			MotY_Axis.run();
  }
  while(digitalRead(Y_Home) == LOW){
    MotY_Axis.stop();

    InitialHomingY++;
      //MotY_Axis.setCurrentPosition(0);
      InitialHomingY++;
      MotY_Axis.run();
  }
//		if (digitalRead(Y_Home) == LOW){
//      InitialHomingY=1;
//      InitialHomingY++;
//      MotY_Axis.setCurrentPosition(0);
//      MotY_Axis.run();
      /*
      while(digitalRead(Y_Home) == LOW){                   // change direction until the switch is released
        //Serial.println(digitalRead(Y_Home));
        InitialHomingY++; 
        MotY_Axis.moveTo(InitialHomingY);
        InitialHomingY++;    
        MotY_Axis.run();
        //Serial.println("here");
      }
      */
//    }
//    else if(digitalRead(Y_Home) == HIGH){
    
      //MotY_Axis.stop();
//      MotY_Axis.setCurrentPosition(0);
      //Y_Homed = true; 
      //Serial.println("now here"); 
//    }
    
                                       // Homing finished
// }
  
  while(!A_Homed && !B_Homed && !C_Homed && !X_Homed && !Y_Homed){
		if(digitalRead(Carousel_A_Home) == HIGH && A_Homed == false){
			MotCarousel_A.moveTo(InitialHomingA);
			InitialHomingA--;
			MotCarousel_A.run();	
		}
		else{
      while(digitalRead(Carousel_A_Home) == HIGH){                   // change direction until the switch is released
			  MotCarousel_A.moveTo(InitialHomingA);
        InitialHomingA++;                                   // 
        MotCarousel_A.run();
      }
      MotCarousel_A.setCurrentPosition(0);
			A_Homed = true;
		}
		if(digitalRead(Carousel_B_Home) == HIGH && B_Homed == false){
			MotCarousel_B.moveTo(InitialHomingB);
			InitialHomingB--;
			MotCarousel_B.run();
		}
		else{
      while(digitalRead(Carousel_B_Home) == HIGH){                   // change direction until the switch is released
			  MotCarousel_B.moveTo(InitialHomingB);
        InitialHomingB++;                                   // 
        MotCarousel_B.run();
      }
      MotCarousel_B.setCurrentPosition(0);
			B_Homed = true;
		}
		if(digitalRead(Carousel_C_Home) == HIGH && C_Homed == false){
			MotCarousel_C.moveTo(InitialHomingC);
			InitialHomingC--;
			MotCarousel_C.run();
		}
		else{
      while(digitalRead(Carousel_C_Home) == HIGH){                   // change direction until the switch is released
			  MotCarousel_C.moveTo(InitialHomingC);
        InitialHomingC++;                                   // 
        MotCarousel_C.run();
      }
      MotCarousel_C.setCurrentPosition(0);
			C_Homed = true;
		}
		if(digitalRead(X_Home) == HIGH && X_Homed == false){
			MotX_Axis.moveTo(InitialHomingX);
			InitialHomingX--;
			MotX_Axis.run();
		}
		else{
      while(digitalRead(Y_Home) == HIGH){                   // change direction until the switch is released
			  MotX_Axis.moveTo(InitialHomingX);
        InitialHomingX++;                                   // 
        MotX_Axis.run();
      }
      MotX_Axis.setCurrentPosition(0);
			X_Homed = true;
		}
		
	HomingDone = true;
	}
}

void ParseDataString(){                               // This counts the number of commas in the string.
  if (DataString !=""){
    IsRunning = true;
  Serial.print("Original string: ");
  Serial.println(DataString);
    for (i=0; i < DataString.length(); i++){               
      if (DataString.charAt(i) == ','){
        count = count + 1;
      }
    }
    for (c=0; c <= count; c++){                       // This separates each elements and puts them in an array.
      pos1 = DataString.indexOf(',' , pos1+1);               
      val1 = DataString.substring(old , pos1);     
      old = pos1+1;    
      Elements[c]=val1;
    }
      j=0;
  }
  
}

void ProcessData(){
if (ElementCount <= count +1){                          // Cycle through the number of elements and then deliver the drink
  if (Elements[j].charAt(0) == 'a'){
    CurrentLoc = Elements[j];
    MotX_Axis.moveTo(a_pos);
    MotCarousel_A.moveTo(RotationPos[String((Elements[j].charAt(1))).toInt()]);
      if (MotCarousel_A.distanceToGo() == 0 && MotX_Axis.distanceToGo() == 0){
        j=j+1;
        PourVolume = Elements[j].toFloat();
        Serial.println("HERE");
        delay(1000);
        PourServoA.write(0);
       Serial.print("Pouring volume of ");
        Serial.print(PourVolume);           //ToDo: calculate pour volume
        Serial.print(" from ");
        Serial.println(CurrentLoc);

        GetPourTime(PourVolume);
        Serial.println(PourTime);
        delay(PourTime);                        // delay to adjust pour volume.
        
        PourServoA.write(90);
        delay(1000);
        j=j+1;
        ElementCount=ElementCount +2;
      }
  }

  if (Elements[j].charAt(0) == 'b'){
    CurrentLoc = Elements[j];
    MotX_Axis.moveTo(b_pos);
    MotCarousel_B.moveTo(RotationPos[String((Elements[j].charAt(1))).toInt()]);
      if (MotCarousel_B.distanceToGo() == 0 && MotX_Axis.distanceToGo() == 0){
        j=j+1;
        PourVolume = Elements[j].toFloat();
        delay(1000);
        PourServoB.write(0);
        Serial.print("Pouring volume of ");
        Serial.print(PourVolume);
        Serial.print(" from ");
        Serial.println(CurrentLoc);

        GetPourTime(PourVolume);
        Serial.println(PourTime);
        delay(PourTime);                        // delay to adjust pour volume.

        PourServoB.write(90);
        delay(1000);                            // Delay to wait for pourer to stop dripping
        j=j+1;
        ElementCount=ElementCount +2;
      }
  }

  if (Elements[j].charAt(0) == 'c'){
    CurrentLoc = Elements[j];
    MotX_Axis.moveTo(c_pos);
    MotCarousel_C.moveTo(RotationPos[String((Elements[j].charAt(1))).toInt()]);
      if (MotCarousel_C.distanceToGo() == 0 && MotX_Axis.distanceToGo() == 0){
        j=j+1;
        PourVolume = Elements[j].toFloat();
        delay(1000);
        PourServoC.write(0);
        Serial.print("Pouring volume of ");
        Serial.print(PourVolume);
        Serial.print(" from ");
        Serial.println(CurrentLoc);

        GetPourTime(PourVolume);
        Serial.println(PourTime);
        delay(PourTime);                        // delay to adjust pour volume.

        PourServoC.write(90);
        delay(1000);                            // Delay to wait for pourer to stop dripping
        j=j+1;
        ElementCount=ElementCount +2;
      }
  }

  if (Elements[j].charAt(0) == 'm'){
    CurrentLoc = Elements[j];
      if(String((Elements[j].charAt(1))).toInt() < 8){
        MotX_Axis.moveTo(m1_pos);
        SolValve = String((Elements[j].charAt(1))).toInt();
        j=j+1;
        SolPourVolume = Elements[j].toFloat();
        SolPourTime = SolPourVolume * SolCalFactor;
        SolDriver_0_7.setPWM(SolValve, 4096, 0);                   // Open the solenoid valve 
        delay(SolPourTime);                              //for x seconds
        SolDriver_0_7.setPWM(SolValve, 0, 4096);                        // Close the solenoid valve
        j=j+1;
        ElementCount=ElementCount +2;
      }
      else if(String((Elements[j].charAt(1))).toInt() > 7) {
        MotX_Axis.moveTo(m8_pos);
        SolValve = String((Elements[j].charAt(1))).toInt();
        j=j+1;
        SolPourVolume = Elements[j].toFloat();
        SolPourTime = SolPourVolume * SolCalFactor;
        SolDriver_8_14.setPWM(SolValve, 4096, 0);                   // Open the solenoid valve 
        delay(SolPourTime);                              //for x seconds
        SolDriver_8_14.setPWM(SolValve, 0, 4096);                        // Close the solenoid valve
        j=j+1;
        ElementCount=ElementCount +2;
      } 
    
  }
  
}
else {                                // Reached the end of the string so deliver the drink
  ElementCount = 0;
  Serial.println("Delivering drink");
  State = "First";
  DeliverDrink();
}
}
 

void loop() {

    if(Serial.available() > 0){
    IsRunning = false;
    DataString = Serial.readString();
		DataString.trim();
    Serial.println(DataString);
    ParseDataString();
      //if (!digitalRead(CupPresent)){          // Make sure there's a cup in place before starting
      //  ParseDataString();                    // Go ahead and start the drink.
     // }
      
    } 

  //if(IsRunning == false){
  // ParseDataString();
  //}

  if(DataString.length() > 0){
    ProcessData();
  }

  if(State != "Idle"){
    DeliverDrink();
  }

  if(digitalRead(Homebutton) == LOW){
    HomeMotors();
  }


  if (MotCarousel_A.distanceToGo() != 0){
    MotCarousel_A.run();
  }
  

 
  if (MotCarousel_B.distanceToGo() != 0){
    MotCarousel_B.run();
  }

  if (MotCarousel_C.distanceToGo() != 0){
    MotCarousel_C.run();
  }


  if (MotX_Axis.distanceToGo() != 0){
    MotX_Axis.run();
  }
  


  if (MotY_Axis.distanceToGo() != 0){
    MotY_Axis.run();
  }

 
}

int GetPourTime(float oz){                        // Function to convert volume to time in milliseconds.
  float milliliters = round(oz * 29.5735);        // Convert US oz to ml
  PourTime = milliliters * 34;                    // Adjust this to calibrate pour time.
  return PourTime;
}

void DeliverDrink(){
  if (State == "First"){
      MotX_Axis.moveTo(DeliveryPosX);
        if (MotX_Axis.distanceToGo() == 0){
          delay(500);
          State = "Y_Delivery";
        }
  }
  if (State == "Y_Delivery"){
      MotY_Axis.moveTo(DeliveryPosY);
        if (MotY_Axis.distanceToGo() == 0){
          //while (!digitalRead(Cup_Present)){                     // waiting for user to remove the cup.
        
          //}
          Serial.println("waiting for cup removal");
          delay(2000);
          Serial.println("cup removed");
          State = "Home_Y";
        }    
  }
  if (State == "Home_Y"){
    MotY_Axis.moveTo(0);
      if (MotY_Axis.distanceToGo() == 0){
        Serial.println("Drink delivered.");
        State = "Idle";
        DataString = "";                                          // Clear the string of data
        IsRunning = false;                                        // If "IsRunning" is false, then it's all clear to make another drink.
      }
  }
  if (State == "Idle"){
    // just keep going
  }
}

void StepperCoils(String CoilState){
  if(CoilState == "Enable"){
    digitalWrite(En_Carousel_A, 0);               // Disable steppers motor coils - '0' is enabled.
    digitalWrite(En_Carousel_B, 0);
    digitalWrite(En_Carousel_C, 0);
    digitalWrite(En_X, 0);		                  				       
    digitalWrite(En_Y, 0);
  }
  else if(CoilState == "Disabled"){
    digitalWrite(En_Carousel_A, 1);               // Disable steppers motor coils - '1' is disabled.
    digitalWrite(En_Carousel_B, 1);
    digitalWrite(En_Carousel_C, 1);
    digitalWrite(En_X, 1);		                  				       
    digitalWrite(En_Y, 1);
  }
}