// #define SERIAL_RX_BUFFER_SIZE 1024
// C:\Users\Eru2\AppData\Local\Arduino15\packages\arduino\hardware\avr\1.8.6\cores\arduino\HardwareSerial.h

// Program ini memerlukan RX SERIAL BUFFER yang besar, agar tidak ada data yang terbuang saat menerima data serial
// Ubah definisi SERIAL_RX_BUFFER_SIZE 1024 
// Cari file HardwareSerial.h pada User Local, atau pancing adanya kesalahan duplikasi definisi dengan mengaktifkan
// definisi di atas, maka akan ditampilkan lokasi file HardwareSerial.h pada komputer saat ini

#include <Servo.h>

Servo M1, M2;                 // Servo 1 dan 2

#define TMIN1     500         // Lebar Pulsa MIN Servo1
#define TMAX1     2450        // Lebar Pulsa MAX Servo1
#define TMIN2     500         // Lebar Pulsa MIN Servo2
#define TMAX2     2450        // Lebar Pulsa MAX Servo2

float L1=        250;          // panjang lengan 1 (misalkan 250 mm atau 25 cm)
float L2=        250;          // panjang lengan 2 (misalkan 250 mm atau 25 cm)

#define INCH      1
#define MM        2
#define ABSOLUTE  3
#define RELATIVE  4

int   Mode=ABSOLUTE;
int   Skala=MM;
float KecepatanG0=24000;         // mm per menit
float KecepatanG1=6000;          // mm per menit
float Spindle=255;
float tujuan_x, tujuan_y, tujuan_z;     // Posisi x,y yang ingin dituju (target)
float t_x, t_y, t_z, t_f;     // Posisi x,y yang ingin dituju (target)
float x, y, z;                   // Posisi Ujung Lengan (x,y) saat ini
float Vx, Vy, Vz;                 // Kecepatan x dan kecepatan y      
float s1, s2;                 // Sudut lengan 1 dan lengan 2 saat ini      
float interval_x, interval_y, interval_z; // penambahan atau pengurangan Posisi
float dt;                     // delta t, waktu antara, selisih t2 dan t1
unsigned long t1,t2;          // penghitung pewaktuan
int   OK=0;

void hitung_kinematika_balik(float x, float y, float *s1, float *s2)
{
  float L, a, b, alfa;        // Menggunakan Motor Servo 1 yang 0° ≤ θ1 ≤ 90°
  L=sqrt(x*x+y*y);            // Menggunakan Motor Servo 2 yang -90° ≤ θ2 ≤ 90°
  a=(L*L-L1*L1-L2*L2)/(2*L1);
  b=sqrt(L2*L2-a*a);
  alfa=atan2(b,L1+a);
  *s1=atan2(y,x)-alfa;        // bentuk 1, 0° ≤ θ2 ≤ 180°
  *s2=atan2(b,a);
  if(*s1<0)                   // Jika s1 minus, ubah ke bentuk 2
  {                           // karena s1, 0° ≤ θ1 ≤ 90°
    *s1=atan2(y,x)+alfa;      // bentuk 2, -180° ≤ θ2 ≤ 0°
    *s2=-atan2(b,a);
  }
  *s1*=180/PI; 
  *s2*=180/PI;

//Serial.print("L: "); Serial.println(L,3);
//Serial.print("L1: "); Serial.println(L1,3);
//Serial.print("L2: "); Serial.println(L2,3);
//Serial.print("a: "); Serial.println(a,3);
//Serial.print("b: "); Serial.println(b,3);
//Serial.print("alfa: "); Serial.println(alfa,3);

}

void setup()
{
  Serial.begin(115200);
  Serial.println("Masukkan Perintah G-Code :");
  Serial.println("Misalkan: G0 X10 Y20 <tekan ENTER>");
  //delay(2000);
  M1.attach(8,TMIN1,TMAX1);       // Servo 1 ke pin 8
  M2.attach(9,TMIN2,TMAX2);       // Servo 2 ke pin 9
  x=L1+L2;                        // Posisi saat ini/sekarang/terakhir
  y=0;
  z=0;
  tujuan_x=x;                     // posisi tujuan berikutnya 
  tujuan_y=y;
  tujuan_z=z;
  Vx=0;
  Vy=0;
  Vz=0;  
  t1=micros();
}

void G0G1(unsigned char C, float P)
{
  C=toupper(C);
  if(C=='X') t_x=P;
  if(C=='Y') t_y=P;
  if(C=='Z') t_z=P;
  if(C=='F') t_f=P;
}

