#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>

int ServoPinA = 7;
int ServoPinB = 6;
int ServoPinC = 5;

int SolValve;

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

String DataString = "a1,1.5,c5,0.75,c2,1";
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

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;

// Instantiate Stepper motors
AccelStepper MotCarousel_A(1, 10, 11);  
AccelStepper MotCarousel_B(1, 12, 13);
AccelStepper MotCarousel_C(1, 8, 9);
AccelStepper MotX_Axis(1, 12, 13);
AccelStepper MotY_Axis(1, 8, 9);   
   
// 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;

void setup() {
  Serial.begin(9600); 

  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(50);   
  MotY_Axis.setMaxSpeed(100);
	MotY_Axis.setAcceleration(50); 
  
PourServoA.attach(ServoPinA);
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(500);
  MotCarousel_B.moveTo(300);
  MotCarousel_C.moveTo(100);

}

void ParseDataString(){                               // This counts the number of commas in the string.
  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();
        delay(1000);
        PourServoA.write(90);
       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(0);
        delay(1000);
        j=j+1;
        ElementCount=ElementCount +2;
      }
  }

  if (Elements[j].charAt(0) == 'b'){
    CurrentLoc = Elements[j];
    MotX_Axis.moveTo(b_pos);
    MotCarousel_C.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(90);
        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(0);
        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(90);
        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(0);
        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 DeliverDrink(){
  if (State == "First"){
      MotX_Axis.moveTo(DeliveryPosX);
        if (MotX_Axis.distanceToGo() == 0){
          delay(500);
          State = "Y_Delivery";
        }
  }
  else 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";
        }    
  }
  else 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.
      }
  }
  else if (State == "Idle"){
    // just keep going
  }
}


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;
}


// Test to make sure the cup didn't fall off. If the cup is not preset after we begin a drink pour (either has been removed 
// or fallen off), we need to shut off all the valves and optics servos so we don't continue
// to dispense liquids all over the floor.
// We also stop all movement and then reset everything to "Home".
void IsCupPresent(){                              
  unsigned long CurrentMillis = millis();
  if (CurrentMillis - PreviousMillis >= 250) {   // Check every quarter second.
    PreviousMillis = CurrentMillis;
      if (!digitalRead(CupPresent)){
        PourServoA.write(0);                     // Shut off the expensive liquids first.
        PourServoB.write(0);
        PourServoC.write(0);
        SolDriver_0_7.reset();                   // Then shut off the mixers.
        SolDriver_8_14.reset();
        MotX_Axis.stop();                        // Then stop the movement.
        MotY_Axis.stop();
        MotCarousel_A.stop();
        MotCarousel_B.stop();
        MotCarousel_C.stop();
        delay(3000);                            // wait 3 seconds
       // HomeAllMotors();                        // Home the motors
      }
    Serial.println("here");
  }

}

unsigned long InitialHomingA = -1;
unsigned long InitialHomingB = -1;
unsigned long InitialHomingC = -1;

void HomeMotors(){
  MotorsHome = true;
  //MotCarousel_A.moveTo(0);
  //MotCarousel_B.moveTo(0);
  //MotCarousel_C.moveTo(0);
    if(MotCarousel_A.distanceToGo() != 0){
      MotCarousel_A.moveTo(InitialHomingA);
      InitialHomingA--;
    }
    if(MotCarousel_B.distanceToGo() == 0){
      MotCarousel_B.moveTo(InitialHomingB);
      InitialHomingB--;
    }
    if(MotCarousel_C.distanceToGo() == 0){
      MotCarousel_C.moveTo(InitialHomingC);
      InitialHomingC--;
    }
}


void loop() {
  if(IsRunning == false){   // Data in buffer and NOT making a drink?
        ParseDataString();                    // Go ahead and start the drink.
  } 

  if(MotCarousel_A.distanceToGo() == 0){
    HomeMotors();
  }

  //ProcessData();

 // DeliverDrink();
  
  //IsCupPresent();

  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();
  }

 
}