#include <Servo.h>

int steps = 0;
int Xpos;
int Ypos;
int Zpos;

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;


void kinematik(){
  Serial.println("Kinematika Terpanggil");
  //Tulis fungsi kinematik
  //X,Y,Z
  float theta0 = Xpos;
  float theta1 = Ypos;
  float theta2 = Zpos;

  servo1.write(theta0);
  servo2.write(theta1);
  servo3.write(theta2);
}



void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);

  servo1.attach(9);
  servo2.attach(10);
  servo3.attach(11);

}

void loop() {
  // put your main code here, to run repeatedly:
  if(steps == 0){
    Serial.println("STEP 0");
    int Xpos0 = 100;
    int Ypos0 = 0;
    int Zpos0 = 100;

    int Xpos1 = 100;
    int Ypos1 = 0;
    int Zpos1 = 50;

    //Ini blok rujak
    for(int times = 0; times <= 10; times++){
      Xpos = Xpos0 + ((Xpos1 - Xpos0) * (times / 10.0));
      Ypos = Ypos0 + ((Ypos1 - Ypos0) * (times / 10.0));
      Zpos = Zpos0 + ((Zpos1 - Zpos0) * (times / 10.0));
      Serial.println(Xpos);
      Serial.println(Ypos);
      Serial.println(Zpos);
      kinematik();
      delay(500);
    }
    steps = 1;
  } else if(steps == 1) {
    Serial.println("STEP 1");

    int Xpos0 = 100;
    int Ypos0 = 0;
    int Zpos0 = 50;

    int Xpos1 = 100;
    int Ypos1 = 0;
    int Zpos1 = 100;

    for(int times = 0; times <= 10; times++){
      Xpos = Xpos0 + ((Xpos1 - Xpos0) * (times / 10.0));
      Ypos = Ypos0 + ((Ypos1 - Ypos0) * (times / 10.0));
      Zpos = Zpos0 + ((Zpos1 - Zpos0) * (times / 10.0));
      Serial.println(Xpos);
      Serial.println(Ypos);
      Serial.println(Zpos);
      kinematik();
      delay(500);
    }

    steps = 2;
  }else if(steps == 2) {
    Serial.println("STEP 2");

    int Xpos0 = 100;
    int Ypos0 = 0;
    int Zpos0 = 100;

    int Xpos1 = 10;
    int Ypos1 = 0;
    int Zpos1 = 100;

    for(int times = 0; times <= 10; times++){
      Xpos = Xpos0 + ((Xpos1 - Xpos0) * (times / 10.0));
      Ypos = Ypos0 + ((Ypos1 - Ypos0) * (times / 10.0));
      Zpos = Zpos0 + ((Zpos1 - Zpos0) * (times / 10.0));
      Serial.println(Xpos);
      Serial.println(Ypos);
      Serial.println(Zpos);
      kinematik();
      delay(500);
    }


    steps = 3;
  }else if(steps == 3) {
    Serial.println("STEP 3");

    int Xpos0 = 10;
    int Ypos0 = 0;
    int Zpos0 = 100;

    int Xpos1 = 10;
    int Ypos1 = 100;
    int Zpos1 = 100;

    for(int times = 0; times <= 10; times++){
      Xpos = Xpos0 + ((Xpos1 - Xpos0) * (times / 10.0));
      Ypos = Ypos0 + ((Ypos1 - Ypos0) * (times / 10.0));
      Zpos = Zpos0 + ((Zpos1 - Zpos0) * (times / 10.0));
      Serial.println(Xpos);
      Serial.println(Ypos);
      Serial.println(Zpos);
      kinematik();
      delay(500);
    }

    steps = 4;
  }else if(steps == 4) {
    Serial.println("STEP 4");

    int Xpos0 = 10;
    int Ypos0 = 100;
    int Zpos0 = 100;

    int Xpos1 = 100;
    int Ypos1 = 0;
    int Zpos1 = 100;

    for(int times = 0; times <= 10; times++){
      Xpos = Xpos0 + ((Xpos1 - Xpos0) * (times / 10.0));
      Ypos = Ypos0 + ((Ypos1 - Ypos0) * (times / 10.0));
      Zpos = Zpos0 + ((Zpos1 - Zpos0) * (times / 10.0));
      Serial.println(Xpos);
      Serial.println(Ypos);
      Serial.println(Zpos);
      kinematik();
      delay(500);
    }

    steps = 5;
  }
  delay(1000);
}