#include <Arduino.h>
#include <WiFi.h>
#include <WebServer.h>
#include <ArduinoJson.h>
#include <ESP32Servo.h>
#include <ESPmDNS.h>

// Motor pin definitions
const int stepPin1 = 21;  // Wire feed
const int dirPin1 = 7;
const int stepPin2 = 6;  // Z-axis
const int dirPin2 = 5;
const int stepPin3 = 4;  // Bend
const int dirPin3 = 3;

// Servo
const int servoPin = 2;
Servo myServo;

// Stepper parameters
const int defaultStepDelay = 4000;  // µs

// Web server
WebServer server(80);

// Wi-Fi credentials
const char* ssid = "EngineeringStudent";
const char* password = "cls2024!";
const char* hostname = "bender";

// Helper: Stepper runner with start_delay
void runStepper(void* parameter) {
  int* p = (int*)parameter;
  int stepPin = p[0];
  int dirPin = p[1];
  int steps = p[2];
  int dir = p[3];
  int stepDelay = p[4];
  int startDelay = p[5];

  Serial.printf("Stepper scheduled: pin=%d, steps=%d, dir=%d, stepDelay=%d, startDelay=%d\n",
                stepPin, steps, dir, stepDelay, startDelay);

  delay(startDelay);

  digitalWrite(dirPin1, LOW);
  digitalWrite(dirPin2, dir);
  Serial.print("Direction set to: ");
  Serial.println(dir);
  for (int i = 0; i < steps; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(stepDelay);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(stepDelay);
  }

  Serial.printf("Stepper complete: pin=%d\n", stepPin);
  delete[] p;
  vTaskDelete(NULL);
}

// Helper: Servo runner with start_delay
void runServo(void* parameter) {
  int* p = (int*)parameter;
  int angle = p[0];
  int startDelay = p[1];

  Serial.printf("Servo scheduled: angle=%d, startDelay=%d\n", angle, startDelay);
  delay(startDelay);
  myServo.write(angle);
  Serial.println("Servo movement complete.");
  delete[] p;
  vTaskDelete(NULL);
}

// HTTP endpointf
void handleBend() {
  if (server.hasArg("plain") == false) {
    server.send(400, "application/json", "{\"error\":\"Missing body\"}");
    return;
  }

  StaticJsonDocument<1024> doc;
  DeserializationError err = deserializeJson(doc, server.arg("plain"));

  if (err) {
    Serial.println("JSON parse failed!");
    server.send(400, "application/json", "{\"error\":\"Invalid JSON\"}");
    return;
  }

  Serial.println("Received valid JSON.");

  // Feed motor: array of movements
  if (doc.containsKey("feed")) {
    JsonArray arr = doc["feed"].as<JsonArray>();
    for (JsonObject m : arr) {
      int steps = m["steps"] | 0;
      int delay = m["stepdelay"] | defaultStepDelay;
      int startDelay = m["start_delay"] | 0;
      int* params = new int[6]{ stepPin1, dirPin1, steps, 0, delay, startDelay };
      
      xTaskCreate(runStepper, "FeedStepper", 4096, params, 1, NULL);
    }
  }

  // Z axis: array of movements
  if (doc.containsKey("z")) {
    JsonArray arr = doc["z"].as<JsonArray>();
    for (JsonObject m : arr) {
      int steps = m["steps"] | 0;
      int dir = m["dir"] | 0;
      int delay = m["stepdelay"] | defaultStepDelay;
      int startDelay = m["start_delay"] | 0;
      int* params = new int[6]{ stepPin2, dirPin2, steps, dir, delay, startDelay };
      xTaskCreate(runStepper, "FeedStepper", 4096, params, 1, NULL);
    }
  }

  // Bend motor
  if (doc.containsKey("bend")) {
    const char* direction = doc["bend"]["direction"] | "left";
    int startDelay = doc["bend"]["start_delay"] | 0;
    int dir = (strcmp(direction, "right") == 0) ? 1 : 0;

    int* params = new int[2]{ dir, startDelay };
    xTaskCreate(runBend, "BendMotor", 4096, params, 1, NULL);
  }

  // Servo: array of movements
  if (doc.containsKey("servo")) {
    JsonArray arr = doc["servo"].as<JsonArray>();
    for (JsonObject m : arr) {
      int angle = m["angle"] | 90;
      int startDelay = m["start_delay"] | 0;

      int* params = new int[2]{ angle, startDelay };
      xTaskCreate(runServo, "ServoTask", 2048, params, 1, NULL);
    }
  }
}

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

  pinMode(stepPin1, OUTPUT);
  pinMode(dirPin1, OUTPUT);
  pinMode(stepPin2, OUTPUT);
  pinMode(dirPin2, OUTPUT);
  pinMode(stepPin3, OUTPUT);
  pinMode(dirPin3, OUTPUT);
  myServo.attach(servoPin);

  WiFi.begin(ssid, password);
  Serial.print("Connecting to WiFi");
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  Serial.println("\nWiFi connected. IP: " + WiFi.localIP().toString());

  if (MDNS.begin(hostname)) {
    Serial.println("MDNS responder started as http://" + String(hostname) + ".local");
  } else {
    Serial.println("Error starting mDNS");
  }

  server.on("/bend", HTTP_POST, handleBend);
  server.begin();
}

void loop() {
  server.handleClient();
}
