#include <Servo.h>

// ==================== Pin Definitions ======================
const int servoPin = 9;          // Servo data pin

// Motor Control Pins
const int motorPwmPin = 3;       // PWM pin for speed control (adjust as needed)
const int motorStartStopPin = 2;  // Start/Stop pin (adjust as needed)
const int motorBrakePin = 4;     // Brake pin (adjust as needed)
const int motorDirPin = 5;      // Direction pin (adjust as needed)

// Motor Encoder Pins
const int motorEncoderA = 6;        // Encoder A pin
const int motorEncoderB = 7;        // Encoder B pin

// ==================== User Input Variables ======================
float coreDiameter;        // Diameter of the core in mm
float finalDiameter;       // Final diameter of coil in mm
float coilHeight;          // Height of the coil in mm
float wireThickness;       // Thickness of the wire in mm

// ==================== Calculated Variables ======================
int numLayers;             // Number of layers
int turnsPerLayer;         // Turns needed for each layer
int totalTurns;            // Total turns for the coil
float totalWireLength;     // Approximate total wire length
float layerSpacing;         // How much the servo must shift per revolution

// ==================== Motor and Servo Objects =====================
Servo myServo;


// ==================== Control Variables ======================
bool running = false;
bool userInput = false;
volatile long encoderCount = 0; // Counts the encoder changes
float motorSpeed = 0.1; // set the motor speed
long currentTurnCount = 0;
float currentServoPosition = 0;
// ==================== Setup Function ======================
void setup() {
    Serial.begin(115200);  // Initialize serial communication

    myServo.attach(servoPin); // Attach servo

    // Motor control pins as outputs
    pinMode(motorPwmPin, OUTPUT);
    pinMode(motorStartStopPin, OUTPUT);
    pinMode(motorBrakePin, OUTPUT);
    pinMode(motorDirPin, OUTPUT);

    // Encoder pins as inputs
    pinMode(motorEncoderA, INPUT_PULLUP);
    pinMode(motorEncoderB, INPUT_PULLUP);

    attachInterrupt(digitalPinToInterrupt(motorEncoderA), updateEncoder, CHANGE);

    // Initialize motor to stopped state
    digitalWrite(motorStartStopPin, LOW); // Make sure the motor is stopped
    digitalWrite(motorBrakePin, LOW);   // Make sure the brake is off
    analogWrite(motorPwmPin, 0);  // No initial PWM signal

    Serial.println("Ready for input: Core Diameter, Final Diameter, Height, Wire Thickness (all in mm).");
}

// ==================== Loop Function ======================
void loop() {
    if (!userInput) {
        getUserInput();
        if (userInput) {
            calculateCoilParams();
            Serial.println("Coil Parameters calculated. Ready to start winding?");
        }
    }
    if (Serial.available() > 0) {
        String command = Serial.readStringUntil('\n');
        command.trim();
        if(command.equalsIgnoreCase("start")) {
            startWinding();
            running = true;
        }
        if(command.equalsIgnoreCase("stop")) {
            running = false;
            stopWinding();
        }
    }

    if(running) {
        windingProcess();
    }

}

// ==================== Interrupt Service Routine for Encoder ======================
void updateEncoder() {
  // Detect direction using the A and B channels
  int currentA = digitalRead(motorEncoderA);
  int currentB = digitalRead(motorEncoderB);

  // Check for a change on channel A
  if(currentA != digitalRead(motorEncoderA))
  {
        if(currentA == currentB) {
          encoderCount++;
       } else {
        encoderCount--;
       }
  }
}


