// Multi-motor stepper control with homing (Pico W, Arduino C++)
// Motors 1-4 require homing (limit switch), motor 5 does not.
// Motor 2 has a special loop (back-and-forth) mode.

#include <DHT.h>         // Include library for DHT sensor
#include <L298N.h>

// --- Constants and Pin Definitions ---
#define DHTPIN 10         // DHT sensor connected to pin 10
#define DHTTYPE DHT22       // Using DHT22 sensor
const int STEPS_PER_MM = 100;      // Steps per millimeter (tune for your hardware)
const int DEFAULT_SPEED_DELAY = 5000; // Default speed (microseconds between steps)
const int DEFAULT_HOMING_SPEED = 50;  // Default homing speed (0-100)

// --- Relay Pins ---
const int relayPins[4] = {14, 15, 16, 17};

// --- DHT Sensor ---
DHT dht(DHTPIN, DHTTYPE);    // Create DHT sensor object
unsigned long lastDHTReadTime = 0;   // To track time between DHT reads
const unsigned long dhtInterval = 5000; // Read DHT every 5 seconds (5000 ms)

// --- Motor States ---
enum MotorState {
  MOTOR_IDLE,
  MOTOR_HOMING,
  MOTOR_MOVING
};

// --- Homing States ---
enum HomingState {
  HOMING_NOT_HOMING,
  HOMING_MOVING_AWAY,
  HOMING_MOVING_TOWARDS,
  HOMING_COMPLETE
};

// --- Motor Structure ---
struct Motor {
  int stepPin;
  int dirPin;
  int limitPin;
  bool requiresHoming;
  bool isHomed;
  float currentPositionMM;
  float minLimitMM;
  float maxLimitMM;
  HomingState homingState;
  MotorState motorState;
  float targetPositionMM;
  long stepsToMove;
  long stepsMoved;
  bool moveForward;
  unsigned long lastStepTime;
  unsigned long stepHighStartTime;
  bool stepPinHigh;
  int currentSpeedDelay;
};

// --- Global Variables ---
Motor motors[6]; // Index 1-5 (we'll ignore index 0 for simplicity)
bool motor2LoopActive = false;
bool motor2LoopForward = true;
int motorToHome = 0;
int motorToMove = 0;
int homingSpeed = DEFAULT_HOMING_SPEED;

// --- Function Declarations ---
void initializeMotorPins(int id);
void stepControl(int id);
void setMotorDirection(int id, bool forward);
void homeMotor(int id);
void moveToPosition(int id, float targetMM);
void setup();
void loop();
void processSerialCommand();
void handleMotor2Loop();
void readDHTData();
bool isNumeric(String str);

// --- Helper Functions ---

// Function to check if a string is a number
bool isNumeric(String str) {
  for (int i = 0; i < str.length(); i++) {
    if (isDigit(str.charAt(i)) == false)
      return false;
  }
  return true;
}

// --- Motor Control Functions ---

// Initialize motor pins
void initializeMotorPins(int id) {
  Motor &m = motors[id];
  pinMode(m.stepPin, OUTPUT);
  pinMode(m.dirPin, OUTPUT);
  digitalWrite(m.dirPin, HIGH); // Initial direction
  digitalWrite(m.stepPin, LOW);  // Initial step state
  m.stepPinHigh = false;
}

// Non-blocking step control
void stepControl(int id) {
  Motor &m = motors[id];

  // Skip stepping if speed is set to -1 (motor disabled)
  if (m.currentSpeedDelay == -1) {
    return;
  }

  if (m.stepPinHigh) {
    // Step pin is HIGH, check if it's time to go LOW
    if (millis() - m.stepHighStartTime >= 10) { // 10 us HIGH duration
      digitalWrite(m.stepPin, LOW);
      m.stepPinHigh = false;
      m.lastStepTime = millis();

      // Update position after the step is complete
      if (id == 2 && motor2LoopActive) {
        if (motor2LoopForward) {
          m.currentPositionMM += 1.0 / STEPS_PER_MM;
        } else {
          m.currentPositionMM -= 1.0 / STEPS_PER_MM;
        }
      }

      // Update stepsMoved for moveToPosition
      if (m.motorState == MOTOR_MOVING) {
        m.stepsMoved++;
      }
    }
  } else {
    // Step pin is LOW, check if it's time to go HIGH
    if (millis() - m.lastStepTime >= m.currentSpeedDelay / 1000.0) {
      digitalWrite(m.stepPin, HIGH);
      m.stepPinHigh = true;
      m.stepHighStartTime = millis();
    }
  }
}

