#include <WiFi.h>
#include <ESPAsyncWebServer.h>
#include <LiquidCrystal_I2C.h>
#include <Adafruit_SSD1306.h>
//#include <Servo.h>

// Define motor and encoder pins
#define FEED_MOTOR_PWM 18
#define FEED_MOTOR_DIR1 19
#define FEED_MOTOR_DIR2 21
#define ENCODER_A 34
#define ENCODER_B 35

// Define runout sensor and relay
#define RUNOUT_SENSOR 25
#define RELAY_PIN 26

// // Define servo pins
// #define SHEATH_GRIP_SERVO 27
// #define CORE_STRIP_SERVO 14
// #define SOLDER_HEAD_SERVO 12

// OLED setup
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// LCD setup
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Global variables
volatile long encoderTicks = 0; // Tracks encoder ticks
float ticksPerCm = 50.0;        // Number of encoder ticks per cm (to be calibrated)
float desiredLength = 0.0;      // User-specified wire length (in cm)
bool systemFlag = false;        // Flag to control system
bool lengthSet = false;         // Flag to indicate length is set

// // Servo objects
// Servo sheathGripServo;
// Servo coreStripServo;
// Servo solderHeadServo;

// Wi-Fi credentials
const char* ssid = "Wokwi-GUEST";
const char* password = "";

// Web server
AsyncWebServer server(80);

// Interrupt Service Routine for encoder
void IRAM_ATTR readEncoder() {
  bool encoderState = digitalRead(ENCODER_A);
  static bool lastEncoderState = LOW;
  if (encoderState != lastEncoderState) {
    if (digitalRead(ENCODER_B) != encoderState) {
      encoderTicks++; // Forward motion
    } else {
      encoderTicks--; // Backward motion
    }
  }
  lastEncoderState = encoderState;
}

// Function to display messages
void displayMessage(String message) {
  oled.clearDisplay();
  oled.setCursor(0, 0);
  oled.print(message);
  oled.display();

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(message);
}

// Function to move motor to feed wire
void feedWire(float lengthCm) {
  encoderTicks = 0; // Reset encoder count
  long targetTicks = lengthCm * ticksPerCm; // Calculate target ticks

  // Start motor to move forward
  digitalWrite(FEED_MOTOR_DIR1, HIGH);
  digitalWrite(FEED_MOTOR_DIR2, LOW);
  analogWrite(FEED_MOTOR_PWM, 128); // 50% speed

  while (encoderTicks < targetTicks) {
    // Display progress
    float currentLength = encoderTicks / ticksPerCm;
    displayMessage("Feeding: " + String(currentLength) + " cm");
  }

  // Stop the motor
  analogWrite(FEED_MOTOR_PWM, 0);
  displayMessage("Length Reached: " + String(lengthCm) + " cm");
}

// Function for sheathing removal
void removeSheathing() {
  displayMessage("Removing Sheathing...");
 // sheathGripServo.write(90); // Grip wire
  delay(500);

  // Move gripper back to remove sheathing
  digitalWrite(FEED_MOTOR_DIR1, LOW);
  digitalWrite(FEED_MOTOR_DIR2, HIGH);
  analogWrite(FEED_MOTOR_PWM, 128); // Move backward
  delay(1000); // Adjust based on sheathing length
  analogWrite(FEED_MOTOR_PWM, 0);

 // sheathGripServo.write(0); // Release wire
  displayMessage("Sheathing Removed");
}

// Function for core stripping
void stripCores() {
  displayMessage("Stripping Cores...");
 // coreStripServo.write(90); // Grip cores
  delay(500);
//  coreStripServo.write(0); // Release cores
  displayMessage("Cores Stripped");
}

// Function for soldering cores
void solderCores() {
  displayMessage("Tinning Cores...");
 // solderHeadServo.write(90); // Position solder head
  delay(500);

  digitalWrite(RELAY_PIN, HIGH); // Turn on heating element
  delay(2000); // Simulate soldering
  digitalWrite(RELAY_PIN, LOW);

  //solderHeadServo.write(0); // Retract solder head
  displayMessage("Cores Tinned");
}

// Function to cut the wire
void cutWire() {
  displayMessage("Cutting Wire...");
  delay(1000); // Simulate cutting (add real hardware logic here)
  displayMessage("Wire Cut");
}

// Web server setup
void setupServer() {
  server.on("/", HTTP_GET, [](AsyncWebServerRequest* request) {
    request->send(200, "text/html",
      "<h1>Wire Prep Machine</h1>"
      "<form action='/setLength' method='get'>"
      "Enter Length (cm): <input type='number' name='length' min='1'>"
      "<input type='submit' value='Submit'>"
      "</form>"
      "<p><a href='/start'>Start</a> | <a href='/stop'>Stop</a></p>");
  });

  server.on("/setLength", HTTP_GET, [](AsyncWebServerRequest* request) {
    if (request->hasParam("length")) {
      desiredLength = request->getParam("length")->value().toFloat();
      lengthSet = true;
      displayMessage("Length Set: " + String(desiredLength) + " cm");
      request->send(200, "text/plain", "Length set to " + String(desiredLength) + " cm");
    } else {
      request->send(400, "text/plain", "Length not specified");
    }
  });

  server.on("/start", HTTP_GET, [](AsyncWebServerRequest* request) {
    if (lengthSet) {
      systemFlag = true;
      request->send(200, "text/plain", "System started!");
    } else {
      request->send(200, "text/plain", "Set the length first!");
    }
  });

  server.on("/stop", HTTP_GET, [](AsyncWebServerRequest* request) {
    systemFlag = false;
    displayMessage("System Stopped");
    request->send(200, "text/plain", "System stopped!");
  });

  server.begin();
}

// Setup function
void setup() {
  Serial.begin(115200);

  // Initialize OLED and LCD
  oled.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  lcd.init();
  lcd.backlight();

  // Initialize motor pins
  pinMode(FEED_MOTOR_PWM, OUTPUT);
  pinMode(FEED_MOTOR_DIR1, OUTPUT);
  pinMode(FEED_MOTOR_DIR2, OUTPUT);

  // Initialize encoder
  pinMode(ENCODER_A, INPUT_PULLUP);
  pinMode(ENCODER_B, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENCODER_A), readEncoder, CHANGE);

  // // Initialize servos
  // sheathGripServo.attach(SHEATH_GRIP_SERVO);
  // coreStripServo.attach(CORE_STRIP_SERVO);
  // solderHeadServo.attach(SOLDER_HEAD_SERVO);

  // Initialize relay
  pinMode(RELAY_PIN, OUTPUT);
  digitalWrite(RELAY_PIN, LOW);

  // Initialize runout sensor
  pinMode(RUNOUT_SENSOR, INPUT_PULLUP);

  // Setup web server
  setupServer();

  displayMessage("System Ready");
}

// Main loop
void loop() {
  if (systemFlag) {
    feedWire(desiredLength);
    removeSheathing();
    stripCores();
    solderCores();
    cutWire();

    // Repeat for the other end
    feedWire(desiredLength);
    removeSheathing();
    stripCores();
    solderCores();
    cutWire();

    systemFlag = false; // Stop after completing the process
    displayMessage("Process Complete");
  }
}