#include <Servo.h>

#define pi 3.1415926
#define l1 8
#define l2 8

Servo arm1;
Servo arm2;
Servo arm3;


String buf;

typedef struct Servo_Value
{
    int arm1_angle;
    int arm2_angle;
    int arm3_angle;
}Servo_Value;

Servo_Value Servo_angle;

void setup()
{
  Serial.begin(115200);
  //Serial.println("ok");
  arm1.attach(2);
  arm2.attach(5);
  arm3.attach(6);

  arm1.write(90);
  arm2.write(90);
  arm3.write(90);

  int L = 50;
  int a1 = 120;
  int x = L*cos(a1*pi/180);
  int y = L*sin(a1*pi/180);
  Serial.println(x);
  Serial.println(y);
  
}


void loop()
{

  


  if(Serial.available()){
    buf = Serial.readString();
    Serial.println(buf);
    if(!strncmp(buf.c_str(),"loca:",5)){//loca:+03,-04,+06
      int x,y,z;
      if(buf[5] == '-'){
        x = -((buf[6]-'0')*10+(buf[7]-'0'));
      }else{
        x = ((buf[6]-'0')*10+(buf[7]-'0'));
      }
      if(buf[9] == '-'){
        y = -((buf[10]-'0')*10+(buf[11]-'0'));
      }else{
        y = ((buf[10]-'0')*10+(buf[11]-'0'));
      }
      if(buf[13] == '-'){
        z = -((buf[14]-'0')*10+(buf[15]-'0'));
      }else{
        z = ((buf[14]-'0')*10+(buf[15]-'0'));
      }
      ServoLocation(&Servo_angle,x,y,z);
      Serial.print("arm1_angle:");
      Serial.println(Servo_angle.arm1_angle);
      Serial.print("arm2_angle:");
      Serial.println(Servo_angle.arm2_angle);
      Serial.print("arm3_angle:");
      Serial.println(Servo_angle.arm3_angle);
      Serial.println("");
    }
  }
}

void ServoLocation(Servo_Value *Servo_Struct,float x,float y,float z)
{
  float L,L1,a1,a2,h,arm2_pos,arm3_pos;

  L = sqrt(x*x+y*y);
  Serial.print("L:");
  Serial.println(L);
  L1 = sqrt(L*L+z*z);
  Serial.print("L1:");
  Serial.println(L1);
  a1 = atan(z/L)*180/pi;
  Serial.print("a1:");
  Serial.println(a1);
  a2 = (acos((l1*l1+L1*L1-l2*l2)/(2*l1*L1)))*180/pi;
  Serial.print("a2:");
  Serial.println(a2);
  arm2_pos = a1+a2;
  Serial.print("arm2_pos:");
  Serial.println(arm2_pos);
  h = l1*sin(arm2_pos*pi/180);
  Serial.print("h:");
  Serial.println(h);
  arm3_pos = (asin((h-z)/l2))*180/pi;
  Serial.print("arm3_pos:");
  Serial.println(arm3_pos);
  

  Servo_Struct->arm1_angle = 90-(atan(x/y)*180/pi);
  Servo_Struct->arm2_angle = 180-arm2_pos;
  Servo_Struct->arm3_angle = 90-arm3_pos;
}


//a1:初始值，a2:目标值，b1:初始值，b2:目标值，delaytime:给定耗时时间，单位ms
void compition(int a1,int a2,int b1,int b2,int delaytime)
{
  unsigned long a_previousMillis=0,b_previousMillis=0;
  unsigned long currentMillis;
  int flag = true;
  int a_diff = abs(a2-a1);//a差值
  int b_diff = abs(b2-b1);//b差值
  int ta = delaytime/(a_diff);//a每一步所花时间
  int tb = delaytime/(b_diff);//b每一步所花时间

  while(flag){
    currentMillis = millis();
    if (currentMillis - a_previousMillis >= ta) {
      a_previousMillis = currentMillis;
      if(a2>a1){
        a1++;
      }else if(a2<a1){
        a1--;
      }
      Serial.print("a1:");
      Serial.println(a1);
    }
    if (currentMillis - b_previousMillis >= tb) {
      b_previousMillis = currentMillis;
      if(b2>b1){
        b1++;
      }else if(b2<b1){
        b1--;
      }
      Serial.print("b1:");
      Serial.println(b1);
    }
    if(a1 == a2 && b1 == b2)flag = false;
  }
}