#include <Braccio.h>
#include <Servo.h>
#include <math.h>

Servo base;
Servo shoulder;
Servo elbow;
Servo wrist_rot;
Servo wrist_ver;
Servo gripper;

#define PI 3.14159265359
#define PIdiv2 PI/2

const float shoulder_and_elbow_length = 12.5;
const float shoulder_plus_elbow_length = shoulder_and_elbow_length * 2;
const float wrist_length = 19;

class Vec2 {
public:
Vec2(float _x, float _y) : x(_x), y(_y) {}
float x,y;  
};

class Vec4 {
public:
  Vec4(float _x, float _y, float _z, float _w) : x(_x), y(_y), z(_z), w(_w) {}
  Vec4() = default;
  
  Vec4 to_rotation(){
    float base_angle = atan2(x, y);
    
    float wRAD = w * PI / 180;

    // take the wrist into account
    Vec2 target_position = Vec2(hypotf(x, y) - sin(wRAD) * wrist_length, -z + cos(wRAD) * wrist_length);

    /////// shoulder and elbow angles calculations ///////
    
    // calculate distance beetween the arm base and the target position and then clamp it to 1
    float target_base_distance = hypotf(target_position.x, target_position.y) / shoulder_plus_elbow_length;
    if (target_base_distance > 1) {
      target_base_distance = 1;
    }

    // set shoulder angle to arm bend angle from distance from target position to base position
    float shoulder_angle = asin(target_base_distance);

    // calculate shoulder angle from the shoulder angle before performing another calculation on it
    float elbow_angle = PI - (shoulder_angle * 2);

    // add angle beetween the target position and the base position to the shoulder angle
    shoulder_angle += atan2(target_position.y, target_position.x);

    ///////// wrist angle calculation ///////
    float shoulder_plus_elbow_angle = shoulder_angle + elbow_angle; // shoulder angle + elbow angle
    //Serial.println(shoulder_plus_elbow_angle);
    //Serial.println(wRAD);
    float wrist_angle = wRAD - shoulder_plus_elbow_angle; // # target wrist angle - shoulder_plus_elbow_angle   so the wrist angle is always fixes to the base rotation
    
    return Vec4(base_angle * 180 / PI, shoulder_angle * 180 / PI + 90, elbow_angle * 180 / PI + 90, wrist_angle * 180 / PI + 90);
  }

  void print(){
    //Serial.println(x);
    Serial.println(y);
    Serial.println(z);
    Serial.println(w);
    Serial.println("==================");
  }
  float x,y,z,w;
};

void setup() {
  Serial.begin(9600);
  Braccio.begin();
}

Vec4 targ;
const int buffer_size = 16;
byte in[buffer_size];


void loop() {
  int amogos = 0;
  while(true){
    if (Serial.available() >= 4){
      Serial.readBytes(in, buffer_size);
      float decoded = byte(in);
      float vec[3];
      vec[0] = targ.x;
      vec[1] = targ.y;
      vec[2] = targ.z;
      vec[3] = targ.w;

      vec[amogos] = decoded;
      
      targ = Vec4(vec[0],vec[1],vec[2],vec[3]);
      amogos++;
      if(amogos == 4){
        break;  
      }
      //float x = Serial.read();
      //float y = Serial.read();
      //float z = Serial.read();
      //float w = Serial.read();

    }
  }
  
  Vec4 rot =  targ.to_rotation();
  //                    speed     inverse knematics           wrist rot  grip
  Braccio.ServoMovement(30,    rot.x, rot.y, rot.z, rot.w,     90,        70); 
}