#include <ESP32Servo.h>

#define servoCenterPin 32    // 5V
#define servoDispensePin 33  // 5V
#define usTrigPin 5
#define usEchoPin 18
#define muxS0Pin 21
#define muxS1Pin 19
#define LED 2

Servo servoCenter, servoDispense;
const char* medType[15] = {"Diabetes", "Cholestrol", "Hypertension"};
const int typeCount = 3, cw = 30, ccw = 160, cut = 95;    // ###calibration
const int fullrev = 250, halfrev = 125, rightrev = 80;
int usDistanceMeasure(int, int);
void containerSelect(int*, int);
void objDispense(int*);
// 5~185 90stop 30cw 160ccw


void setup() {
  // put your setup code here, to run once:

  // Initialize UART0 (Serial) for one device
  Serial.begin(9600);
  // Initialize UART2 (Serial2) for another device
  Serial2.begin(9600);

  servoCenter.attach(servoCenterPin);
  servoDispense.attach(servoDispensePin);
  pinMode(usTrigPin, OUTPUT);   
  pinMode(usEchoPin, INPUT);
  pinMode(muxS0Pin, OUTPUT);
  pinMode(muxS1Pin, OUTPUT);
  pinMode(LED, OUTPUT);
}

void loop() {
  // put your main code here, to run repeatedly:
  
  static int usMUXCtr = 0, muxS0 = 0, muxS1 = 0, obsdistance = 0;
  static int centerPos = 0, dispensePos; 
  static int dStock[typeCount], rStock[typeCount];
  static char medRStockMessage[100];
  char medRStockStr[10];

  static int typeType = 0, typeAmount = 5, medRStock = 0;
  static bool dispense = false;
  // data input + dispense logic
  if (dispense == false){
    if (Serial.available()){
      String receivedMessage = Serial.readStringUntil('\n');
      if (receivedMessage.charAt(0) == '@'){
        int typeIndex = 0;
        int startIndex = 1;
        for (int i=0;i<receivedMessage.length();i++){
          if (receivedMessage.charAt(i) == '/'){
            dStock[typeIndex] = atoi(receivedMessage.substring(startIndex, i).c_str());
            typeIndex++;
            startIndex = i + 1;
          }
        }
        dStock[typeIndex] = atoi(receivedMessage.substring(startIndex).c_str());
        for (int i=0; i<typeCount; i++){
          if (dStock[i] > rStock[i]){         // ### INSUFFICIENT STOCK ERROR
            digitalWrite(LED, LOW);
            while (1){
              digitalWrite(LED, HIGH);
              delay(500);
              digitalWrite(LED, LOW);
              delay(500);
            }
          }
        }
        dispense = true;
      }
    }
    // remaining stock tracker + data output
    muxS0 = usMUXCtr & 0x1;
    muxS1 = (usMUXCtr >> 1) & 0x1;
    digitalWrite(muxS0Pin, muxS0);
    digitalWrite(muxS1Pin, muxS1);
    obsdistance = usDistanceMeasure(usTrigPin, usEchoPin);
    medRStock = obsdistance * 2;                   // ###calibration
    rStock[usMUXCtr] = medRStock;
    snprintf(medRStockStr, sizeof(medRStockStr), "%d", medRStock);
    strcat(medRStockMessage, medRStockStr);
    if (usMUXCtr < (typeCount-1)){   // run for two times (typecount = 3)
      strcat(medRStockMessage, "/");
      usMUXCtr++;
    }
    else{
      Serial.println(medRStockMessage);
      usMUXCtr = 0;
      medRStockMessage[0] = '\0';
    }
  }
  else{
    digitalWrite(muxS0Pin, 1);
    digitalWrite(muxS1Pin, 1);
    for (int i=0; i<typeCount; i++){
      if (dStock[i] > 0 ){
        containerSelect(&centerPos, i);
        objDispense(&dStock[i]);
      }
    }
    dispense = false;
  }
  delay(500);
}


int usDistanceMeasure(int trigPin, int echoPin){
  // ultrasonic sensor
  int obsduration = 0, min = 0, rate = 10, obsdistance[rate];
  for (int i=0; i<rate; i++){
    digitalWrite(trigPin,LOW);
    delayMicroseconds(5);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    // sensor triggers HIGH pulse of 10 microseconds
    // short LOW pulse beforehand to ensure a clean HIGH pulse
    obsduration = pulseIn(echoPin, HIGH);
    obsdistance[i] = (obsduration/4) / 29.1;  // ###calibration
    // obsduration: 2 * time taken for ultrasonic wave to reach obstacle (uS)
    // by s = v*t & t = traveltime / 2
    // speed of sound = 343 m/s = 0.0343 cm/uS = 1/29.1 cm/uS
    // obsdistance: distance from the obstacle (cm)
    if (min == 0 || obsdistance[i] < min)
      min = obsdistance[i];
  }
  // Serial.println(obsdistance);
  return min;
}

void containerSelect(int* centerPos, int targetPos){
  int step, servoMag;
  int diff = targetPos - *centerPos;
  if (abs(diff) > targetPos/2)      // determine number of steps
    step = targetPos - abs(diff);
  else
    step = abs(diff);

  if (*centerPos >= targetPos/2){    // determine direction
    if (diff >= -targetPos/2 && diff < 0)
      servoMag = cw;
    else
      servoMag = ccw;    
  }
  else{
    if (diff >= 1 && diff < targetPos/2)
      servoMag = ccw;
    else
      servoMag = cw;
  }
  while (step > 0){
    servoCenter.write(servoMag);
    while (usDistanceMeasure(usTrigPin, usEchoPin) < 5){
      if (usDistanceMeasure(usTrigPin, usEchoPin) > 5){
        step--;
        break;
      }
    }      
  }
  servoCenter.write(cut);
  *centerPos = targetPos;
}

void objDispense(int* amount){
  while (amount > 0){
    servoDispense.write(180);
    delay(1000);
    servoDispense.write(0);
    *amount--;
  }
}