// Set the direction for a motor
void setMotorDirection(int id, bool forward) {
  Motor &m = motors[id];
  digitalWrite(m.dirPin, forward ? HIGH : LOW);
  delayMicroseconds(5); // Allow hardware settle time
  m.moveForward = forward;
  m.lastStepTime = millis();
}

// Non-blocking homeMotor function
void homeMotor(int id) {
  if (id < 1 || id > 5) {
    Serial.println("Invalid motor ID.");
    motorToHome = 0;
    return;
  }

  Motor &m = motors[id];

  if (!m.requiresHoming) {
    Serial.println("Motor does not require homing.");
    m.isHomed = true;
    motorToHome = 0;
    return;
  }

  if (m.homingState == HOMING_NOT_HOMING) {
    // Start homing
    Serial.print("Starting homing motor ");
    Serial.println(id);
    m.homingState = HOMING_MOVING_AWAY;
    m.lastStepTime = millis();

    // Use the global homingSpeed for the delay calculation
    const int minDelay = 500;
    const int maxDelay = 10000;
    m.currentSpeedDelay = map(homingSpeed, 1, 100, maxDelay, minDelay);
    setMotorDirection(id, true); // Move forward 10mm away from switch
  }

  if (m.homingState == HOMING_MOVING_AWAY) {
    // Moving away from the switch
    int steps10mm = 0.5 * STEPS_PER_MM;
    static int stepsMoved = 0;

    if (!motors[id].stepPinHigh) {
      if (millis() - m.lastStepTime >= m.currentSpeedDelay / 1000.0) {
        stepsMoved++;
        setMotorDirection(id, true);
        digitalWrite(m.stepPin, HIGH);
        m.stepPinHigh = true;
        m.stepHighStartTime = millis();
      }
    }

    if (stepsMoved >= steps10mm) {
      m.homingState = HOMING_MOVING_TOWARDS;
      stepsMoved = 0;
      Serial.println("Moving towards limit switch");
    }
    return;
  }

  if (m.homingState == HOMING_MOVING_TOWARDS) {
    // Moving towards the switch
    if (digitalRead(m.limitPin) == LOW) {
      // Limit switch pressed
      m.homingState = HOMING_COMPLETE;
      Serial.println("Limit switch pressed");
    } else {
      if (!motors[id].stepPinHigh) {
        if (millis() - m.lastStepTime >= m.currentSpeedDelay / 1000.0) {
          setMotorDirection(id, false);
          digitalWrite(m.stepPin, HIGH);
          m.stepPinHigh = true;
          m.stepHighStartTime = millis();
        }
      }
      return;
    }
  }

  if (m.homingState == HOMING_COMPLETE) {
    // Homing complete
    m.isHomed = true;
    m.currentPositionMM = 0.0;
    Serial.print("Motor ");
    Serial.print(id);
    Serial.println(" homed. Position set to 0.");
    m.homingState = HOMING_NOT_HOMING;
    motorToHome = 0;
  }
}

// Non-blocking moveToPosition function
void moveToPosition(int id, float targetMM) {
  if (id < 1 || id > 5) {
    Serial.println("Invalid motor ID.");
    motorToMove = 0;
    return;
  }

  Motor &m = motors[id];
  if (m.requiresHoming && !m.isHomed) {
    Serial.print("Motor ");
    Serial.print(id);
    Serial.println(" not homed! Use HOME command first.");
    motorToMove = 0;
    return;
  }

  if (m.motorState == MOTOR_IDLE) {
    // Start moving
    Serial.print("Starting moving motor ");
    Serial.print(id);
    Serial.print(" to ");
    Serial.print(targetMM);
    Serial.println(" mm");

    m.targetPositionMM = targetMM;
    float delta = targetMM - m.currentPositionMM;
    m.stepsToMove = abs(delta) * STEPS_PER_MM;
    m.stepsMoved = 0;
    m.motorState = MOTOR_MOVING;
    m.lastStepTime = millis();
    setMotorDirection(id, (delta > 0)); // Set initial direction
  }

  if (m.motorState == MOTOR_MOVING) {
    // Moving
    if (m.stepsMoved < m.stepsToMove) {
      // Stepping is handled in stepControl
      return;
    } else {
      // Moving complete
      m.motorState = MOTOR_IDLE;
      motorToMove = 0;
      m.currentPositionMM = m.targetPositionMM;

      Serial.print("Motor ");
      Serial.print(id);
      Serial.print(" now at ");
      Serial.print(m.currentPositionMM);
      Serial.println(" mm");
    }
  }
}

