#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>

Servo joint;
LiquidCrystal_I2C lcd(0x27,20,4);

int angle = 0;
float x = 0, y = 0, z = 0;

void setup() {

  Serial.begin(9600);
  Serial.println("Initialized [...] ");
  joint.attach(9);
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0,0);
  lcd.print("SM ctrl.instructions :");
  lcd.setCursor(0,1);
  lcd.print("For angle w.: angle");
  lcd.setCursor(0,2);
  lcd.print("For f(x,y,z),w.:xyz");
  lcd.setCursor(0,3);
  lcd.print("Serial Monitor->>");

}

void loop() {

  if (Serial.available() > 0) {

    String input = Serial.readStringUntil('\n');

    if (input.startsWith("angle")) {
      
      lcd.clear();
      angle = input.substring(6).toInt();
      joint.write(angle);
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print("Joint angle: ");
      lcd.print(angle);

    } else if (input.startsWith("xyz")) {

      String xyz = input.substring(4);
      int comma1 = xyz.indexOf(",");
      int comma2 = xyz.indexOf(",", comma1+1);
      x = xyz.substring(0,comma1).toFloat();
      y = xyz.substring(comma1+1,comma2).toFloat();
      z = xyz.substring(comma2+1).toFloat();
      float theta1 = atan2(y,x);
      float theta2 = acos((x*x+y*y+z*z-200*200)/(2*200*sqrt(x*x+y*y+z*z)));
      float theta3 = atan2(sqrt(1-(theta2*theta2)), theta2);
      int servo1 = int(theta1 * 180/PI);
      //int servo2 = int((theta2 + theta3) * 180/PI);
      //int servo3 = int(90 - theta2 * 180/PI);
      joint.write(servo1);
      delay(1000);
      //joint.write(servo2);
      //delay(500);
      //joint.write(servo3);
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print("X: ");
      lcd.print(x);
      lcd.setCursor(0,1);
      lcd.print("Y: ");
      lcd.print(y);
      lcd.setCursor(9,0);
      lcd.print("Z: ");
      lcd.print(z);
      lcd.setCursor(9,1);
      lcd.print("Servo1: ");
      lcd.print(servo1);
      //lcd.print(" Servo2: ");
      //lcd.print(servo2);
      //lcd.print(" Servo3: ");
      //lcd.print(servo3);

    }
  }
}