#include <Adafruit_MotorShield.h>
#include <Adafruit_NeoPixel.h>

Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_StepperMotor *L = AFMS.getStepper(200, 1); // left motor
Adafruit_StepperMotor *R = AFMS.getStepper(200, 2); // right motor
Adafruit_NeoPixel pixel(1, PIN_NEOPIXEL, NEO_GRB + NEO_KHZ800);

void LED_red()     { pixel.setPixelColor(0, 255,   0,   0); pixel.show(); }
void LED_green()   { pixel.setPixelColor(0,   0, 255,   0); pixel.show(); }
void LED_blue()    { pixel.setPixelColor(0,   0,   0, 255); pixel.show(); }
void LED_yellow()  { pixel.setPixelColor(0, 255, 255,   0); pixel.show(); }
void LED_cyan()    { pixel.setPixelColor(0,   0, 255, 255); pixel.show(); }
void LED_magenta() { pixel.setPixelColor(0, 255,   0, 255); pixel.show(); }

void setup() {
  pixel.begin();
  
  if (!AFMS.begin()) { // failed to connect
    LED_red();
    while (1);
  }
  
  L->setSpeed(5);
  R->setSpeed(5);
}

void loop() {
  LED_green();
  L->step(50, FORWARD, INTERLEAVE);
  LED_cyan();
  L->step(50, BACKWARD, INTERLEAVE);
  LED_blue();
  R->step(50, FORWARD, INTERLEAVE);
  LED_magenta();
  R->step(50, BACKWARD, INTERLEAVE);
}

//        45      (constraint p4 = p5)
//       .  .
//     .      . 
//    2        3
//    .        .
//    .        .
//    0        1
float x0 = -0.4;
float y0 = 0.0;
float x1 = 0.4;
float y1 = 0.0;
float L0 = 0.6;
float L1 = 0.6;
float L2 = 1.0;
float L3 = 1.0;
float phi0;
float phi1;
float phi2;
float phi3;
float x2() { return x0 + L0 * cos(phi0); }
float y2() { return y0 + L0 * sin(phi0); }
float x3() { return x1 + L1 * cos(phi1); }
float y3() { return y1 + L1 * sin(phi1); }
float x4() { return x2() + L2 * cos(phi2); }
float y4() { return y2() + L2 * sin(phi2); }
float x5() { return x3() + L3 * cos(phi3); }
float y5() { return y3() + L3 * sin(phi3); }

int TRACE_LENGTH = 2048;
float[] x_trace = new float[TRACE_LENGTH];
float[] y_trace = new float[TRACE_LENGTH];

float cross(float a_x, float a_y, float b_x, float b_y) {
  return (a_x * b_y) - (a_y * b_x);
}

float[] linspace(float left, float right, int num_samples) {
  float[] result = new float[num_samples];
  float dx = (right - left) / (num_samples - 1);
  for (int i = 0; i < num_samples; ++i) {
    result[i] = left + (i * dx);
  }
  return result;
}
float[] sample = linspace(0, PI, 64);
int i = 0;
int j = 0;
void setup() { size(512, 512); }
void draw() {
  // // setup
  // white background
  // origin at center of screen
  // x-axis points right
  // y-axis points up
  frameRate(1000);
  background(255);
  float world_width = 4.0;
  float world_height = 4.0;
  translate(width / 2, height / 2);
  scale(width / world_width, -height / world_height);
  strokeWeight((world_width / width) * 3.0);
  fill(0, 0, 0, 0);
  float transformedMouseX = mouseX * (world_width / width) - (world_width / 2);
  float transformedMouseY = -mouseY * (world_height / height) + (world_height / 2);

  // // update
  // drive phi0, phi1
  // phi0 = (2 * PI / 3) - (transformedMouseX);
  // phi1 = (PI / 3) - (transformedMouseY);
  phi0 = PI - sample[i];
  phi1 = sample[j];
  
  {
    i += 1;
    if (i == -1 || i == sample.length) {
      i = 0;
      if (++j == sample.length) {
        j = 0;
      }
    }
  }

  // TODO: enforce distance constraint between p2 and p3
  // solve (FK) for phi2, phi3
  float p_x;
  float p_y;
  {
    // // analytic
    // problem reduces to circle-circle intersection (https://dassencio.org/102)
    // circle A has center p2 and radius L2
    // circle B has center p3 and radius L3
    float center_A_x = x2();
    float center_A_y = y2();
    float center_B_x = x3();
    float center_B_y = y3();
    float dx = (x3() - x2());
    float dy = (y3() - y2());
    float squared_center_distance = (dx * dx) + (dy * dy);
    float center_distance = sqrt(squared_center_distance);
    float unit_center_vector_x = (dx / center_distance);
    float unit_center_vector_y = (dy / center_distance);
    float perp_to_unit_center_vector_x = -unit_center_vector_y;
    float perp_to_unit_center_vector_y = unit_center_vector_x;
    float squared_A_radius = (L2 * L2);
    float squared_B_radius = (L3 * L3);
    float length_u = (squared_A_radius - squared_B_radius + squared_center_distance) / (2 * center_distance);
    float u_x = (length_u * unit_center_vector_x);
    float u_y = (length_u * unit_center_vector_y);
    float squared_length_u = (length_u * length_u);
    float length_v = sqrt(squared_A_radius - squared_length_u);
    float v_x = (length_v * perp_to_unit_center_vector_x);
    float v_y = (length_v * perp_to_unit_center_vector_y);
    p_x = (center_A_x + u_x + v_x);
    p_y = (center_A_y + u_y + v_y);
    stroke(0, 255, 0);
    circle(p_x, p_y, 10);
  }
  phi2 = atan2(p_y - y2(), p_x - x2());
  phi3 = atan2(p_y - y3(), p_x - x3());

  // // draw
  stroke(  0, 0, 0); line(x0, y0, x1, y1);
  stroke(255, 0, 0); line(x0, y0, x2(), y2());
  stroke(0, 0, 255); line(x1, y1, x3(), y3());
  stroke(128, 0, 0); line(x2(), y2(), x4(), y4());
  stroke(0, 0, 128); line(x3(), y3(), x5(), y5());
  // TODO: configuration across the singularity
  
  // FORNOW: don't plot concave / "kinked" configurations of robot, e.g. don't plot
  //      45  
  //     .  .
  //     .    . 
  //     2     3
  //     .     .
  //   .       .
  //  0        1
  boolean skip0 = cross(x0 - x2(), y0 - y2(), x4() - x2(), y4() - y2()) < 0.0;
  boolean skip1 = cross(x1 - x3(), y1 - y3(), x5() - x3(), y5() - y3()) > 0.0;
  boolean skip = (skip0 || skip1);
  
  if (!skip) {
    // // // trace
    // // push new data point to trace buffer
    // shift over data one slot [ A, B, C, D ] -> [ A, A, B, C ] 
    for (int i = (TRACE_LENGTH - 1); i > 0; --i) {
      x_trace[i] = x_trace[i - 1];
      y_trace[i] = y_trace[i - 1];
    }
    // write new data point to slot 0
    x_trace[0] = p_x;
    y_trace[0] = p_y;
  }
  
  // draw trace
  for (int i = 0; i < TRACE_LENGTH; ++i) {
    stroke(0, 255 * (1.0 - float(i) / (TRACE_LENGTH - 1)), 0);
    circle(x_trace[i], y_trace[i], 0.01);
  }
  
  // draw mouse position
  stroke(0, 255, 0);
  circle(transformedMouseX, transformedMouseY, 0.02);
}