void perintahG0G1(int G)
{
  if(Skala==INCH)
  {
    t_x*=25.4;
    t_y*=25.4;
    t_z*=25.4;
  }
  if(Mode==RELATIVE)
  {
    t_x+=tujuan_x;
    t_y+=tujuan_y;
    t_z+=tujuan_z;
  }
  tujuan_x=t_x;
  tujuan_y=t_y;
  tujuan_z=t_z;

//Serial.println("tx ty tz");
//Serial.println(t_x);
//Serial.println(t_y);
//Serial.println(t_z);
//Serial.println(t_f);
//delay(5000);

  float R=sqrt(pow(tujuan_x-x,2)+pow(tujuan_y-y,2)+pow(tujuan_z-z,2));
  float t=60*R/(G==0?KecepatanG0:t_f);
  if(t>0)
  {
    Vx=(tujuan_x-x)/t;
    Vy=(tujuan_y-y)/t;
    Vz=(tujuan_z-z)/t;
  } else
  {
    Vx=0; Vy=0; Vz=0;
  }

//Serial.println("Vx Vy Vz");
//Serial.println(Vx);
//Serial.println(Vy);
//Serial.println(Vz);
//Serial.println(t);
//delay(5000);

}

void bacaSerial()
{
/*
Contoh perintah G-Code

G21                 satuan mm
M3 S12000           nyalakan spindle motor, dengan kecepatan 12000 rpm
G90                 mode gerakan absolut
G1 Z3 F500          bergerak ke Z=3mm (naik) dengan kecepatan 500 mm/menit
G0 X100 Y450        bergerak dengan cepat ke x=100mm dan y=450mm
G1 Z-1 F500         bergerak ke z=-1mm (turun) dengan kecepatan 500 mm/menit
G1 X450 Y100 F6000  bergerak ke x=450mm dan y=100 dg kecepatan 6000 mm/menit
G1 Z3 F500          bergerak ke Z=3mm (naik) dengan kecepatan 500 mm/menit
G0 X500 Y0          kembali ke x=500mm dan y=0mm dengan cepat
G4 P0.1             tunggu 0,1 ms
M5                  matikan spindle

*/
  if(Serial.available()) {    // Menerima perintah dari Serial
    /*
    Serial.println(Serial.read());
    Serial.println(Serial.parseInt());
    Serial.println(Serial.read());
    Serial.println(Serial.read());
    Serial.println(Serial.parseFloat());
    */
    
    char C=Serial.read();

    //Serial.println(C);

    C=toupper(C);
    if(C=='G')
    {
      int G=Serial.parseInt();

//Serial.println(G);

      if(G==0||G==1)
      {
        t_x=tujuan_x;
        t_y=tujuan_y;
        t_z=tujuan_z;
        t_f=KecepatanG1;

//Serial.println("tx ty tz");
//Serial.println(t_x);
//Serial.println(t_y);
//Serial.println(t_z);
//Serial.println(t_f);
//delay(5000);


        if(Serial.available()>=2)
        {
          Serial.read();                        // membuang spasi
          C=Serial.read();
          float P=Serial.parseFloat();
          //Serial.print(C); Serial.println(P,3);
          G0G1(C,P);
        }
        if(Serial.available()>=2)
        {
          Serial.read();                        // membuang spasi
          C=Serial.read();
          float P=Serial.parseFloat();
          //Serial.print(C); Serial.println(P,3);
          G0G1(C,P);
        }
        if(Serial.available()>=2)
        {
          Serial.read();                        // membuang spasi
          C=Serial.read();
          float P=Serial.parseFloat();
          //Serial.print(C); Serial.println(P,3);
          G0G1(C,P);
        }
        if(Serial.available()>=2)
        {
          Serial.read();                        // membuang spasi
          C=Serial.read();
          float P=Serial.parseFloat();
          //Serial.print(C); Serial.println(P,3);
          G0G1(C,P);
        }
        perintahG0G1(G);

//Serial.println("Vx Vy Vz");
//Serial.println(Vx);
//Serial.println(Vy);
//Serial.println(Vz);


      } else if(G==4)
      {
        if(Serial.available()>=2)
        {
          Serial.read();                // buang spasi
          C=toupper(Serial.read());
          if(C=='P')
          {
            float P=Serial.parseFloat();
            Serial.print("Delay("); Serial.print(P,3); Serial.println("ms)");
            delay(P);
          }
        }
        OK=1;
      } else if(G==20)
      {
        Skala=INCH;
        Serial.println("Skala=inchi");
        OK=1;
      } else if(G==21)
      {
        Skala=MM;
        Serial.println("Skala=mm");
        OK=1;
      } else if(G==90)
      {
        Mode=ABSOLUTE;
        Serial.println("Mode Absolute");
        OK=1;
     } else if(G==91)
      {
        Mode=RELATIVE;
        Serial.println("Mode Relative");
        OK=1;
      } else
      {
        Serial.println("Fungsi belum dibuat");
        OK=1;
      }
    } else if(C=='M')
    {
      int M=Serial.parseInt();
      if(M==3||M==4)
      {
        if(Serial.available()>=2)
        {
          Serial.read();                // buang spasi
          C=toupper(Serial.read());
          if(C=='S')
          {
            float S=Serial.parseFloat();
            Spindle=S;
          }
        }
        if(M==3)
          Serial.println("Spindle berputar CW");
        else
          Serial.println("Spindle berputar CCW");
        Serial.print("Kecepatan Spindle "); Serial.print(Spindle,3); Serial.println("rpm");
        OK=1;
      } else if(M==5)
      {
        Serial.println("Spindle berhenti");
        OK=1;
      } else
      {
        Serial.println("Fungsi belum dibuat");
        OK=1;
      }

    } else if(C=='F')
    {
      float F=Serial.parseFloat();
      KecepatanG1=F;
      Serial.print("Kecepatan Gerak "); Serial.print(F,3); Serial.println("mm/menit");
      OK=1;
    } else if(C=='S')
    {
      float S=Serial.parseFloat();
      Spindle=S;
      Serial.print("Kecepatan Spindle "); Serial.print(S,3); Serial.println("rpm");
      OK=1;
    } else if(C==10||C==32||C==13)
    {

    }else
    {
      Serial.println("Perintah tidak dikenal");
      OK=1;
    }
  }
}

