#define module_driver   
#include "stepdance.hpp"  

// -- Pin Definitions --
#define LIMIT_A_PIN 29  
#define LIMIT_B_PIN 30  
#define LIMIT_PRESSED_STATE HIGH 

// -- CALIBRATION --
#define UNIT_INCH 1
#define UNIT_STEPS 2030

// -- SPEED --
#define TRAVEL_SPEED 2.5       
#define HOMING_SPEED 2.5       

#define SERVO_POS_UP 1000
#define SERVO_POS_DOWN -1000
#define SERVO_SPEED 5000

#define STEPPER_MOVE 1000
#define RELATIVE 0    
#define ABSOLUTE 1    

// -- ADJUSTMENT FACTORS --
float AdjX = -2.4;
float AdjY = -3.2;

// -- COORDINATES --
float Ax = 2.1596; float Ay = 9.7417;
float Bx = 3.2296; float By = 9.7417;
float Cx = 4.4496; float Cy = 9.7417;
float Dx = 15.4796; float Dy = 9.9682;
float Ex = 16.6796; float Ey = 9.9682;
float Fx = 17.8796; float Fy = 9.9682;

float Gx = 2.6503; float Gy = 10.9502;
float Hx = 3.8967; float Hy = 10.9502;
float Ix = 5.2503; float Iy = 10.9502;
float Jx = 14.1191; float Jy = 10.8464;
float Kx = 15.5323; float Ky = 10.8464;
float Lx = 16.8576; float Ly = 10.8464;

float Mx = 3.0878; float My = 12.3292;
float Nx = 4.5346; float Ny = 12.3292;
float Ox = 5.9370; float Oy = 12.3292;
float Px = 7.3649; float Py = 12.3292;
float Qx = 12.8076; float Qy = 12.1758;
float Rx = 14.4489; float Ry = 12.1758;
float Sx = 15.7554; float Sy = 12.1758;
float Tx = 17.0038; float Ty = 12.1758;

float Ux = 3.7247; float Uy = 14.0103;
float Vx = 5.1127; float Vy = 14.0103;
float Wx = 6.7018; float Wy = 14.0103;
float Xx = 12.4600; float Xy = 13.8189;
float Yx = 14.0432; float Yy = 13.8189;
float Zx = 15.6766; float Zy = 13.8189;

float N1x = 7.2501; float N1y = 15.4912;
float N2x = 7.9187; float N2y = 15.4912;
float N3x = 8.6042; float N3y = 15.4912;
float N4x = 9.3628; float N4y = 15.4912;
float N5x = 10.0243; float N5y = 15.4912;
float N6x = 10.7655; float N6y = 15.4912;
float N7x = 11.3700; float N7y = 15.4912;
float N8x = 12.0963; float N8y = 15.4912;
float N9x = 12.7963; float N9y = 15.4912;

float YESx = 4.7725; float YESy = 5.7117;
float NOx = 15.0954; float NOy = 5.7117;
float GOODBYEx = 10.1661; float GOODBYEy = 17.0957;

OutputPort output_a; OutputPort output_b; OutputPort output_c; OutputPort output_d;
Channel channel_a; Channel channel_b; Channel channel_c;
Channel channel_w; Channel channel_x; Channel channel_y; Channel channel_z; 
PositionGenerator motor_a_gen; PositionGenerator motor_b_gen; PositionGenerator motor_c_gen;
PositionGenerator motor_w_gen; PositionGenerator motor_x_gen; PositionGenerator motor_y_gen; PositionGenerator motor_z_gen;

KinematicsCoreXY axidraw_kinematics;
Homing homing;
TimeBasedInterpolator tbi;
WaveGenerator2D wave_gen;
Vector2DToAngle vec2angle;
AnalogInput analog_a1;

int homing_state = 0; 
bool is_homed = false; 

// --- MOVE FUNCTION ---
void moveTo(float x, float y) {
  /**/
  // if (!is_homed) {
  //   Serial.println("ERROR: Type 'h' first.");
  //   return;
  // }
  // float target_a = x + y;
  // float target_b = x - y;

  tbi.add_move(GLOBAL, TRAVEL_SPEED, x, y, 0, 0, 0, 0);
  tbi.add_timed_move(GLOBAL, 1, x, y, 0, 0, 0, 0);

  Serial.print("Moving to: "); Serial.print(x); Serial.print(", "); Serial.println(y);
  // motor_a_gen.go(target_a, ABSOLUTE, TRAVEL_SPEED);
  // motor_b_gen.go(target_b, ABSOLUTE, TRAVEL_SPEED);
}

