// https://forum.arduino.cc/t/robot-dog-code-appears-to-do-nothing/1438107/3
// also https://wokwi.com/projects/358356476390848513
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

//robot dimensions
uint8_t servonum = 0;
int bodylength = 171;
int bodywidth = 105;
int segment1 = 36;
int segment2 = 50;
int segment3 = 100;
int myDelay = 20000;

//controller inputs
int leftstickhorizontal;
int leftstickvertical;
int rightstickhorizontal;
int rightstickvertical;

//robot dog parts
int Rx = -50;
int Ry = 100;
int Rz = 36;
int Lx = 0;
int Ly = 50;
int Lz = 36;
int dogroll;
int dogyaw;
int dogpitch;

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

int SERVO_MIN  = 150;
int SERVO_MAX  = 600;
int SERVO_FREQ = 50;
int walkspeed = 300;

int servobackrighthip = 5;
int servobackrightshoulder = 6;
int servobackrightelbow = 7;
int servobacklefthip = 8;
int servobackleftshoulder = 9;
int servobackleftelbow = 10;

int servofrontlefthip = 11;
int servofrontleftshoulder = 12;
int servofrontleftelbow = 13;
int servofrontrighthip = 14;
int servofrontrightshoulder = 15;
int servofrontrightelbow = 16;

void setup() {
  Serial.begin(115200);
  pwm.begin();
  pwm.setPWMFreq(SERVO_FREQ);
}

void servowrite(int s, int servodeg) {
  int pulse = map(servodeg, 0, 180, SERVO_MIN, SERVO_MAX);
  pwm.setPWM(s, 0, pulse);
}

void IKR(int Rx, int Ry, int Rz) {
  //theta 1, the hip in radians
  float alpha1rad = atan2(Ry, Rx);
  float alpha2rad = asin((segment1 / (sqrt(pow(abs(Rx), 2)) + (pow(abs(Ry), 2)))) * sin(90 * (PI / 180)));
  float alpha3rad = PI - (90 * (PI / 180)) - alpha2rad;
  //subtract for righ, add for left
  float theta1rad = alpha1rad - alpha3rad;

  //rotation matrix
  float R = theta1rad + (90 * (PI / 180)) - PI / 2;
  int newx = Rx * (-1 * R);
  int newy = Ry * (-1 * R);
  int newz = Rz * (-1 * R);

  //theta 2 & 3
  float B = sqrt(pow(abs(newz), 2) + pow(abs(newy), 2));
  float B4 = atan2(newz, newy);
  float B2 = acos(pow(abs(segment2), 2) + pow(B, 2) - pow(segment3, 2));
  float B3 = acos((pow(segment2, 2) + pow(segment3, 2) - pow(B, 2)) / (2 * segment2 * segment3));
  float theta2rad = B4 - B2;
  float theta3rad = PI - B3;
  int theta1deg = degrees(theta1rad);
  int theta2deg = degrees(theta2rad);
  int theta3deg = degrees(theta3rad);
  int ikresults [3] = {theta1deg, theta2deg, theta3deg};
  return ikresults;
}

void IKL(int Lx, int Ly, int Lz) {
  //theta 1, the hip in radians
  float alpha1rad = atan2(Ly, Lx);
  float alpha2rad = asin((segment1 / (sqrt(pow(abs(Lx), 2)) + (pow(abs(Ly), 2)))) * sin(90 * (PI / 180)));
  float alpha3rad = PI - (90 * (PI / 180)) - alpha2rad;
  //subtract for righ, add for left
  float theta1rad = alpha1rad + alpha3rad;

  //rotation matrix
  float R = theta1rad + (90 * (PI / 180)) - PI / 2;
  int newx = Lx * (-1 * R);
  int newy = Ly * (-1 * R);
  int newz = Lz * (-1 * R);
  //theta 2 & 3
  float B = sqrt(pow(abs(newz), 2) + pow(abs(newy), 2));
  float B4 = atan2(newz, newy);
  float B2 = acos(pow(abs(segment2), 2) + pow(B, 2) - pow(segment3, 2));
  float B3 = acos((pow(segment2, 2) + pow(segment3, 2) - pow(B, 2)) / (2 * segment2 * segment3));
  float theta2rad = B4 - B2;
  float theta3rad = PI - B3;
  int theta1deg = degrees(theta1rad);
  int theta2deg = degrees(theta2rad);
  int theta3deg = degrees(theta3rad);
  int ikresults [3] = {theta1deg, theta2deg, theta3deg};
  return ikresults;
}

void stepforwards() {
  Serial.print(1);
  int ikresults[3];
  IKR(0, -50, 36);
  servowrite(servobackrighthip, ikresults[1]);
  servowrite(servobackrightshoulder, ikresults[2]);
  servowrite(servobackrightelbow, ikresults[3]);
  delay(walkspeed);
  IKR(20, -30, 36);
  servowrite(servobackrighthip, ikresults[1]);
  servowrite(servobackrightshoulder, ikresults[2]);
  servowrite(servobackrightelbow, ikresults[3]);
  IKR(40, -50, 36);
  delay(walkspeed);
  servowrite(servobackrighthip, ikresults[1]);
  servowrite(servobackrightshoulder, ikresults[2]);
  servowrite(servobackrightelbow, ikresults[3]);
  delay(walkspeed);
  IKR(0, -50, 36);
  servowrite(servobackrighthip, ikresults[1]);
  servowrite(servobackrightshoulder, ikresults[2]);
  servowrite(servobackrightelbow, ikresults[3]);
  delay(walkspeed);

  IKL(0, -50, 36);
  servowrite(servobacklefthip, ikresults[1]);
  servowrite(servobackleftshoulder, ikresults[2]);
  servowrite(servobackleftelbow, ikresults[3]);
  delay(walkspeed);
  IKL(20, -30, 36);
  servowrite(servobacklefthip, ikresults[1]);
  servowrite(servobackleftshoulder, ikresults[2]);
  servowrite(servobackleftelbow, ikresults[3]);
  IKL(40, -50, 36);
  delay(walkspeed);
  servowrite(servobacklefthip, ikresults[1]);
  servowrite(servobackleftshoulder, ikresults[2]);
  servowrite(servobackleftelbow, ikresults[3]);
  delay(walkspeed);
  IKL(0, -50, 36);
  servowrite(servobacklefthip, ikresults[1]);
  servowrite(servobackleftshoulder, ikresults[2]);
  servowrite(servobackleftelbow, ikresults[3]);
  delay(walkspeed);
}

void loop() {
  Serial.print(2);
  leftstickhorizontal = analogRead(A0);
  leftstickvertical = analogRead(A1);
  rightstickhorizontal = analogRead(A2);
  rightstickvertical = analogRead(A3);
  stepforwards();
  Serial.print(3);
  Serial.println();
  // delay(myDelay);
}