void loop()
{
  hitung_kinematika_balik(x,y,&s1,&s2);
  if(x!=tujuan_x||y!=tujuan_y||z!=tujuan_z)
  {
    if(Skala==MM)
    {
      Serial.print("x="); Serial.print(x,1); Serial.print(" mm");    
      Serial.print(" y="); Serial.print(y,1); Serial.print(" mm");
      Serial.print(" z="); Serial.print(z,1); Serial.print(" mm");
    } else
    {
      Serial.print("x="); Serial.print(x/25.4,1); Serial.print(" inch");    
      Serial.print(" y="); Serial.print(y/25.4,1); Serial.print(" inch");
      Serial.print(" z="); Serial.print(z/25.4,1); Serial.print(" inch");
    }
    Serial.print(" --> θ1="); Serial.print(s1,2); Serial.print("°");    
    Serial.print(" θ2="); Serial.print(s2,2); Serial.println("°");
  }
  if(s1<0||s1>90)
  {
    Serial.println("Sudut lengan 1 (θ1) melebihi batas 0° ≤ θ1 ≤ 90°");
    Serial.println("Di luar batas area kerja (Workspace)");
  } else if(s2<-90||s2>90)
  {
    Serial.println("Sudut lengan 2 (θ2) melebihi batas -90° ≤ θ2 ≤ 90°");
    Serial.println("Di luar batas area kerja (Workspace)");
  } else if(isnan(s1)||isnan(s2))     
  {
    Serial.println("Di luar batas area kerja (Workspace)");
  } else
  {
    M1.write(ceil(s1));                   
    M2.write(ceil(90.0-s2));      // Posisi Servo 2 dibalik, sudut s2: -90° ≤ θ2 ≤ 90°                 
    delay(15);
  }

  if(x==tujuan_x&&y==tujuan_y&&z==tujuan_z)
  {  // Sudut mencapai tujuan ?
    if(OK)
    {
      OK=0;
      Serial.println("OK");
    }
    bacaSerial();
  } else OK=1;

//Serial.println("Vx Vy Vz");
//Serial.println(Vx);
//Serial.println(Vy);
//Serial.println(Vz);


  t2=micros();
  dt=(t2-t1)/1000000.0;     // t1 dan t2 dalam us, dt dalam s
  interval_x=Vx*dt;         
  interval_y=Vy*dt;
  interval_z=Vz*dt;

//Serial.println("Interval");
//Serial.println(interval_x);
//Serial.println(interval_y);
//Serial.println(interval_z);
//delay(5000);

  t1=t2;
  x+=interval_x;
  y+=interval_y;
  z+=interval_z;

//Serial.println("X Y Z");
//Serial.println(x);
//Serial.println(y);
//Serial.println(z);
//delay(5000);

  // Menghindari s1 melebihi tujuan
  if((interval_x>0&&x>tujuan_x)||(interval_x<0&&x<tujuan_x)) x=tujuan_x;
  if((interval_y>0&&y>tujuan_y)||(interval_y<0&&y<tujuan_y)) y=tujuan_y;
  if((interval_z>0&&z>tujuan_z)||(interval_z<0&&z<tujuan_z)) z=tujuan_z;
}