#include <LiquidCrystal_I2C.h>
#include <Keypad.h>

// Define the LCD
#define LCD_ADDRESS 0x27
#define LCD_COLUMNS 16
#define LCD_ROWS 2
LiquidCrystal_I2C lcd(LCD_ADDRESS, LCD_COLUMNS, LCD_ROWS);

// Define the Keypad
const byte ROWS = 4;
const byte COLS = 3;
char hexaKeys[ROWS][COLS] = {
  {'1', '2', '3'},
  {'4', '5', '6'},
  {'7', '8', '9'},
  {'*', '0', '#'}
};
byte rowPins[ROWS] = {12, 13, 14, 27};
byte colPins[COLS] = {26, 25, 33};
Keypad myKeypad = Keypad(makeKeymap(hexaKeys), rowPins, colPins, ROWS, COLS);

// Define Stepper Motor pins and settings
const int dirPin = 19;
const int stepPin = 18;
const int enablePin = 15; // Optional, can be used to enable/disable the driver
const int stepsPerRevolution = 200;
const int microstepsPerRevolution = stepsPerRevolution * 16; // Assuming 1/16 microstepping

// State Machine States
enum State {
  SELECT_SYRINGE,
  SELECT_VOLUME,
  SELECT_TIME,
  LOAD_SYRINGE,
  WAIT_FOR_START,
  PUMP_DOSE,
  FINISH_DOSE
};

State currentState = SELECT_SYRINGE;

// Variables for dose and speed control
String dose = "";
String timeInput = "";
int syringeSize = 0; // 0 for not selected, 10 for 10ml, 20 for 20ml, 50 for 50ml
float doseValue = 0;
float timeValue = 0;
const float screwPitch = 8.0; // Example screw pitch in mm/rev
const float pi = 3.14159;
float syringeRadius = 0;

void setup() {
  // Initialize LCD
  lcd.begin(LCD_COLUMNS, LCD_ROWS);
  lcd.init();
  lcd.backlight();
  
  // Initialize Serial
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");

  // Initialize Stepper Motor Pins
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(enablePin, OUTPUT);

  // Enable the stepper motor driver
  digitalWrite(enablePin, LOW);

  // Display startup messages
  lcd.setCursor(0, 0);
  lcd.print("IU-VNU-HCMC");
  lcd.setCursor(0, 1);
  lcd.print("School of BME");
  delay(2000);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("MEDICALDESIGN2B");
  lcd.setCursor(0, 1);
  lcd.print("Group 5");
  delay(2000);
  lcd.clear();
  displaySyringePrompt();
}

void loop() {
  char key = myKeypad.getKey();

  if (key) {
    Serial.print("Key Pressed: ");
    Serial.println(key);

    switch (currentState) {
      case SELECT_SYRINGE:
        handleSyringeSelection(key);
        break;
      case SELECT_VOLUME:
        handleVolumeSelection(key);
        break;
      case SELECT_TIME:
        handleTimeSelection(key);
        break;
      case WAIT_FOR_START:
        if (key == '*') {
          currentState = PUMP_DOSE;
        }
        break;
      case PUMP_DOSE:
        executeDose(doseValue, timeValue);
        break;
      case FINISH_DOSE:
        syringeSize = 0;
        doseValue = 0;
        currentState = SELECT_SYRINGE;
        displaySyringePrompt();
        break;
      default:
        break;
    }
  }
}

void handleSyringeSelection(char key) {
  if (key == '*') {
    syringeSize = 0;
    currentState = SELECT_SYRINGE;
    displaySyringePrompt();
  } else if (key == '1') {
    syringeSize = 10;
    syringeRadius = 0.795; // 7.95mm in cm
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("10ml SYRINGE");
    delay(2000);
    displayDosePrompt();
    currentState = SELECT_VOLUME;
  } else if (key == '2') {
    syringeSize = 20;
    syringeRadius = 0.990; // 9.90mm in cm
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("20ml SYRINGE");
    delay(2000);
    displayDosePrompt();
    currentState = SELECT_VOLUME;
  } else if (key == '3') {
    syringeSize = 50;
    syringeRadius = 1.432; // 14.32mm in cm
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("50ml SYRINGE");
    delay(2000);
    displayDosePrompt();
    currentState = SELECT_VOLUME;
  }
}