// ==================== Get User Input Function ======================
void getUserInput() {
    if (Serial.available() > 0) {
        String input = Serial.readStringUntil('\n');
        input.trim();
        
        // Parse comma separated values
        int commaIndex1 = input.indexOf(',');
        int commaIndex2 = input.indexOf(',', commaIndex1 + 1);
        int commaIndex3 = input.indexOf(',', commaIndex2 + 1);

        if (commaIndex1 != -1 && commaIndex2 != -1 && commaIndex3 != -1) {
            String coreStr = input.substring(0, commaIndex1);
            String finalStr = input.substring(commaIndex1 + 1, commaIndex2);
            String heightStr = input.substring(commaIndex2 + 1, commaIndex3);
            String wireStr = input.substring(commaIndex3 + 1);

            coreDiameter = coreStr.toFloat();
            finalDiameter = finalStr.toFloat();
            coilHeight = heightStr.toFloat();
            wireThickness = wireStr.toFloat();

            if (coreDiameter > 0 && finalDiameter > coreDiameter && coilHeight > 0 && wireThickness > 0)
            {
              userInput = true;
            }
            else {
                Serial.println("Invalid input. Please enter positive values for all diameters, and final diameter > core diameter.");
                
            }
        }
        else {
            Serial.println("Invalid Input format. Please enter comma-separated values: coreDiameter, finalDiameter, coilHeight, wireThickness");
        }
    }
}


// ==================== Calculate Coil Parameters Function ======================
void calculateCoilParams() {
    // Calculate number of layers based on the wire diameter. 
    numLayers = (int)ceil((finalDiameter - coreDiameter) / (2 * wireThickness));

    // Calculate how many turns are needed per layer, assuming consistent winding
    float circumference = PI * coreDiameter;  // Starting core circumference
    turnsPerLayer = (int)ceil(circumference / wireThickness);  // Turns needed based on the core

    // Total turns for coil
    totalTurns = turnsPerLayer * numLayers;
    
    // Calculate how much the servo needs to move per revolution
    layerSpacing = coilHeight / turnsPerLayer;

    // Calculate approximate wire length
    float avgDiameter = (coreDiameter + finalDiameter) / 2;
    float avgCircumference = PI * avgDiameter;
    totalWireLength = avgCircumference * totalTurns;

    Serial.print("Calculated Layers: ");
    Serial.println(numLayers);
    Serial.print("Calculated Turns per Layer: ");
    Serial.println(turnsPerLayer);
    Serial.print("Calculated Total Turns: ");
    Serial.println(totalTurns);
    Serial.print("Estimated Wire Length: ");
    Serial.print(totalWireLength);
    Serial.println(" mm");
     Serial.print("Layer Spacing: ");
     Serial.print(layerSpacing);
    Serial.println(" mm");
}

// ==================== Start Winding Function ======================
void startWinding() {
    Serial.println("Winding started.");
     myServo.write(0); // Start servo on one side 
     currentTurnCount = 0;
     encoderCount = 0;
     currentServoPosition = 0;

    // Start the motor
    digitalWrite(motorDirPin, HIGH); // Set initial direction
    digitalWrite(motorStartStopPin, HIGH); // Start the motor
    digitalWrite(motorBrakePin, LOW); // make sure the brake is off
    analogWrite(motorPwmPin, map(motorSpeed * 100, 0, 100, 0, 255)); // Set an initial speed
}

void stopWinding() {
    digitalWrite(motorStartStopPin, LOW);
    digitalWrite(motorBrakePin, HIGH);
    analogWrite(motorPwmPin, 0);  // Stop PWM
    Serial.println("Winding Stopped.");
}

// ==================== Winding Process Function ======================
void windingProcess() {
    if (currentTurnCount < totalTurns) {
        float rotationsDone = (float)encoderCount / 1440; // Assuming 1440 CPR
      
      if (rotationsDone >= (float)(currentTurnCount + 1)/turnsPerLayer)
      {
          currentTurnCount++;
        currentServoPosition += layerSpacing; // move servo the distance of one turn's height
          myServo.write(currentServoPosition);
           Serial.print("Turns done: ");
          Serial.println(currentTurnCount);
      }

    } else {
        Serial.println("Winding finished");
        stopWinding();
        running = false;
    }
}