// --- Serial Communication ---

void processSerialCommand() {
  // Check for and read any incoming serial command (newline-terminated)
  if (Serial.available() > 0) {
    String line = Serial.readStringUntil('\n');
    line.trim(); // Remove whitespace/newline

    if (line.length() == 0) {
      return; // Ignore empty lines
    }

    // Parse the command
    int colon1 = line.indexOf(':');
    int colon2 = line.indexOf(':', colon1 + 1);
    int colon3 = line.indexOf(':', colon2 + 1);
    String cmd = (colon1 > 0) ? line.substring(0, colon1) : line;
    cmd.toUpperCase();

    // --- Command Processing ---

    // Relay control command: R:<relayId>:<state>
    if (cmd.startsWith("R") && colon1 > 0 && colon2 > colon1) {
      int relayId = line.substring(colon1 + 1, colon2).toInt();
      int state = line.substring(colon2 + 1).toInt();

      if (relayId >= 1 && relayId <= 4 && (state == 0 || state == 1)) {
        digitalWrite(relayPins[relayId - 1], state ? HIGH : LOW);
        Serial.print("Relay ");
        Serial.print(relayId);
        Serial.println(state ? " ON" : " OFF");
      } else {
        Serial.println("Invalid relay ID or state. Use R:<1-4>:<0/1>");
      }
    }

    // Motor 2 limit commands: M2:MIN:<value>, M2:MAX:<value>
    else if (cmd == "M2" && colon1 > 0) {
      String subCommand = line.substring(colon1 + 1, colon2);
      if (subCommand == "MIN" && colon2 > colon1) {
        float minLimit = line.substring(colon2 + 1).toFloat();
        motors[2].minLimitMM = minLimit;
        Serial.print("Motor 2 minLimit set to: ");
        Serial.println(minLimit);
      } else if (subCommand == "MAX" && colon2 > colon1) {
        float maxLimit = line.substring(colon2 + 1).toFloat();
        motors[2].maxLimitMM = maxLimit;
        Serial.print("Motor 2 maxLimit set to: ");
        Serial.println(maxLimit);
      } else {
        Serial.println("Invalid M2 command. Use M2:MIN:<value> or M2:MAX:<value>");
      }
    }

    // Home motor command: HOME:<id>
    else if (cmd == "HOME" && colon1 > 0) {
      int id = line.substring(colon1 + 1).toInt();
      if (id >= 1 && id <= 5) {
        motorToHome = id;
      } else {
        Serial.println("Invalid motor ID for HOME command.");
      }
    }

    // Move to position command: POS:<id>:<mm>
    else if (cmd == "POS" && colon1 > 0 && colon2 > colon1) {
      int id = line.substring(colon1 + 1, colon2).toInt();
      float pos = line.substring(colon2 + 1).toFloat();
      if (id >= 1 && id <= 5) {
        motors[id].targetPositionMM = pos;
        motorToMove = id;
      } else {
        Serial.println("Invalid motor ID for POS command.");
      }
    }

    // Loop mode command for motor 2: LOOP:2
    else if (cmd == "LOOP" && colon1 > 0) {
      int id = line.substring(colon1 + 1).toInt();
      if (id == 2) {
        motor2LoopActive = !motor2LoopActive;
        Serial.print("Motor 2 loop mode ");
        Serial.println(motor2LoopActive ? "activated." : "deactivated.");
      } else {
        Serial.println("LOOP command only supported for motor 2.");
      }
    }

    // Speed command: SPEED:<id>:<value>
    else if (cmd == "SPEED" && colon1 > 0 && colon2 > colon1) {
      int id = line.substring(colon1 + 1, colon2).toInt();
      String speedStr = line.substring(colon2 + 1);

      if (id >= 1 && id <= 5) {
        if (isNumeric(speedStr)) {
          int speedInput = speedStr.toInt();
          speedInput = constrain(speedInput, 0, 1000);

          if (speedInput == 0) {
            motors[id].currentSpeedDelay = -1; // Stop motor
            Serial.print("Motor ");
            Serial.print(id);
            Serial.println(" stopped (speed 0).");
          } else {
            const int minDelay = 1;
            const int maxDelay = 10000;
            int delayMicros = map(speedInput, 1, 1000, maxDelay, minDelay);

            motors[id].currentSpeedDelay = delayMicros;
            motors[id].lastStepTime = millis();
            Serial.print("Motor ");
            Serial.print(id);
            Serial.print(" speed set to ");
            Serial.print(speedInput);
            Serial.print(" (delay = ");
            Serial.print(delayMicros);
            Serial.println(" us).");
          }
        } else {
          Serial.println("Invalid speed value. Must be a number between 0 and 1000.");
        }
      } else {
        Serial.println("Invalid motor ID for SPEED command.");
      }
    }

    // Direction command: DIR:<id>:<0/1>
    else if (cmd == "DIR" && colon1 > 0 && colon2 > colon1) {
      int id = line.substring(colon1 + 1, colon2).toInt();
      int dir = line.substring(colon2 + 1).toInt();
      if (id >= 1 && id <= 5 && (dir == 0 || dir == 1)) {
        setMotorDirection(id, (dir == 1));
        Serial.print("Motor ");
        Serial.print(id);
        Serial.print(" direction set to ");
        Serial.println(dir == 1 ? "CW" : "CCW");
      } else {
        Serial.println("Invalid motor ID or direction for DIR command.");
      }
    }

    // Combined motor command: M:<id>:<speed>:<dir>
    else if (cmd == "M" && colon1 > 0 && colon2 > colon1 && colon3 > colon2) {
      int id = line.substring(colon1 + 1, colon2).toInt();
      String speedStr = line.substring(colon2 + 1, colon3);
      int dir = line.substring(colon3 + 1).toInt();

      if (id >= 1 && id <= 5 && (dir == 0 || dir == 1)) {
        if (isNumeric(speedStr)) {
          int speedInput = speedStr.toInt();
          speedInput = constrain(speedInput, 0, 1000);

          if (speedInput == 0) {
            motors[id].currentSpeedDelay = -1;
            Serial.print("Motor ");
            Serial.print(id);
            Serial.println(" stopped (speed 0).");
            motors[id].motorState = MOTOR_IDLE;
          } else {
            const int minDelay = 500;
            const int maxDelay = 10000;
            int delayMicros = map(speedInput, 1, 1000, maxDelay, minDelay);

            motors[id].currentSpeedDelay = delayMicros;
            motors[id].lastStepTime = millis();
            Serial.print("Motor ");
            Serial.print(id);
            Serial.print(" speed set to ");
            Serial.print(speedInput);
            Serial.print(", direction set to ");
            Serial.print(dir == 1 ? "CW" : "CCW");
            Serial.print(" (delay = ");
            Serial.print(delayMicros);
            Serial.println(" us).");

            motors[id].motorState = MOTOR_MOVING;
          }
          setMotorDirection(id, (dir == 1));
        } else {
          Serial.println("Invalid speed value. Must be a number between 0 and 1000.");
        }
      } else {
        Serial.println("Invalid motor ID or direction for M command.");
      }
    }

    // Get position command: GETPOS:<id>
    else if (cmd == "GETPOS" && colon1 > 0) {
      int id = line.substring(colon1 + 1).toInt();
      if (id >= 1 && id <= 5) {
        Serial.print("Motor ");
        Serial.print(id);
        Serial.print(" position: ");
        Serial.print(motors[id].currentPositionMM);
        Serial.println(" mm");
      } else {
        Serial.println("Invalid motor ID for GETPOS command.");
      }
    }

    // Homing speed command: HOMINGSPEED:<value>
    else if (cmd == "HOMINGSPEED" && colon1 > 0) {
      String speedStr = line.substring(colon1 + 1);
      if (isNumeric(speedStr)) {
        homingSpeed = constrain(speedStr.toInt(), 1, 100);
        Serial.print("Homing speed set to ");
        Serial.println(homingSpeed);
      } else {
        Serial.println("Invalid homing speed value. Must be a number between 1 and 100.");
      }
    }

    // Unknown command
    else {
      Serial.println("Unknown command. Use HOME, POS, LOOP, SPEED, DIR, GETPOS, HOMINGSPEED, R, or M.");
    }
  }
}

