#include <Servo.h>
#include <LiquidCrystal.h>
#define stepPin 20
#define dirPin 21
#define stepPin1 42
#define dirPin1 43
#define stepPin2 40
#define dirPin2 41
#define BUTTON_PIN 4

Servo servo1;
Servo servo2;
Servo servo3;

const int kar1 = 60.0000;
const int kar2 = 50.0000;
const int c = 20.0000;
const int time = 1000;
int gamma1 = 0;
int mgamma1;
int corx;
int corz;
int cory;
int corxsq;
int corysq;
int distance;
int angler;
int angled;
int ra;
int ra2;
int angler2;
int angled2;
int sra;
int ra3;
int angler3;
int angled3;
int angle1;
int angle2;
int pos1 = 90;
int pos2 = 90;
int fusiona1;
int fusiona2;
int posz;
int fusionaz;
int poszs;
int poszs1;
int poszs2;
int poszs3;
int x = 0;
int y = 3;
int xs1 = 0;
int ys1 = 3;
int xs2 = 0;
int ys2 = 3;
int length;
int ra4;
int angler4;
int angled4;
int angled4m;
int angled4sq;
int angled4sqrt;
int ax;
int ay;
int az;
int clv;
int gamma;
int beta;
int distancer;
int corxa;
int corza;
int corya;
int l2;
int cga;
int ga;
int sap;
int ap;
int sap2;
int ap2;
int sap3;
int ap3;

LiquidCrystal lcd(14, 15, 16, 17, 18, 19);
int tick;
int lastState = HIGH;
int man=0;
int cx;
int cy;
int cz;
int cycle;
String command;
int cv1=0;
int cv2=0;
int cv3=0;
int cv4=0;
int cv5=0;
int cv6=1;
int cm0=0;
int pb0=0;
int ch0=0;
int ch1=0;
int cs0=0;
int lcdv;
int rs0=0;
int dl=0;
int cl1=0;



char myData[30] = { 0 };

//========================================memory
int x1;
int y1;
int z1;
int x2;
int y2;
int z2;
int x3;
int y3;
int z3;
int x4;
int y4;
int z4;
int x5;
int y5;
int z5;
//===========================================

void setup() {

Serial.begin(115200);
//================================================================
  Serial.println("=====Commands:=====");
     Serial.println("cords");
     Serial.println("load");
     Serial.println("delete");
     Serial.println("clear");
     Serial.println("home");
     Serial.println("manual");
     Serial.println("auto");
     Serial.println("start");
     Serial.println("stop");
     Serial.println("===================");
//========================================================================


servo1.attach(44);
servo2.attach(45);
servo3.attach(46);
pinMode(stepPin,OUTPUT); 
pinMode(dirPin,OUTPUT);
pinMode(stepPin1,OUTPUT); 
pinMode(dirPin1,OUTPUT);
pinMode(stepPin2,OUTPUT); 
pinMode(dirPin2,OUTPUT);
pinMode(BUTTON_PIN, INPUT_PULLUP);


}

