#include <DHT.h>

// Motor operating modes
enum MotorMode {
  IDLE,
  HOMING,
  MOVING_TO_POSITION,
  CONTINUOUS
};

// Motor structure
struct Motor {
  int stepPin;
  int dirPin;
  int speed;             // Steps per second
  int direction;         // 1 = CW, -1 = CCW
  long position;         // Current position in steps
  bool homed;            // Is homed
  bool continuous;       // If it rotates forever (like drum)
  MotorMode mode;        // Current mode
  long targetPosition;   // Where to go (for MOVING_TO_POSITION)
};

Motor motors[5] = {
  {3, 2, 0, 1, 0, false, false, IDLE, 0},  // X
  {5, 4, 0, 1, 0, false, false, IDLE, 0},  // Y - Homogenizer
  {7, 6, 0, 1, 0, false, false, IDLE, 0},  // Syringe Pump-1
  {12,11,0, 1, 0, true, true,  CONTINUOUS, 0}, // Drum (always ready)
  {8, 9, 0, 1, 0, false, false, IDLE, 0}   // Syringe Pump-2
};

// Limit switches for motors 0,1,2,4
const int limitSwitchPins[4] = {20, 21, 22, 26};

// Relays
const int relayPins[4] = {13,14,15,16};

// DC Motor Driver (L298N)
const int in1Pin = 17;
const int in2Pin = 18;
const int enaPin = 19;

// DHT22 Sensor
#define DHTPIN 10
#define DHTTYPE DHT22
DHT dht(DHTPIN, DHTTYPE);

// Step timers
unsigned long lastStepTime[5] = {0};

void setup() {
  Serial1.begin(9600);  // HMI UART

  for (int i = 0; i < 5; i++) {
    pinMode(motors[i].stepPin, OUTPUT);
    pinMode(motors[i].dirPin, OUTPUT);
    digitalWrite(motors[i].stepPin, LOW);
    digitalWrite(motors[i].dirPin, LOW);
  }

  for (int i = 0; i < 4; i++) {
    pinMode(limitSwitchPins[i], INPUT_PULLUP);
    pinMode(relayPins[i], OUTPUT);
    digitalWrite(relayPins[i], LOW);
  }

  pinMode(in1Pin, OUTPUT);
  pinMode(in2Pin, OUTPUT);
  pinMode(enaPin, OUTPUT);

  analogWrite(enaPin, 0);
  dht.begin();
}

void loop() {
  checkSerial();

  // Step all motors
  for (int i = 0; i < 5; i++) {
    updateMotor(i);
  }
}

void updateMotor(int motorIndex) {
  Motor &m = motors[motorIndex];
  
  if (m.speed == 0 || m.mode == IDLE) return;

  unsigned long interval = 1000000L / m.speed;
  if (micros() - lastStepTime[motorIndex] >= interval) {
    lastStepTime[motorIndex] = micros();

    if (m.mode == HOMING) {
      digitalWrite(m.dirPin, LOW); // Homing always CCW

      digitalWrite(m.stepPin, HIGH);
      delayMicroseconds(2);
      digitalWrite(m.stepPin, LOW);

      if (digitalRead(limitSwitchPins[motorIndex]) == LOW) { 
        // Switch triggered
        m.homed = true;
        m.position = 0;
        m.mode = IDLE;
        Serial1.println("Motor_" + String(motorIndex) + "_Homed");
      }

    } else if (m.mode == MOVING_TO_POSITION) {
      if (m.position != m.targetPosition) {
        m.direction = (m.targetPosition > m.position) ? 1 : -1;
        digitalWrite(m.dirPin, (m.direction == 1) ? HIGH : LOW);

        digitalWrite(m.stepPin, HIGH);
        delayMicroseconds(2);
        digitalWrite(m.stepPin, LOW);

        m.position += m.direction;
      } else {
        m.mode = IDLE;
      }

    } else if (m.mode == CONTINUOUS) {
      digitalWrite(m.dirPin, (m.direction == 1) ? HIGH : LOW);
      digitalWrite(m.stepPin, HIGH);
      delayMicroseconds(2);
      digitalWrite(m.stepPin, LOW);

      m.position += m.direction; // just counting steps if needed
    }
  }
}

void checkSerial() {
  if (Serial1.available()) {
    String command = Serial1.readStringUntil('\n');
    command.trim();
    processCommand(command);
  }
}

void processCommand(String cmd) {
  if (cmd.startsWith("HOME_")) {
    int motorIndex = cmd.substring(5).toInt();
    if (motorIndex >= 0 && motorIndex < 5) {
      motors[motorIndex].mode = HOMING;
      motors[motorIndex].speed = 200; // Set a safe homing speed
    }
  }

  else if (cmd.startsWith("SPEED_")) {
    int motorIndex = cmd.substring(6,7).toInt();
    int spd = cmd.substring(8).toInt();
    if (motorIndex >= 0 && motorIndex < 5) {
      motors[motorIndex].speed = spd;
    }
  }

  else if (cmd.startsWith("X+")) jogMotor(0, 5);
  else if (cmd.startsWith("X-")) jogMotor(0, -5);
  else if (cmd.startsWith("Y+")) jogMotor(1, 5);
  else if (cmd.startsWith("Y-")) jogMotor(1, -5);
  else if (cmd.startsWith("P1+")) jogMotor(2, 5);
  else if (cmd.startsWith("P1-")) jogMotor(2, -5);
  else if (cmd.startsWith("P2+")) jogMotor(4, 5);
  else if (cmd.startsWith("P2-")) jogMotor(4, -5);

  else if (cmd.startsWith("DRUM_START=")) {
    int spd = cmd.substring(11).toInt();
    motors[3].speed = spd;
    motors[3].mode = CONTINUOUS;
    motors[3].direction = 1;
  }

  else if (cmd == "DRUM_STOP") {
    motors[3].speed = 0;
    motors[3].mode = IDLE;
  }
}

void jogMotor(int motorIndex, int mm) {
  Motor &m = motors[motorIndex];

  if (!m.homed) {
    Serial1.println("WARN: Motor_" + String(motorIndex) + "_Not_Homed");
    return;
  }

  int steps = mmToSteps(mm);
  m.targetPosition = m.position + steps;
  m.mode = MOVING_TO_POSITION;
}

int mmToSteps(float mm) {
  return (mm * 200) / 5.0; // 5mm pitch = 200 steps/rev
}