// --- Finish Homing ---
// void finishHoming() {
//   Serial.println("Homing Complete");
//   motor_a_gen.go(0, RELATIVE, 0);
//   motor_b_gen.go(0, RELATIVE, 0);

//   motor_a_gen.begin();
//   motor_b_gen.begin();
  
//   homing_state = 0; 
//   is_homed = true;
// }

void init_homing() {
  Serial.println("Init homing");
  homing.add_axis(
  // LIMIT_A,
  LIMIT_A_PIN,
  0, // coordinate value we want to set at the limit switch
  HOMING_DIR_BWD,
  HOMING_SPEED, // speed at which we move to find the limit 
  &axidraw_kinematics.input_x);

  homing.add_axis(
  // LIMIT_B,
  LIMIT_B_PIN,
  0, // coordinate value we want to set at the limit switch
  HOMING_DIR_BWD,
  HOMING_SPEED, // speed at which we move to find the limit 
  &axidraw_kinematics.input_y);

  homing.begin();
}

void start_homing() {

  homing.start_homing_routine();
  Serial.println("start homing");
  is_homed = true;
    
}

void setup() {
  Serial.begin(9600);
  while (!Serial) { ; }

  pinMode(LIMIT_A_PIN, INPUT_PULLUP);
  pinMode(LIMIT_B_PIN, INPUT_PULLUP);

  output_a.begin(OUTPUT_A); output_b.begin(OUTPUT_B); output_c.begin(OUTPUT_C); output_d.begin(OUTPUT_D);
  enable_drivers();

  // -- Local Channels --
  channel_a.begin(&output_a, SIGNAL_E); channel_a.set_ratio(UNIT_INCH, UNIT_STEPS); 
  channel_b.begin(&output_b, SIGNAL_E); channel_b.set_ratio(UNIT_INCH, UNIT_STEPS);
  channel_c.begin(&output_c, SIGNAL_E); channel_c.set_ratio(1, 1); 
  
  // -- Remote Channels (Output D) --
  // Mapping W, X, Y, Z to signals X, Y, Z, E
  channel_w.begin(&output_d, SIGNAL_X); channel_w.set_ratio(1, 1);
  channel_x.begin(&output_d, SIGNAL_Y); channel_x.set_ratio(1, 1);
  channel_y.begin(&output_d, SIGNAL_Z); channel_y.set_ratio(1, 1); // Signal Z goes to remote Port C
  channel_z.begin(&output_d, SIGNAL_E); channel_z.set_ratio(1, 1);

  // motor_a_gen.output.map(&channel_a.input_target_position); motor_a_gen.begin();
  // motor_b_gen.output.map(&channel_b.input_target_position); motor_b_gen.begin();
  motor_c_gen.output.map(&channel_c.input_target_position); motor_c_gen.begin();

  motor_w_gen.output.map(&channel_w.input_target_position); motor_w_gen.begin();
  motor_x_gen.output.map(&channel_x.input_target_position); motor_x_gen.begin();
  motor_y_gen.output.map(&channel_y.input_target_position); motor_y_gen.begin();
  motor_z_gen.output.map(&channel_z.input_target_position); motor_z_gen.begin();


  axidraw_kinematics.begin();
  axidraw_kinematics.output_a.map(&channel_a.input_target_position);
  axidraw_kinematics.output_b.map(&channel_b.input_target_position);


  tbi.begin();
  tbi.output_x.map(&axidraw_kinematics.input_x);
  tbi.output_y.map(&axidraw_kinematics.input_y);



  vec2angle.input_x.map(&tbi.output_x);
  vec2angle.input_y.map(&tbi.output_y);
  vec2angle.output_theta.map(&wave_gen.input_theta, ABSOLUTE);
  vec2angle.begin();

  wave_gen.output_x.map(&axidraw_kinematics.input_x);
  wave_gen.output_y.map(&axidraw_kinematics.input_y);
  wave_gen.begin();

    // pedal
  analog_a1.set_floor(0, 25);
  analog_a1.set_ceiling(0.5, 1020); //radians per second
  analog_a1.map(&wave_gen.amplitude);
  analog_a1.begin(IO_A1);

  // init_homing();


  // wave_gen.amplitude = 1;




  // pedal
  // analog_a1.set_floor(0, 25);
  // analog_a1.set_ceiling(0.5, 1020); //radians per second
  // analog_a1.map(&wave_gen.amplitude);
  // analog_a1.begin(IO_A1);

  dance_start();
  
  Serial.println("Type 'h' to HOME.");
}