// --- Motor 2 Loop Mode ---

void handleMotor2Loop() {
  Motor &m2 = motors[2];
  if (!motor2LoopActive)
    return;

  if (!m2.isHomed) {
    motor2LoopActive = false;
    Serial.println("Motor 2 not homed – stopping loop mode.");
    return;
  }

  if (motor2LoopForward && m2.currentPositionMM >= m2.maxLimitMM) {
    motor2LoopForward = false;
    setMotorDirection(2, false);
    m2.lastStepTime = millis();
  } else if (!motor2LoopForward && m2.currentPositionMM <= m2.minLimitMM) {
    motor2LoopForward = true;
    setMotorDirection(2, true);
    m2.lastStepTime = millis();
  }
}

// --- DHT Sensor ---

// Read and display humidity and temperature from DHT
void readDHTData() {
  float h = dht.readHumidity();
  float t = dht.readTemperature();
  float f = dht.readTemperature(true);

  if (isnan(h) || isnan(t) || isnan(f)) {
    Serial.println("Failed to read from DHT sensor!");
    return;
  }

  Serial.print("Humidity: ");
  Serial.print(h);
  Serial.print("%  Temperature: ");
  Serial.print(t);
  Serial.print("°C ");
  Serial.println(f);
}

// --- Arduino Core ---

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

  // --- Motor Pin Configuration ---
  motors[1].stepPin = 3;
  motors[1].dirPin = 2;
  motors[1].limitPin = 20;
  motors[1].requiresHoming = true;
  motors[1].minLimitMM = 0.0;
  motors[1].maxLimitMM = 100.0;

  motors[2].stepPin = 5;
  motors[2].dirPin = 4;
  motors[2].limitPin = 21;
  motors[2].requiresHoming = true;
  motors[2].minLimitMM = 1.0;
  motors[2].maxLimitMM = 2.0;

  motors[3].stepPin = 7;
  motors[3].dirPin = 6;
  motors[3].limitPin = 22;
  motors[3].requiresHoming = true;
  motors[3].minLimitMM = 0.0;
  motors[3].maxLimitMM = 100.0;

  motors[4].stepPin = 12;
  motors[4].dirPin = 11;
  motors[4].limitPin = 26;
  motors[4].requiresHoming = true;
  motors[4].minLimitMM = 0.0;
  motors[4].maxLimitMM = 100.0;

  motors[5].stepPin = 8;
  motors[5].dirPin = 9;
  motors[5].limitPin = -1;
  motors[5].requiresHoming = false;
  motors[5].minLimitMM = 0.0;
  motors[5].maxLimitMM = 100.0;

  // --- Initialize Motor States ---
  for (int i = 1; i <= 5; i++) {
    initializeMotorPins(i);
    if (motors[i].requiresHoming) {
      pinMode(motors[i].limitPin, INPUT_PULLUP);
    }
    motors[i].isHomed = !motors[i].requiresHoming;
    motors[i].currentPositionMM = 0.0;
    motors[i].homingState = HOMING_NOT_HOMING;
    motors[i].motorState = MOTOR_IDLE;
    motors[i].stepPinHigh = false;
    motors[i].currentSpeedDelay = DEFAULT_SPEED_DELAY;
  }

  // --- Relay Pin Configuration ---
  for (int i = 0; i < 4; i++) {
    pinMode(relayPins[i], OUTPUT);
    digitalWrite(relayPins[i], LOW);
  }

  // --- Start Serial Communication ---
  Serial.println("Stepper control initialized. Awaiting commands...");
  Serial.println("Commands: HOME:<id>, POS:<id>:<mm>, LOOP:2, M2:MIN:<value>, M2:MAX:<value>, SPEED:<id>:<value>, DIR:<id>:<0/1>, GETPOS:<id>, HOMINGSPEED:<value>, R:<relayId>:<state>, M:<id>:<speed>:<dir>");

  // --- Start DHT Sensor ---
  dht.begin();
}

void loop() {
  processSerialCommand();

  // --- Motor Control ---
  if (motorToHome != 0) {
    homeMotor(motorToHome);
  }

  if (motorToMove != 0) {
    moveToPosition(motorToMove, motors[motorToMove].targetPositionMM);
  }

  handleMotor2Loop();

  for (int i = 1; i <= 5; i++) {
    if (motors[i].homingState != HOMING_NOT_HOMING || motors[i].motorState != MOTOR_IDLE || (i == 2 && motor2LoopActive)) {
      stepControl(i);
    }
  }

  // --- DHT Sensor Reading ---
  if (millis() - lastDHTReadTime >= dhtInterval) {
    readDHTData();
    lastDHTReadTime = millis();
  }
}