void handleVolumeSelection(char key) {
  if (key == '*') {
    dose = "";
    currentState = SELECT_SYRINGE;
    displaySyringePrompt();
  } else if (key == '#') {
    doseValue = dose.toFloat();
    if (doseValue >= 1 && doseValue <= syringeSize) {
      displayTimePrompt();
      currentState = SELECT_TIME;
    } else {
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Invalid dose");
      delay(2000);
      displayDosePrompt();
    }
    dose = ""; // Reset the dose input
  } else {
    dose += key;
    lcd.setCursor(0, 1);
    lcd.print(dose);
  }
}

void handleTimeSelection(char key) {
  if (key == '*') {
    timeInput = "";
    currentState = SELECT_VOLUME;
    displayDosePrompt();
  } else if (key == '#') {
    timeValue = timeInput.toFloat();
    if (timeValue > 0) {
      moveToLoadPosition(doseValue);
      currentState = WAIT_FOR_START;
    } else {
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Invalid time");
      delay(2000);
      displayTimePrompt();
    }
    timeInput = ""; // Reset the time input
  } else {
    timeInput += key;
    lcd.setCursor(0, 1);
    lcd.print(timeInput);
  }
}

void displaySyringePrompt() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("SELECT SYRINGE:");
  delay(1000);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("1:10ml 2:20ml");
  lcd.setCursor(0, 1);
  lcd.print("3:50ml");
}

void displayDosePrompt() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Dose of (mL)");
  lcd.setCursor(0, 1);
}

void displayTimePrompt() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Time (min)");
  lcd.setCursor(0, 1);
}

void displayStartPrompt() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Press * to Pump");
}

void moveToLoadPosition(float ml) {
  int stepsPerML = getStepsPerML(syringeSize);
  int steps = ml * stepsPerML;
  
  // Move backward to load syringe
  digitalWrite(dirPin, HIGH);
  for (int i = 0; i < steps; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(2000);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(2000);
  }

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Finish Load");
  delay(2000);
  displayStartPrompt();
}

void executeDose(float ml, float time) {
  int stepsPerML = getStepsPerML(syringeSize);
  int steps = ml * stepsPerML;
  
  float flowRate = ml / time; // Flow rate in mL/min
  float pushSpeed = flowRate / (pi * syringeRadius * syringeRadius); // Speed in cm/min
  float timeOneRevolution = (screwPitch * 60) / (pushSpeed * 10); // Time for one revolution in seconds
  float timeOnePulse = (timeOneRevolution * 1000000) / (microstepsPerRevolution * 2); // Time per pulse in microseconds

  // Move forward to dispense
  digitalWrite(dirPin, LOW);
  for (int i = 0; i < steps; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(timeOnePulse);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(timeOnePulse);
  }

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Finish Dose");
  delay(2000);
  currentState = FINISH_DOSE;
}

float calculateRadius(int size) {
  switch (size) {
    case 10:
      return 0.795; // 7.95mm in cm
    case 20:
      return 0.990; // 9.90mm in cm
    case 50:
      return 1.432; // 14.32mm in cm
    default:
      return 0.795; // Default radius if syringe size is not set
  }
}

int getStepsPerML(int syringeSize) {
  switch (syringeSize) {
    case 10:
      return 100; // Adjust this value based on your 10ml syringe pump mechanism
    case 20:
      return 200; // Adjust this value based on your 20ml syringe pump mechanism
    case 50:
      return 500; // Adjust this value based on your 50ml syringe pump mechanism
    default:
      return 100; // Default value if syringe size is not set
  }
}