void loop() {


//=====================================inputs======================================

byte n = Serial.available();
  if ((n != 0) && (cv1 == 1)) {
    byte m = Serial.readBytesUntil('\n', myData, 30);
    myData[m] = '\0';  //null-byte
    if (sscanf(myData, "%d,%d,%d", &cx, &cy, &cz) == 3) {
      Serial.println("                              ");
      Serial.println("parsed three ints ok");
      Serial.print("int x = ");
      Serial.println(cx);
      Serial.print("int y = ");
      Serial.println(cy);
      Serial.print("int z = ");
      Serial.println(cz);
      Serial.println("                              ");
      cycle++;
      if (cycle > 5){Serial.println("Memory full");}

      switch (cycle) {
        case 1: x1 = cx;
                y1 = cy;
                z1 = cz;
                break;
        case 2: x2 = cx;
                y2 = cy;
                z2 = cz;
                break;   
        case 3: x3 = cx;
                y3 = cy;
                z3 = cz;
                break;    
        case 4: x4 = cx;
                y4 = cy;
                z4 = cz;
                break; 
        case 5: x5 = cx;
                y5 = cy;
                z5 = cz;
                break;                          
      }
      if (((cx<10)&&(cx>-10))||((cy>-10)&&(cy<10))){
      Serial.println("===collosion detected===");
                      cv1 = 0;
                      cl1=1;
                      Serial.println("Do you want to continue?");
                      Serial.println("========================");
                      Serial.println("                        ");}
                      

          Serial.print("1st cords: ");Serial.print(x1);Serial.print(", ");Serial.print(y1);Serial.print(", ");Serial.println(z1);
          Serial.print("2nd cords: ");Serial.print(x2);Serial.print(", ");Serial.print(y2);Serial.print(", ");Serial.println(z2);
          Serial.print("3rd cords: ");Serial.print(x3);Serial.print(", ");Serial.print(y3);Serial.print(", ");Serial.println(z3);
          Serial.print("4th cords: ");Serial.print(x4);Serial.print(", ");Serial.print(y4);Serial.print(", ");Serial.println(z4);
          Serial.print("5th cords: ");Serial.print(x5);Serial.print(", ");Serial.print(y5);Serial.print(", ");Serial.println(z5);

    } else {Serial.println("error in input!");
            cv1 = 0;
            Serial.println("===Cords disabled====");}

        
  }



  if((Serial.available())&& (cv1 == 0)){
        command = Serial.readStringUntil('\n');
         
        if(command.equals("load") && cl1==0){
            Serial.println("Loading");
            Serial.print("Select an operating mode:");Serial.println(" -'manual'");
            Serial.print("                         ");Serial.println(" -'auto'");
            cv2 = 1;
        }
        //============================================collosion options==============
        else if (command.equals("no") && cl1==1){                           
          Serial.println("deleting value...");
          cx=0;cy=0;cz=0;
          cycle--;
          cv1 = 1;
          delay(200);
          Serial.println("enter a new value!");
          Serial.println("                 ");   
          cl1=0;
        }
        else if (command.equals("yes") && cl1==1){
          cv1 = 1;
          cl1=0;
        }
        //=============================================================================
        else if((command.equals("start")) && (cv4 == 1 || cv3 == 1)){
          Serial.println("Starting");
          cv5 = 1;
          cv6 = 0;
        }
        else if(command.equals("stop")){
          Serial.println("Stoping");
          cv6 = 1;
          cv5 = 0;
          cv3 = 0;
          cv4 = 0;
        }
        //=================================home position=============================
        else if(command.equals("home") && cv6 == 1){
          Serial.println("Do you wanna return to home position?");
          ch0=1;
        }
        else if(command.equals("yes") && ch0 == 1){
          corx = 0;
          cory = 30;
          corz = 20;
          ch1 = 1;

        }
        else if(command.equals("no") && ch0 == 1){
          ch0 = 0;
        }
        //===========================================================================
        else if(command.equals("auto")){
          Serial.println("Selected operating mode: automatic");
          cv3 = 1;
          cv4 = 1;
        }
        else if(command.equals("manual")){
          Serial.println("Selected operating mode: manual");
          cv4 = 1;
          cv3 = 0;
        }
        else if(command.equals("cords")){
          cv1 = 1;
          Serial.println("                              ");
          Serial.println("========Cords enabled=========");
          Serial.println("If you're finished, hit enter!");
          Serial.println("==============================");
          Serial.println("                              ");
        }
        else if(command.equals("clear") && cv6==1){
          Serial.println("Do you wanna clear memory?");
          cm0=1;
        }
        else if(command.equals("yes") && cm0==1){
        x1=0; y1=0; z1=0; x2=0; y2=0; z2=0; x3=0; y3=0; z3=0; x4=0; y4=0; z4=0; x5=0; y5=0; z5=0;
        Serial.println("===Memory cleared===");
        cm0=0;
        cycle=0;
        }
        else if(command.equals("no") && cm0==1){cm0=0;Serial.println("===Clear cancelled===");}
        else if(command.equals("delete")){
          Serial.print("Deleting value: ");Serial.println(cycle);
          cx=0;cy=0;cz=0;
          cycle--;
          dl = 1;
          Serial.println("If you want to enter new values, type 'cords'!"); 
          }
          else{
            Serial.println("Invalid command or parameter");
        }
        
    }

//==================================data selecting=======================================

if (cv5==1 && man ==0 && cv6==0){man = 1;}

int value = digitalRead((BUTTON_PIN));
  if (lastState != value) {
    lastState = value;
        if ((value == LOW)&&(cv5==1)) {
      man += 1;
      pb0=1;
    }
  }
  if (man == cycle+1){
    man = 0;
  }
  switch (man) {
    case 1: corx = x1;
            cory = y1;
            corz = z1;
            break;
    case 2: corx = x2;
            cory = y2;
            corz = z2;
            break;
    case 3: corx = x3;
            cory = y3;
            corz = z3;
            break;
    case 4: corx = x4;
            cory = y4;
            corz = z4;
            break;
    case 5: corx = x5;
            cory = y5;
            corz = z5;
            break;                        
  }
  if ((value == LOW)&&(cv5==1)) {       
      Serial.println(man);
      Serial.print("x: ");Serial.println(corx);
      Serial.print("y: ");Serial.println(cory);
      Serial.print("z: ");Serial.println(corz);
    }

//==================================calculating==========================================


 mgamma1 = map(gamma1,-90,90,0,180);
 
 float ax = c*sin(mgamma1*3.1416/180);
 float az = sqrt(sq(c)-sq(ax));


if (gamma1<0){corza = corz+az;}
if (gamma1>0){corza = corz-az;}
if (gamma1==0){corza = corz;}


float length = sqrt(sq(corx)+sq(cory)); 
float distance = sqrt(sq(length)+sq(corz));


 if (distance < (kar1+kar2+ax)){
  lcdv=0;

float l2 = sqrt(sq(length-ax)+sq(corza));
float cga = acos((sq(l2)-sq(kar1)-sq(kar2))/(-2*kar1*kar2));
float ga = cga*180/3.1416;                         //angle gamma
float sap = asin((sin(cga)/l2)*kar2);
float ap = sap*180/3.1416;                        //angle alpha
float sap2 = asin(corza/l2);
float ap2 = sap2*180/3.1416;                       //angle alpha'
float sap3 = asin(cory/length);
float ap3 = sap3*180/3.1416;                        //angle alpha''

float gamma = 360-90-ap-ap2-ga;
poszs3 = gamma+mgamma1-180;

 angle1 = ap+ap2;                                            //stepper 1
 angle2 = ga;                                                //stepper 2

if (cory > 0 && corx > 0){                                  //stepper 0
  posz = ap3;
}

if (corx < 0 && cory > 0){
  posz = ap3 + 90;
}

if (cory < 0 && corx > 0){
  posz = ap3 + 270;
}

if (cory < 0 && corx < 0){
  posz = ap3 + 180;
}
 
pos1 = 180-angle1; 
pos2 = 180-angle2;


fusiona1 = map(pos1,0,180,-90,90);
fusiona2 = map(pos2,0,180,90,-90);
fusionaz = map(posz,0,180,-90,90);



//=============================================stepper 0============================
if ((cv5==1) && (cv6==0) || (ch1==1)){
    poszs = map(posz,0,360,2,202);

    poszs *= 1;                                //rotation multiplier


    if (poszs > x){digitalWrite(21, HIGH);}     // Enables the motor to move in a particular direction
       while (poszs > x){
        digitalWrite(stepPin,HIGH); 
        delayMicroseconds(2500);                  // by changing this time delay between the steps we can change the rotation speed
        digitalWrite(stepPin,LOW); 
        delayMicroseconds(2500);
        x++;
      }
    if (poszs < x){digitalWrite(21, LOW);}
      while (poszs < x){
        digitalWrite(stepPin,HIGH); 
        delayMicroseconds(2500);
        digitalWrite(stepPin,LOW); 
        delayMicroseconds(2500);
        x--;
      }

        

      //=============================================stepper's===========================
    poszs1 = map(pos1,0,180,0,200);
    poszs1 *= 1;                                //rotation multiplier
    poszs2 = map(pos2,0,180,0,201);
    poszs2 *= 1;                                //rotation multiplier

    if ((poszs1 > xs1)||(poszs2 > xs2)){digitalWrite(43, HIGH);digitalWrite(41, HIGH);}     // Enables the motor to move in a particular direction
      while ((poszs1 > xs1)||(poszs2 > xs2)){
        if(poszs1>xs1){
        digitalWrite(stepPin1,HIGH); 
        delayMicroseconds(2500);                  // by changing this time delay between the steps we can change the rotation speed
        digitalWrite(stepPin1,LOW); 
        delayMicroseconds(2500);
        xs1++;}
        if(poszs2>xs2){
        digitalWrite(stepPin2,HIGH); 
        delayMicroseconds(2500);                  // by changing this time delay between the steps we can change the rotation speed
        digitalWrite(stepPin2,LOW); 
        delayMicroseconds(2500);
        xs2++;}
      }
    if ((poszs1 < xs1)||(poszs2 < xs2)){digitalWrite(43, LOW);digitalWrite(41, LOW);}
      while ((poszs1 < xs1)||(poszs2 < xs2)){
        if(poszs1 < xs1){
          digitalWrite(stepPin1,HIGH); 
        delayMicroseconds(2500);
        digitalWrite(stepPin1,LOW); 
        delayMicroseconds(2500);
        xs1--;}
        if(poszs2 < xs2){
        digitalWrite(stepPin2,HIGH); 
        delayMicroseconds(2500);
        digitalWrite(stepPin2,LOW); 
        delayMicroseconds(2500);
        xs2--;}
      }
    
}
if ((poszs==x) && (poszs1==xs1) && (poszs2==xs2) && (ch0 == 1 || cv5 == 1)){cs0=1;
if (ch1==1 && ch0==1) {Serial.println("===Position reached!===");ch0=0;ch1=0;}
}
else{cs0=0;}
if (cs0==1 && cv3 == 1){
  delay(time);
  man += 1;
}

//=================================================================lcd======================================

lcd.begin(20, 4);

uint8_t degree[8] = {                                 //custom character                                
  0b11100,
  0b10100,
  0b11100,
  0b00000,
  0b00000,
  0b00000,
  0b00000,
  0b00000,
  };
  lcd.createChar(1, degree);


tick++;
if (tick >= 50){
  tick=0;
}
  

 }
 else {
    if (lcdv = 1){lcd.clear();}
    if (lcdv <= 10){lcdv++;}
    tick = 51;
  }

  switch (tick){
case 1 ... 24:
  lcd.setCursor(0,0);
  lcd.print("st1:");
  lcd.setCursor(4,0);
  lcd.print(posz);
  lcd.setCursor(7,0);
  lcd.print("\x01");//============
  lcd.setCursor(0,1);
  lcd.print("st2:");
  lcd.setCursor(4,1);
  lcd.print(pos1);
  lcd.setCursor(7,1);
  lcd.print("\x01");//==========
  lcd.setCursor(0,2);
  lcd.print("st3:");
  lcd.setCursor(4,2);
  lcd.print(pos2);
  lcd.setCursor(7,2);
  lcd.print("\x01");
  lcd.setCursor(0,4);
  lcd.print("st4:");
  lcd.setCursor(4,4);
  lcd.print(poszs3);
  lcd.setCursor(7,4);
  lcd.print("\x01");
  
  //===================================fusion data==================================
  lcd.setCursor(10,0);
  lcd.print("Fst1:");
  lcd.setCursor(15,0);
  lcd.print(fusionaz);
  lcd.setCursor(18,0);
  lcd.print("\x01");//==========
  lcd.setCursor(10,1);
  lcd.print("Fst1:");
  lcd.setCursor(15,1);
  lcd.print(fusiona1);
  lcd.setCursor(18,1);
  lcd.print("\x01");//=========
  lcd.setCursor(10,2);
  lcd.print("Fst2:");
  lcd.setCursor(15,2);
  lcd.print(fusiona2);
  lcd.setCursor(18,2);
  lcd.print("\x01");//==========
  lcd.setCursor(10,3);
  lcd.print("Fst3:");
  lcd.setCursor(15,3);
  lcd.print(poszs3);
  lcd.setCursor(18,3);
  lcd.print("\x01");//==========
  break;
  

case 25 ... 50:  
  lcd.setCursor(0,0);
  lcd.print("distance:");
  lcd.setCursor(10,0);
  lcd.print(length,4);
  lcd.setCursor(18,0);
  lcd.print("cm");

   lcd.setCursor(0,1);
   lcd.print("coordinate x:");
   lcd.setCursor(14,1);
   lcd.print(corx);

   lcd.setCursor(0,2);
   lcd.print("coordinate y:");
   lcd.setCursor(14,2);
   lcd.print(cory);

   lcd.setCursor(0,3);
   lcd.print("coordinate z:");
   lcd.setCursor(14,3);
   lcd.print(corz);

   lcd.setCursor(18,4);
   lcd.print(man);
   lcd.setCursor(19,4);
   lcd.print(".");
  break;

case 51:
  Serial.println(distancer, 2);
  Serial.println("=====invalid value=====");
  Serial.println("clearing memory...");
  lcd.setCursor(0,0);
  lcd.print("====================");
  lcd.setCursor(6,1) ;
  lcd.print("Invalid");
  lcd.setCursor(7,2) ;
  lcd.print("value!");
  lcd.setCursor(0,3);
  lcd.print("====================");
  delay(1000);
  x1=0; y1=0; z1=0; x2=0; y2=0; z2=0; x3=0; y3=0; z3=0; x4=0; y4=0; z4=0; x5=0; y5=0; z5=0;
  Serial.println("===Memory cleared===");
  cv6 = 1;
  cv5 = 0;
  cv3 = 0;
  cv4 = 0;
  rs0 = 1;
  cycle = 0;
  break;

}

 

delay(100);
}