#include <AccelStepper.h>
#include <SafeString.h>
#include <Servo.h>
#include <loopTimer.h>
#include <BufferedOutput.h>
#include "Stepper.cpp"

#define MotorInterfaceType 4


#define X_STEP_PIN 2
#define X_DIR_PIN 5

#define Y_STEP_PIN 3
#define Y_DIR_PIN 6


#define Z_STEP_PIN 23
#define Z_DIR_PIN 25
#define Z_STEP_PIN1 27
#define Z_STEP_PIN2 29
//                                              IN1         IN3         IN2          IN4
//                                               23          27          25           29
//AccelStepper stepperZ(MotorInterfaceType, Z_STEP_PIN, Z_STEP_PIN1, Z_DIR_PIN, Z_STEP_PIN2);



#define X_MIN_PIN 9
#define Y_MIN_PIN 10
#define Z_MIN_PIN 11
#define SW_PIN 14
#define X_PIN A15 
#define Y_PIN A14


createBufferedOutput(bufferedOut, 80, DROP_UNTIL_EMPTY);

const int CALIBRATION_STEP_SPEED = 100;
const int MIN_SPEED = 0; 
const int MAX_SPEED = 4000; 
int joyX = 0, joyY = 0;
bool joyPressed = false;




// Speed control
const int SLOW_SPEED = 500;
const int MEDIUM_SPEED = 1000;
const int FAST_SPEED = 4000;
int currentSpeedMode = MEDIUM_SPEED;
int currentMoveDistance = 100; 
const int deadZone = 6;


millisDelay joyStickDelay;
millisDelay stepperMotorDelay;

Stepper stepperX(X_STEP_PIN, X_DIR_PIN);
Stepper stepperY(Y_STEP_PIN, Y_DIR_PIN);
AccelStepper stepperZ(MotorInterfaceType, Z_STEP_PIN, Z_STEP_PIN1, Z_DIR_PIN, Z_STEP_PIN2);


void setup() {
    Serial.begin(115200);
    Serial.println("AutoScope System Initializing");
    // bufferedOut.connect(Serial);

    pinMode(X_MIN_PIN, INPUT_PULLUP);
    pinMode(Y_MIN_PIN, INPUT_PULLUP);
    pinMode(Z_MIN_PIN, INPUT_PULLUP);
    pinMode(SW_PIN, INPUT_PULLUP);

    stepperX.init();
    stepperX.setMaxSpeed(MAX_SPEED);
    stepperX.setSpeed(MEDIUM_SPEED);
    stepperY.init();
    stepperY.setMaxSpeed(MAX_SPEED);
    stepperY.setSpeed(MEDIUM_SPEED);

    // // stepperMotorDelay.start(25);
    joyStickDelay.start(50);//update joystick pos every 50 ms
    // Serial.println("Autoscope System Ready!");


    stepperZ.setMaxSpeed(MAX_SPEED);
    stepperZ.setAcceleration(200.0);
    stepperZ.setSpeed(MEDIUM_SPEED);
}

void loop() {
  bufferedOut.nextByteOut(); 
  getCurrentJoystickPos();
  runStepper();

  // // char c = readChar();
  // // processChar(c);
  // if (!stepperX.isRunning())  
  //     stepperX.disableOutputs();
  // else
  //     stepperX.enableOutputs();
  // if (!stepperY.isRunning())  
  //     stepperY.disableOutputs();
  // else
  //     stepperY.enableOutputs();
  // if (!stepperZ.isRunning())  
  //     stepperZ.disableOutputs();
  // else
  //     stepperZ.enableOutputs();
}


void runStepper(){
    loopTimer.check(bufferedOut);
    stepperX.run();
    stepperZ.run();
    stepperY.run();
}


void getCurrentPositionCM(float &x, float &y, float &z) {
    x = stepperX.currentPosition();
    y = stepperY.currentPosition();
    z = stepperZ.currentPosition();
}