void loop() {
  if (Serial.available() > 0) {
    String cmd = Serial.readStringUntil('\n');
    cmd.trim(); 
    if (cmd.length() == 0) return; 

    if (cmd == "h") {
       Serial.println("Homing...");
       start_homing();
      //  is_homed = false; 
      //  homing_state = 1; 
      //  motor_a_gen.go(-STEPPER_MOVE, RELATIVE, HOMING_SPEED);
      //  motor_b_gen.go(0, RELATIVE, HOMING_SPEED);
       return; 
    }
    // if (cmd == "S") {
    //   Serial.println("STOP");
    //   motor_a_gen.go(0, RELATIVE, 0);
    //   motor_b_gen.go(0, RELATIVE, 0);
    //   homing_state = 0;
    //   return;
    // }
    
    if (cmd == "OFF") {
      Serial.println("CANDLES");
      motor_c_gen.go(SERVO_POS_UP, ABSOLUTE, SERVO_SPEED); // Local C
      motor_w_gen.go(SERVO_POS_UP, ABSOLUTE, SERVO_SPEED); // Remote A
      motor_x_gen.go(SERVO_POS_UP, ABSOLUTE, SERVO_SPEED); // Remote B
      motor_y_gen.go(SERVO_POS_UP, ABSOLUTE, SERVO_SPEED); // Remote C
      motor_z_gen.go(SERVO_POS_UP, ABSOLUTE, SERVO_SPEED); // Remote D
      delay(300);

      motor_c_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED); // Local C
      motor_w_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED); // Remote A
      motor_x_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED); // Remote B
      motor_y_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED); // Remote C
      motor_z_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED); // Remote D
      delay(300);

      motor_c_gen.go(SERVO_POS_UP, ABSOLUTE, SERVO_SPEED); // Local C
      motor_w_gen.go(SERVO_POS_UP, ABSOLUTE, SERVO_SPEED); // Remote A
      motor_x_gen.go(SERVO_POS_UP, ABSOLUTE, SERVO_SPEED); // Remote B
      motor_y_gen.go(SERVO_POS_UP, ABSOLUTE, SERVO_SPEED); // Remote C
      motor_z_gen.go(SERVO_POS_UP, ABSOLUTE, SERVO_SPEED); // Remote D
      delay(300);

      motor_c_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED); // Local C
      motor_w_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED); // Remote A
      motor_x_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED); // Remote B
      motor_y_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED); // Remote C
      motor_z_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED); // Remote D
      delay(300);

      motor_c_gen.go(SERVO_POS_UP, ABSOLUTE, SERVO_SPEED); // Local C
      motor_w_gen.go(SERVO_POS_UP, ABSOLUTE, SERVO_SPEED); // Remote A
      motor_x_gen.go(SERVO_POS_UP, ABSOLUTE, SERVO_SPEED); // Remote B
      motor_y_gen.go(SERVO_POS_UP, ABSOLUTE, SERVO_SPEED); // Remote C
      motor_z_gen.go(SERVO_POS_UP, ABSOLUTE, SERVO_SPEED); // Remote D
      delay(300);

      motor_c_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED); // Local C
      motor_w_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED); // Remote A
      motor_x_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED); // Remote B
      motor_y_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED); // Remote C
      motor_z_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED); // Remote D
      delay(300);

      motor_c_gen.go(SERVO_POS_UP, ABSOLUTE, SERVO_SPEED); // Local C
      motor_w_gen.go(SERVO_POS_UP, ABSOLUTE, SERVO_SPEED); // Remote A
      motor_x_gen.go(SERVO_POS_UP, ABSOLUTE, SERVO_SPEED); // Remote B
      motor_y_gen.go(SERVO_POS_UP, ABSOLUTE, SERVO_SPEED); // Remote C
      motor_z_gen.go(SERVO_POS_UP, ABSOLUTE, SERVO_SPEED); // Remote D
      delay(300);

      motor_c_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED); // Local C
      motor_w_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED); // Remote A
      motor_x_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED); // Remote B
      motor_y_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED); // Remote C
      motor_z_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED); // Remote D
      delay(300);
      
      return;
    }

    if (cmd == "DOWN") {
      Serial.println("CANDLES DOWN");
      motor_c_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED);
      motor_w_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED);
      motor_x_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED);
      motor_y_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED);
      motor_z_gen.go(SERVO_POS_DOWN, ABSOLUTE, SERVO_SPEED);
      return;
    }

    if (homing_state == 0) {
      if (cmd == "TEST") moveTo(0.5 + AdjX, 0.5 + AdjY);
      else if (cmd == "A") moveTo(Ax + AdjX, Ay + AdjY);
      else if (cmd == "B") moveTo(Bx + AdjX, By + AdjY);
      else if (cmd == "C") moveTo(Cx + AdjX, Cy + AdjY);
      else if (cmd == "D") moveTo(Dx + AdjX, Dy + AdjY);
      else if (cmd == "E") moveTo(Ex + AdjX, Ey + AdjY);
      else if (cmd == "F") moveTo(Fx + AdjX, Fy + AdjY);
      else if (cmd == "G") moveTo(Gx + AdjX, Gy + AdjY);
      else if (cmd == "H") moveTo(Hx + AdjX, Hy + AdjY);
      else if (cmd == "I") moveTo(Ix + AdjX, Iy + AdjY);
      else if (cmd == "J") moveTo(Jx + AdjX, Jy + AdjY);
      else if (cmd == "K") moveTo(Kx + AdjX, Ky + AdjY);
      else if (cmd == "L") moveTo(Lx + AdjX, Ly + AdjY);
      else if (cmd == "M") moveTo(Mx + AdjX, My + AdjY);
      else if (cmd == "N") moveTo(Nx + AdjX, Ny + AdjY);
      else if (cmd == "O") moveTo(Ox + AdjX, Oy + AdjY);
      else if (cmd == "P") moveTo(Px + AdjX, Py + AdjY);
      else if (cmd == "Q") moveTo(Qx + AdjX, Qy + AdjY);
      else if (cmd == "R") moveTo(Rx + AdjX, Ry + AdjY);
      else if (cmd == "S") moveTo(Sx + AdjX, Sy + AdjY);
      else if (cmd == "T") moveTo(Tx + AdjX, Ty + AdjY);
      else if (cmd == "U") moveTo(Ux + AdjX, Uy + AdjY);
      else if (cmd == "V") moveTo(Vx + AdjX, Vy + AdjY);
      else if (cmd == "W") moveTo(Wx + AdjX, Wy + AdjY);
      else if (cmd == "X") moveTo(Xx + AdjX, Xy + AdjY);
      else if (cmd == "Y") moveTo(Yx + AdjX, Yy + AdjY);
      else if (cmd == "Z") moveTo(Zx + AdjX, Zy + AdjY);
      else if (cmd == "1") moveTo(N1x + AdjX, N1y + AdjY);
      else if (cmd == "2") moveTo(N2x + AdjX, N2y + AdjY);
      else if (cmd == "3") moveTo(N3x + AdjX, N3y + AdjY);
      else if (cmd == "4") moveTo(N4x + AdjX, N4y + AdjY);
      else if (cmd == "5") moveTo(N5x + AdjX, N5y + AdjY);
      else if (cmd == "6") moveTo(N6x + AdjX, N6y + AdjY);
      else if (cmd == "7") moveTo(N7x + AdjX, N7y + AdjY);
      else if (cmd == "8") moveTo(N8x + AdjX, N8y + AdjY);
      else if (cmd == "9") moveTo(N9x + AdjX, N9y + AdjY);
      else if (cmd == "YES") moveTo(YESx + AdjX, YESy + AdjY);
      else if (cmd == "NO") moveTo(NOx + AdjX, NOy + AdjY);
      else if (cmd == "GOODBYE") moveTo(GOODBYEx + AdjX, GOODBYEy + AdjY);
      else Serial.println("Unknown Command");
    }
  } 

  // if (homing_state == 1) {
  //   if (digitalRead(LIMIT_A_PIN) == LIMIT_PRESSED_STATE) {
  //     motor_a_gen.go(-STEPPER_MOVE, RELATIVE, HOMING_SPEED);
  //     motor_b_gen.go(STEPPER_MOVE, RELATIVE, HOMING_SPEED); 
  //     homing_state = 2; 
  //   } else if (digitalRead(LIMIT_B_PIN) == LIMIT_PRESSED_STATE) {
  //     motor_a_gen.go(-STEPPER_MOVE, RELATIVE, HOMING_SPEED);
  //     motor_b_gen.go(-STEPPER_MOVE, RELATIVE, HOMING_SPEED); 
  //     homing_state = 3; 
  //   }
  // }
  // else if (homing_state == 2) {
  //   if (digitalRead(LIMIT_B_PIN) == LIMIT_PRESSED_STATE) finishHoming();
  // }
  // else if (homing_state == 3) {
  //   if (digitalRead(LIMIT_A_PIN) == LIMIT_PRESSED_STATE) finishHoming();
  // }

  dance_loop(); 
}