#include <math.h>
#include <ESP32Servo.h>
#include <stdlib.h>

//////////////////////////////////////////////////////////////

using namespace std;

struct Node{
  Node* next;
  int arm;
  float x,y,z;
  bool relative;
};
typedef struct Node Node;

class Arm{
  private:
    static constexpr float l1=100,l2=100;
    const float e=1,speed=0.1;

    float x0,y0,z0;
    float x2,y2,z2;

    float tempx,tempy,tempz;
    float distance=10;
    
    int servopins[3]={};

  public:
    float o1,o2,o3;
    float x1,y1,z1;
    Servo servos[3];
    bool moving;

    Arm(){
      x1=x2=tempx=y1=y2=tempy=z1=z2=tempz=0.0;
    }

    Arm(int servopins[3],float x0,float y0,float z0){
      for(int i=0;i<3;i++){
        this->servopins[i]=servopins[i];
      }

      moving=false;

      x1=x2=tempx=y1=y2=tempy=z1=z2=tempz=0.0;

      this->x0=x0;
      this->y0=y0;
      this->z0=z0;

      calculateAngles(0,0,0);
    }

    void move(float x,float y,float z){
      x2=x;
      y2=y;
      z2=z;

      if(max(fabs(x2-x1),max(fabs(y2-y1),fabs(z2-z1))) > e) moving=true;
      distance = sqrt(pow(x2-tempx,2)+pow(y2-tempy,2)+pow(z2-tempz,2));
    }

    void calculateAngles(float x,float y,float z){
      x+=x0;
      y+=y0;
      z+=z0;

      float h = sqrt(x*x+y*y);
      float l = sqrt(h*h+z*z);

      o3 = acos(constrain((l1*l1 + l2*l2 - l*l)/(2*l1*l2),-1.0,1.0));
      o2 = PI/2.0 - (acos(constrain((l*l + l1*l1 - l2*l2)/(2*l*l1),-1.0,1.0)) + atan2(z,h));
      o1 = atan2(x,y);
    }

    void interpolate(){
      if(max(fabs(x2-x1),max(fabs(y2-y1),fabs(z2-z1))) < e){
        tempx=x1;
        tempy=y1;
        tempz=z1;

        moving=false;
        return;
      }

      x1+=(x2-tempx)/distance * speed;
      y1+=(y2-tempy)/distance * speed;
      z1+=(z2-tempz)/distance * speed;

      calculateAngles(x1,y1,z1);
      
      moving=true;

      writeServos();
    }

    void writeServos(){
      servos[0].write(o1*180/PI);
      servos[1].write(o2*180/PI);
      servos[2].write(o3*180/PI);
    }

};

////////////////////////////////////////////////////////////////

void addToQueue(int,int,float,float,float,bool);
void goThroughQueue(int);
void serialInput();
void updateState();
void stateLogic();
void startWalking();
bool queuesEmpty();

Node* heads[4] = {nullptr};
Node* tails[4] = {nullptr};

int svps[4][3]={{14,27,26},
                {12,13,19},
                {18,5,17},
                {16,4,0}};


Arm arms[4]={Arm(svps[0],10,10,-10),
             Arm(svps[1],10,10,-10),
             Arm(svps[2],10,10,-10),
             Arm(svps[3],10,10,-10)};

enum BOT_STATES{IDLE,WALKING,WAVING};
BOT_STATES State=IDLE;

long long int now=0;
float arr[5]={};
bool sign[5]={};
int z=0;

////////////////////////////////////////////////////////////////

void setup(){

  for(int i=0;i<4;i++){
    for(int j=0;j<3;j++){
      arms[i].servos[j].attach(svps[i][j]);
    }
    arms[i].writeServos();
  }

  delay(1000);

  Serial.begin(9600);
}

void loop(){

  for(int i=0;i<4;i++){
    goThroughQueue(i);
  }

  for(int i=0;i<4;i++){
    arms[i].interpolate();
  }
  
  // if(millis()-now>=100){
  //   Serial.print(arms[0].o1 * 180/PI);
  //   Serial.print(" ");
  //   Serial.print(arms[0].o2 * 180/PI);
  //   Serial.print(" ");
  //   Serial.println(arms[0].o3 * 180/PI);
  //   now=millis();
  // }

  updateState();
  stateLogic();

  serialInput();

}

///////////////////////////////////////////////////////////////////////

void addToQueue(int queue,int arm,float x,float y,float z,bool relative=false){
  Node* node = (Node*) malloc(sizeof(Node));

  node->arm=arm;
  node->x=x;
  node->y=y;
  node->z=z;
  node->next=nullptr;
  node->relative=relative;

  if(heads[queue]==nullptr){
    heads[queue]=tails[queue]=node;
    return;
  }

  tails[queue]->next=node;
  tails[queue]=node;
}

bool execute[4]={1,1,1,1};
void goThroughQueue(int queue){
  if(heads[queue]==nullptr) return;

  if(execute[queue]){
    int armn=heads[queue]->arm;

    if(heads[queue]->relative)
      arms[armn].move(heads[queue]->x+arms[armn].x1,heads[queue]->y+arms[armn].y1,heads[queue]->z+arms[armn].z1);
    else
      arms[heads[queue]->arm].move(heads[queue]->x,heads[queue]->y,heads[queue]->z);

    execute[queue]=false;
  }

  if(!arms[heads[queue]->arm].moving){
    Node *temp = heads[queue];

    heads[queue]=heads[queue]->next;
    free(temp);

    execute[queue]=true;
  }
}

void serialInput(){
  while(Serial.available()){
    int val=Serial.read();

    if(val==(int)' '){
      z++;
      continue;
    }

    if(val==(int)'-'){
      sign[z]=true;
      continue;
    }

    if(val==10 || val==(int)'n'){
      for(int j=0;j<5;j++){
        arr[j]*= sign[j]?-1:1;
      }
      addToQueue((int)arr[0],(int)arr[1],arr[2],arr[3],arr[4]);

      for(int j=0;j<5;j++){
        arr[j]=0;
        sign[j]=false;
      }

      z=0;
      break;
    }

    arr[z]=(arr[z]*10)+val-48;
  }
}

void updateState(){
  if(State==IDLE) startWalking();
}

short ws=0;
void stateLogic(){
  if(State==WALKING){
    if(queuesEmpty()){
      if(ws==0 || ws==2){
        Serial.println("PULLBACK");
        addToQueue(0,0,-10,0,0,true);
        addToQueue(1,1,-10,0,0,true);
        addToQueue(2,2,-10,0,0,true);
        addToQueue(3,3,-10,0,0,true);
      }
      else if(ws==1){
        Serial.println("LEFT");
        addToQueue(0,2,10,0,5);
        addToQueue(0,2,20,0,0);

        addToQueue(0,3,10,0,5);
        addToQueue(0,3,20,0,0);
      }
      else if(ws==3){
        Serial.println("RIGHT");
        addToQueue(0,0,10,0,5);
        addToQueue(0,0,20,0,0);

        addToQueue(0,1,10,0,5);
        addToQueue(0,1,20,0,0);
      }

      ws++;
      if(ws==4) ws=0;
    }
  }
}

void startWalking(){
  addToQueue(0,0,5,0,5);
  addToQueue(0,0,10,0,0);

  addToQueue(0,1,5,0,5);
  addToQueue(0,1,10,0,0);

  State=WALKING;
}

bool queuesEmpty(){
  for(int i=0;i<4;i++){
    if(heads[i]!=nullptr) return false;
  }
  return true;
}