void printCurrentPosition() {
    float x, y, z;
    getCurrentPositionCM(x, y, z);
    Serial.print("Current Position - X: "); Serial.print(x);
    Serial.print(", Y: "); Serial.print(y);
    Serial.print(", Z: "); Serial.print(z); Serial.println("");
}



void getCurrentJoystickPos() {
    // if(joyStickDelay.justFinished()){
        // joyStickDelay.repeat();
        bool locJoyPressed = !digitalRead(SW_PIN);
        int locJoyX = mapJoystickValue(analogRead(X_PIN));
        int locJoyY = mapJoystickValue(analogRead(Y_PIN));
        bool zPressedChanged = false, xChanged = false, yChanged = false;
        if(locJoyPressed != joyPressed){
            zPressedChanged = true;
        }
        if(locJoyX != joyX && abs(locJoyX) > deadZone){
            xChanged = true;
        }
        if(locJoyY != joyY && abs(locJoyY) > deadZone){
            yChanged = true;
        }
        joyX = locJoyX;
        joyY = locJoyY;
        joyPressed = locJoyPressed;

        if(zPressedChanged || xChanged || yChanged){
          joySteppersUpdate(xChanged, yChanged, zPressedChanged);
        }else {
          joySteppersContinue();
        }
    // }
}


void joySteppersUpdate(bool xChanged, bool yChanged, bool zBtnChanged) {
    if(zBtnChanged && joyPressed){
        stepperX.setSpeed(0);
        stepperY.setSpeed(0);
        stopX();
        stopY();
    }else if(zBtnChanged){
        stepperZ.setSpeed(0);
        stopZ();
    }

    Serial.print("Pressed: "); Serial.print(joyPressed);
    Serial.print(" joyY: "); Serial.print(joyY);
    Serial.print(" joyX: "); Serial.println(joyX);
    
    if(joyPressed){
        // Control Z axis with joystick Y
        if(abs(joyY) > deadZone){
            float zSpeed = mapJoystickToSpeed((float)joyY);
            stepperZ.setSpeed(zSpeed);  // Positive joyY = positive speed, negative joyY = negative speed
            stepperZ.runSpeed();
        } else {
            stopZ();
        }
    } else {
        // Control X/Y axes
        if(abs(joyX) > deadZone){
            stepperX.setSpeed(mapJoystickToSpeed((float)joyX));
            if(joyX > 0)
                stepperX.setDirection(1);
            else
                stepperX.setDirection(-1);
            stepperX.runSpeed();
        } else {
            stopX();
        }
        
        if(abs(joyY) > deadZone){
            stepperY.setSpeed(mapJoystickToSpeed((float)joyY));
            if(joyY > 0)
                stepperY.setDirection(1);
            else
                stepperY.setDirection(-1);
            stepperY.runSpeed();
        } else {
            stopY();
        }
    }
}

void joySteppersContinue() {
    if(joyPressed){
        stepperX.setSpeed(0);
        stepperY.setSpeed(0);
        stopX();
        stopY();
        stepperZ.setSpeed(MAX_SPEED / 4);
    }else{
        // stepperX.setSpeed(MAX_SPEED / 4);
        // stepperY.setSpeed(MAX_SPEED / 4);
        stepperZ.setSpeed(0);
        stopZ();
    }

    Serial.print("joySteppersContinue Pressed: "); Serial.print(joyPressed);Serial.print(" joyY: "); Serial.print(joyY);Serial.print("joyX: "); Serial.println(joyX);
    if(joyPressed){
        // if(yChanged){
        //     stepperZ.setSpeed(-1 * ((joyY / 100.0) * MAX_SPEED));
        // }
        // stepperZ.setSpeed(-1 * ((joyY / 100.0) * MAX_SPEED));
        if(abs(joyY) > deadZone){
            float zSpeed = mapJoystickToSpeed((float)joyY);
            stepperZ.setSpeed(zSpeed);  
            stepperZ.runSpeed();
        }else {
            stopZ();
        }
    }else { 
        if(abs(joyY) > deadZone){
          if(joyY > 0)
            stepperY.setDirection(1);
          else
            stepperY.setDirection(-1);
          stepperY.runSpeed();
        }else {
            stopY();
            // Serial.println("StopY");
        }
        if(abs(joyX) > deadZone){
          if(joyX > 0)
            stepperX.setDirection(1);
          else
            stepperX.setDirection(-1);
          stepperX.runSpeed();
        }else {
            stopX();
            // Serial.println("StopX");
        }
    }
}

