#include <AccelStepper.h>
#include <MultiStepper.h>

// Stepper motor pins
#define PUL_PIN_X 2
#define DIR_PIN_X 5
#define ENA_PIN_X 8

#define PUL_PIN_Y 3
#define DIR_PIN_Y 6
#define ENA_PIN_Y 9

#define PUL_PIN_Z 4
#define DIR_PIN_Z 7
#define ENA_PIN_Z 10

// Potentiometer pins
#define PULLING_SPEED_POT A0
#define WINDING_TENSION_POT A1
#define TRANSVERSE_POSITION_POT A2 // Sliding potentiometer

// Tension sensor pin
#define TENSION_SENSOR_PIN A3

// Manual control pins
#define MANUAL_MODE_PIN 11
#define MANUAL_FORWARD_PIN 12
#define MANUAL_REVERSE_PIN 13

// Constants
const float GEAR_RATIO = 50.0 / 13.0;
const float TRANSVERSE_STEP = 1.7; // mm per revolution

// Transverse unit limits
const int TRANSVERSE_MIN_LIMIT = 0;
const int TRANSVERSE_MAX_LIMIT = 1000; // Set to fixed values

// Stepper motor objects
AccelStepper puller(1, PUL_PIN_X, DIR_PIN_X); // Pulling stepper
AccelStepper winder(1, PUL_PIN_Y, DIR_PIN_Y); // Winding stepper
AccelStepper transverse(1, PUL_PIN_Z, DIR_PIN_Z); // Transverse stepper

MultiStepper steppers;

// State variables
bool movingForward = true; // Direction of transverse movement

void setup() {
  Serial.begin(9600);
  
  // Setup puller stepper motor
  puller.setMaxSpeed(1000);
  puller.setAcceleration(500);
  pinMode(ENA_PIN_X, OUTPUT);
  digitalWrite(ENA_PIN_X, LOW);
  
  // Setup winder stepper motor
  winder.setMaxSpeed(1000);
  winder.setAcceleration(1);
  pinMode(ENA_PIN_Y, OUTPUT);
  digitalWrite(ENA_PIN_Y, LOW);
  
  // Setup transverse stepper motor
  transverse.setMaxSpeed(1000);
  transverse.setAcceleration(500);
  pinMode(ENA_PIN_Z, OUTPUT);
  digitalWrite(ENA_PIN_Z, LOW);
  
  // Add steppers to MultiStepper
  steppers.addStepper(puller);
  steppers.addStepper(winder);
  steppers.addStepper(transverse);
  
  // Setup manual control pins
  pinMode(MANUAL_MODE_PIN, INPUT_PULLUP);
  pinMode(MANUAL_FORWARD_PIN, INPUT_PULLUP);
  pinMode(MANUAL_REVERSE_PIN, INPUT_PULLUP);
}

void loop() {
  // Read potentiometer values
  int pullingSpeed = analogRead(PULLING_SPEED_POT);
  int windingTension = analogRead(WINDING_TENSION_POT);
  int transversePositionRaw = analogRead(TRANSVERSE_POSITION_POT); // Read transverse position from sliding potentiometer
  
  // Map potentiometer values to speed
  pullingSpeed = map(pullingSpeed, 0, 1023, 0, 1000);
  windingTension = map(windingTension, 0, 1023, 0, 1000);
  int transversePosition = map(transversePositionRaw, 0, 1023, TRANSVERSE_MIN_LIMIT, TRANSVERSE_MAX_LIMIT);
  
  // Read tension sensor value
  int tension = analogRead(TENSION_SENSOR_PIN);
  
  // Read manual mode state
  bool manualMode = digitalRead(MANUAL_MODE_PIN) == HIGH;
  
  // Control pulling stepper
  puller.setSpeed(pullingSpeed);
  puller.runSpeed();
  
  // Control winding stepper based on tension
  if (tension > windingTension) {
    winder.setSpeed(-windingTension);
  } else {
    winder.setSpeed(windingTension);
  }
  winder.runSpeed();
  
  // Automatic control for transverse stepper
  if (!manualMode) {
    if (transversePosition >= TRANSVERSE_MAX_LIMIT) {
      movingForward = false;
    } else if (transversePosition <= TRANSVERSE_MIN_LIMIT) {
      movingForward = true;
    }
    
    float transverseSpeed = (winder.speed() / GEAR_RATIO) * (TRANSVERSE_STEP / 1.7);
    transverse.setSpeed(movingForward ? transverseSpeed : -transverseSpeed);
    transverse.runSpeed();
  }
  
  // Manual control for transverse stepper
  if (manualMode) {
    if (digitalRead(MANUAL_FORWARD_PIN) == LOW) {
      transverse.setSpeed(500);
    } else if (digitalRead(MANUAL_REVERSE_PIN) == LOW) {
      transverse.setSpeed(-500);
    } else {
      transverse.setSpeed(0);
    }
    transverse.runSpeed();
  }
  //Serial.print(manualMode);
}