void stopZ(){
    stepperZ.stop();
}
void stopX(){
    stepperX.stop();
}
void stopY(){
    stepperY.stop();
}

int mapJoystickValue(int potValue) {
    return map(potValue, 0, 1024, -100, 100);
}
float mapJoystickToSpeed(float joystickValue) {
    // Apply dead zone
    if (abs(joystickValue) <= 6) {
        return 0.0;
    }
    
    // Determine direction and work with positive values
    bool isNegative = joystickValue < 0;
    float absValue = abs(joystickValue);
    
    // Normalize to 0.0 to 1.0 (excluding dead zone)
    float normalized = (float)(absValue - 6) / (100 - 6);
    
    // Apply power curve (try values between 1.5 and 3.0)
    float exponent = 2.0;  // Start with this, adjust to taste
    float curved = pow(normalized, exponent);
    // Apply direction and max speed
    if (isNegative) {
        return -curved * MAX_SPEED;
    } else {
        return curved * MAX_SPEED;
    }
}


char readChar() {
  char c = 0;
  if (Serial.available()) {
    c = Serial.read();
    while (Serial.available()) {
      Serial.read();
    }
  }
  return c;
}

void processChar(char c) {
  if (c == 0) { // usual 
     return;
  }
  
  // MOVEMENT COMMANDS
  if (c == 'N') {        // Move North (Y+)7
    moveDirection(0, 1, 0);
  } else if (c == 'S') { // Move South (Y-)
    moveDirection(0, -1, 0);
  } else if (c == 'E') { // Move East (X+)
    moveDirection(1, 0, 0);
  } else if (c == 'W') { // Move West (X-)
    moveDirection(-1, 0, 0);
  } else if (c == 'U') { // Move Up (Z+)
    moveDirection(0, 0, 1);
  } else if (c == 'D') { // Move Down (Z-)
    moveDirection(0, 0, -1);
  } else if (c == 'X') { // Stop all movement
    stopAllSteppers();
    
  // HOMING COMMANDS
  } else if (c == 'H') { // Home all axes
    homeAllAxes();
  } else if (c == 'h') { // Home current axis only
    homeCurrentAxis();
    
  // POSITION COMMANDS
  } else if (c == 'P') { // Print current position
    printCurrentPosition();
  } else if (c == 'Z') { // Zero/reset position counters
    zeroPosition();
    
  // SPEED CONTROL
  } else if (c == 'F') { // Fast speed mode
    setSpeedMode(FAST_SPEED);
  } else if (c == 'M') { // Medium speed mode  
    setSpeedMode(MEDIUM_SPEED);
  } else if (c == 'L') { // Low speed mode
    setSpeedMode(SLOW_SPEED);
    
  // SCANNING COMMANDS
  } else if (c == 'G') { // Go to origin (0,0,0)
    goToOrigin();
  } else if (c == 'R') { // Ready status check
    reportStatus();
    
  // CALIBRATION/SETUP
  } else if (c == 'C') { // Calibrate/find limits
    calibrateAxes();
  } else if (c == 'I') { // Initialize system
    initializeSystem();
    
  // EMERGENCY
  } else if (c == '!') { // Emergency stop
    emergencyStop();
    
  } else if ((c == '\n') || (c == '\r')) {
    // skip end of line chars
  } else if (c != 0) {
    Serial.print("Invalid cmd:"); Serial.println(c);
  }
}

// Movement function implementations
void moveDirection(int xDir, int yDir, int zDir) {
  int moveDistance = getCurrentMoveDistance(); // Based on current speed mode
  
  if (xDir != 0) {
    stepperX.moveTo(stepperX.currentPosition() + (xDir * moveDistance));
  }
  if (yDir != 0) {
    stepperY.moveTo(stepperY.currentPosition() + (yDir * moveDistance));
  }
  if (zDir != 0) {
    stepperZ.moveTo(stepperZ.currentPosition() + (zDir * moveDistance));
  }
}

void homeCurrentAxis() {

}


void stopAllSteppers() {
  stepperX.stop();
  stepperY.stop(); 
  stepperZ.stop();
  Serial.println("STOPPED");
}

void homeAllAxes() {
  Serial.println("HOMING_START");
  // Home X axis
  homeAxis(stepperX, X_MIN_PIN, -1);
  // Home Y axis  
  homeAxis(stepperY, Y_MIN_PIN, -1);
  // Home Z axis
  homeAxisZ(stepperZ, Z_MIN_PIN, -1);
  Serial.println("HOMING_COMPLETE");
}

void homeAxis(Stepper &stepper, int limitPin, int direction) {
  stepper.setSpeed(CALIBRATION_STEP_SPEED * direction);
  while (digitalRead(limitPin) == HIGH) {
    stepper.runSpeed();
  }
  stepper.setCurrentPosition(0);
  stepper.stop();
}

void homeAxisZ(AccelStepper &stepper, int limitPin, int direction) {
  stepper.setSpeed(CALIBRATION_STEP_SPEED * direction);
  while (digitalRead(limitPin) == HIGH) {
    stepper.runSpeed();
  }
  stepper.setCurrentPosition(0);
  stepper.stop();
}

void goToOrigin() {
  Serial.println("GOTO_ORIGIN");
  stepperX.moveTo(0);
  stepperY.moveTo(0);
  stepperZ.moveTo(0);
}

void zeroPosition() {
  stepperX.setCurrentPosition(0);
  stepperY.setCurrentPosition(0);
  stepperZ.setCurrentPosition(0);
  Serial.println("POSITION_ZEROED");
}

void setSpeedMode(int speed) {
  currentSpeedMode = speed;
  stepperX.setMaxSpeed(speed);
  stepperY.setMaxSpeed(speed);
  stepperZ.setMaxSpeed(speed);
  
  if (speed == SLOW_SPEED) {
    currentMoveDistance = 50;  
    Serial.println("SPEED_SLOW");
  } else if (speed == MEDIUM_SPEED) {
    currentMoveDistance = 100; 
    Serial.println("SPEED_MEDIUM");
  } else if (speed == FAST_SPEED) {
    currentMoveDistance = 200;  
    Serial.println("SPEED_FAST");
  }
}

int getCurrentMoveDistance() {
  return currentMoveDistance;
}


void reportStatus() {
  Serial.print("STATUS:");
  Serial.print("X:"); Serial.print(stepperX.currentPosition());
  Serial.print(",Y:"); Serial.print(stepperY.currentPosition());  
  Serial.print(",Z:"); Serial.print(stepperZ.currentPosition());
  Serial.print(",RUNNING:"); 
  Serial.print(stepperX.isRunning() || stepperY.isRunning() || stepperZ.isRunning());
  Serial.println();
}

void calibrateAxes() {
  Serial.println("CALIBRATE_START");
  homeAllAxes();
  // Could add max limit finding here too
  Serial.println("CALIBRATE_COMPLETE");
}

void initializeSystem() {
  Serial.println("INIT_START");
  zeroPosition();
  setSpeedMode(MEDIUM_SPEED);
  Serial.println("INIT_COMPLETE");
}

void emergencyStop() {
  // Immediately stop all motors
  stepperX.stop();
  stepperY.stop();
  stepperZ.stop();
  stepperX.disableOutputs();
  stepperY.disableOutputs(); 
  stepperZ.disableOutputs();
  Serial.println("EMERGENCY_STOP");
}