/*
serial config:
Board: Arduiono Pro / Pro Mini
Port:  tty.usbseriala400eMNr
Programmer: USBtinyISP
*/

// Definitionen
#include <Servo.h> // servo treiber
#include <EEPROM.h>

#define EEPROM_ADDR_ARRAY_STEP 0
#define EEPROM_ADDR_ARRAY_MAX  2
#define EEPROM_ADDR_JOINT0     4
#define EEPROM_ADDR_JOINT1     364
#define EEPROM_ADDR_JOINT2     724
#define EEPROM_ADDR_JOINT3     1084

Servo servo_0;
Servo servo_1;
Servo servo_2;
Servo servo_3;

int sensorPin0 = A0;    // Schulter
int sensorPin1 = A1;    // Handfind
int sensorPin2 = A2;    // Ellbogen
int sensorPin3 = A3;    // Zange
int count0, arrayStep, arrayMax, countverz, Taster, stepsMax, steps, time = 1000, del = 1000, temp;
// arraystep = memory what pos in the array
// arrayMax = max steps we safed to array
// countverz = seems to be something to calculate the delay between complete moves
// Taster = Button
// stepsMax = longest way a servo have to travel
// steps = single steps for a move between stored positions
unsigned int  verz = 0;

long previousMillis1 = 0;
long previousMillis2 = 0;
long previousMillis3 = 0;
long previousMillis4 = 0;
long previousMicros = 0;
unsigned long currentMillis = millis();
unsigned long currentMicros = micros();

// arrays
int Delay[7] = {0,0,1,3,15,60,300}; // array to map gripper pot to delay in seconds
int SensVal[4]; // sensor value
float dif[4], ist[4], sol[4],  dir[4]; // difference between stored position and momentary position
int joint0[90];// array for servo(s)
int joint1[90];
int joint2[90];
int joint3[90];
int top = 89; // we should not write over the end from a array
// status 
boolean playmode = false, Step = false;

void setup()
{
  pinMode(4, INPUT_PULLUP);  // sets the digital pin 4 as input
  pinMode(6, INPUT_PULLUP);
  pinMode(7, INPUT_PULLUP);
  pinMode(8, INPUT_PULLUP);
  pinMode(13, OUTPUT);  // sets the digital pin 13 as outtput
  digitalWrite(13, HIGH);   // sets the LED on
  servo_0.attach(3); // attaches the servo
  servo_1.attach(10);
  servo_2.attach(9);
  servo_3.attach(11);
  Serial.begin(115200); // Baudrate have to be same on the IDE
  Serial.println("mini robot ready...");     
  //delay(1000);
  digitalWrite(13, LOW);
  loadDataFromEEPROM();
}


void loop() // here we go!
{
  currentMillis = millis(); // all is about timing
  currentMicros = micros();
  
  // read the button  
  Button();
  
  if(!playmode) // manualy modus
  {        
    if(currentMillis - previousMillis1 > 25) // 25miliseconds until next manual mode update
    {
      if (arrayStep < top) 
      {
        previousMillis1 = currentMillis; //reset
        readPot(); // get the value from potentiometers
        mapping(); // map to milliseconds for servos
        move_servo(); // setz newservo position
        saveDataToEEPROM();
        //record();   
      } // end counter < max
    } // end step check
  } // ende manualy move
   
  else if(playmode) // play
  {
    if (Step) // next step read from array
    {
      digitalWrite(13, HIGH); //LED
      if (arrayStep < arrayMax) // we not reach the end from stored data
      {
        arrayStep += 1; // next array pos
        Read(); // from the arrays
        calculate(); // find biggest travel distance and calculate the other 3 servos (the have to do smaler steps to be finished at same time!)
        Step = 0;
        digitalWrite(13, LOW);  
      }
      else // array read finished > start over
      {
        arrayStep = 0; // 
        calc_pause(); // delay between moves read from potentiometer
        countverz = 0; // used for the delay
        while(countverz < verz) // verz = time getting from calc_pause();
        { // here we do loop and wait until next start over
          countverz += 1;
          calc_pause();
          digitalWrite(13, HIGH); delay(25);   
          digitalWrite(13, LOW); delay(975); 
        }
      }
      Serial.println(arrayStep);
    }
    else // do the servos!
    {
      if (currentMicros - previousMicros > time) // here we do a single micro step
      { // 
        if(arrayMax == arrayStep){
          playmode = 0;
        }
        previousMicros = currentMicros;
        play_servo(); 
       // Serial.println("SERVO IS play");
      }
    }
  }// ende playmode

// ---------------------------------------------------------------------------------Hardware pause switch PIN 6
    while (digitalRead(4) == false)
      { 
        digitalWrite(13, HIGH); delay(500);   
        digitalWrite(13, LOW); delay(500);
      }
// ---------------------------------------------------------------------------------- Textout serial
    // serial ausgabe 1 sek
    if(currentMillis - previousMillis2 > 5000)
    { 
      previousMillis2 = currentMillis;
     count0 = 0;
      while(count0 < 4)
      {
        int val = SensVal[count0];
      // val = map(val, 142, 888, 0, 180);
       // Serial.println(val);
        //Serial.println("test");
        count0 += 1;
      }
      Serial.print("playmode: "); 
      Serial.println(playmode); 
      Serial.print("playmode: "); 
      Serial.print("arrayStep: "); 
      Serial.println(arrayStep);    
      //Serial.println(arrayMax);    
      Serial.println(" ");    
    }
}

// ---------------------------------------------------------------------------------------- sub routinen
void calc_pause() // read pot and map to usable delay time after a complete move is done
{
    readPot();
    temp = SensVal[3];
    if (temp < 0) temp = 0;
    temp = map(temp, 0, 680, 0 ,6); 
    verz = Delay[temp]; // verz = delay in second
    /*Serial.print(temp);
          Serial.print(" ");
          Serial.print(verz);
          Serial.print(" ");
          Serial.println(countverz);*/
}

void readPot() // read analog inputs and add some offsets (mechanical corrections)
{
   SensVal[0] = analogRead(sensorPin0); //SensVal[0] += -10; // rotate
   SensVal[1] = analogRead(sensorPin1); //SensVal[1] += 280; // Shoulder
   SensVal[2] = analogRead(sensorPin2); //SensVal[2] += -50; // hand
   SensVal[3] = analogRead(sensorPin3); // SensVal[3] += 0;// gripper
//  Serial.print(SensVal[2]);Serial.print(" "); // CHECK
}
void mapping() // we need microsecond for the servos instead potentiometer values
{
  ist[0] = map(SensVal[0], 150, 900, 600, 2400);//  drehen
  ist[1] = map(SensVal[1], 1000, 100, 550, 2400);// Schulter
  ist[2] = map(SensVal[2], 120, 860, 400, 2500);// Hand
  ist[3] = map(SensVal[3], 1023, 0, 500, 2500);// Zange
// Serial.println(ist[2]); // CHECK
}
void record()
{
    joint0[arrayStep] = ist[0]; // write positions in servo array
    joint1[arrayStep] = ist[1];
    joint2[arrayStep] = ist[2];
    joint3[arrayStep] = ist[3];
}
void Read()
{
    sol[0] = joint0[arrayStep]; // read from the array
    sol[1] = joint1[arrayStep];
    sol[2] = joint2[arrayStep];
    sol[3] = joint3[arrayStep];
}
void move_servo()
{       
  servo_0.writeMicroseconds(ist[3]); // send milissecond values to servos
  servo_1.writeMicroseconds(ist[2]); 
  servo_2.writeMicroseconds(ist[0]); 
  servo_3.writeMicroseconds(ist[1]); 
}

// ------------------------------------------------------------ single steps calculating
void calculate()
{
      // travel distance for each servo
      dif[0] = abs(ist[0]-sol[0]);
      dif[1] = abs(ist[1]-sol[1]);
      dif[2] = abs(ist[2]-sol[2]);
      dif[3] = abs(ist[3]-sol[3]);

      // biggest travel way from all 4 servos
      stepsMax = max(dif[0],dif[1]);
      stepsMax = max(stepsMax,dif[2]);
      stepsMax = max(stepsMax,dif[3]);
      // stepsMax is the biggest distance a servo have to do beween momentary position and new pos read from the array
      
      //Serial.println(stepsMax); 
      
      if (stepsMax < 500) // del(ay) between a single step is bigger is move is smaler. just looks cool
        del = 1200;
      else
        del = 600;
      
       // calculating single (micro) step for each servo
       // need that to do move all servos in a loop (stepsMax times done) with different values.
       // This makes all servos have done the traveling distance at same time
      if (sol[0] < ist[0]) dir[0] = 0-dif[0]/stepsMax; else dir[0] = dif[0]/stepsMax;
      if (sol[1] < ist[1]) dir[1] = 0-dif[1]/stepsMax; else dir[1] = dif[1]/stepsMax;
      if (sol[2] < ist[2]) dir[2] = 0-dif[2]/stepsMax; else dir[2] = dif[2]/stepsMax;
      if (sol[3] < ist[3]) dir[3] = 0-dif[3]/stepsMax; else dir[3] = dif[3]/stepsMax;
        //Serial.println(dir4); 

}
void play_servo()
{
    steps += 1;
    if (steps < stepsMax) // sure we not reach the end from a move
    {
      //time = del*5;// anfahr rampe
      if(steps == 20) time = del*4;         // ramp up 
      else if(steps == 40) time = del*3;    // time is the delay in microsecns we wait in the mainloop until
      else if(steps == 80) time = del*2;    // a micro step will be done
      else if(steps == 100) time = del-1;    // cannot explain here is not del*1
      
      if(steps == stepsMax-200) time = del*2;        // stop ramp down (200 microsteps before end time will be increased
      else if(steps == stepsMax-80) time = del*3;
      else if(steps == stepsMax-40) time = del*4;
      else if(steps == stepsMax-20) time = del*5;
      
      ist[0] += dir[0]; // set new pos
      ist[1] += dir[1];
      ist[2] += dir[2];
      ist[3] += dir[3];

      servo_0.writeMicroseconds(ist[3]); // Zange //anschlüsse gemappt!
      servo_1.writeMicroseconds(ist[2]); // Hand
      servo_2.writeMicroseconds(ist[0]); // Schulter
      servo_3.writeMicroseconds(ist[1]); // Ellbogen
    }
    else
    {
      Step = 1; // next step aus array lesen
      steps = 0; // servo zwischenschritte
    }
}

void data_out() // just to write the recorded data to serial
{
  int i = 0;
  while(i < arrayMax)
  {
    digitalWrite(13, HIGH);
    i += 1;
    Serial.print(joint0[i]); Serial.print(", ");
  }
  Serial.println("Joint0");
  i = 0;
  while(i < arrayMax)
  {
    digitalWrite(13, HIGH);
    i += 1;
    Serial.print(joint1[i]); Serial.print(", ");
  }
  Serial.println("Joint1");
  i = 0;
  while(i < arrayMax)
  {
    digitalWrite(13, HIGH);
    i += 1;
    Serial.print(joint2[i]); Serial.print(", ");
  }
  Serial.println("Joint2");
  i = 0;
  while(i < arrayMax)
  {
    digitalWrite(13, HIGH);
    i += 1;
    Serial.print(joint3[i]); Serial.print(", ");
  }
  saveDataToEEPROM();
  //Serial.println("Joint3");
}
void clear_positions() {
  // Clear all arrays used for storing joint positions
  memset(joint0, 0, sizeof(joint0));
  memset(joint1, 0, sizeof(joint1));
  memset(joint2, 0, sizeof(joint2));
  memset(joint3, 0, sizeof(joint3));

  // Reset array step and max
  arrayStep = 0;
  arrayMax = 0;

  // Indicate positions cleared (optional)
  Serial.println("Recorded positions cleared.");
}

void saveDataToEEPROM() {
  // حفظ قيم arrayStep و arrayMax
  EEPROM.put(EEPROM_ADDR_ARRAY_STEP, arrayStep);
  EEPROM.put(EEPROM_ADDR_ARRAY_MAX, arrayMax);
  
  // حفظ مصفوفات المفاصل
  for (int i = 0; i < arrayMax; i++) {
    EEPROM.put(EEPROM_ADDR_JOINT0 + i * sizeof(int), joint0[i]);
    EEPROM.put(EEPROM_ADDR_JOINT1 + i * sizeof(int), joint1[i]);
    EEPROM.put(EEPROM_ADDR_JOINT2 + i * sizeof(int), joint2[i]);
    EEPROM.put(EEPROM_ADDR_JOINT3 + i * sizeof(int), joint3[i]);
  }
  
  // تأكيد الحفظ
 
  
  // Serial.println("تم حفظ جميع البيانات في EEPROM");
}

void loadDataFromEEPROM() {
  // استرجاع قيم arrayStep و arrayMax
  EEPROM.get(EEPROM_ADDR_ARRAY_STEP, arrayStep);
  EEPROM.get(EEPROM_ADDR_ARRAY_MAX, arrayMax);
  
  // استرجاع مصفوفات المفاصل
  for (int i = 0; i < arrayMax; i++) {
    EEPROM.get(EEPROM_ADDR_JOINT0 + i * sizeof(int), joint0[i]);
    EEPROM.get(EEPROM_ADDR_JOINT1 + i * sizeof(int), joint1[i]);
    EEPROM.get(EEPROM_ADDR_JOINT2 + i * sizeof(int), joint2[i]);
    EEPROM.get(EEPROM_ADDR_JOINT3 + i * sizeof(int), joint3[i]);
  }
  
  Serial.println("تم استرجاع البيانات من EEPROM");
}


void Button() // check buttons for single and doubleclick
{
 

  if (digitalRead(7) == LOW) {
    delay(50);  // تأخير بسيط للتأكد من عدم الضغط المتكرر
    loadDataFromEEPROM();  // استرجاع البيانات المخزنة مسبقًا
    if (arrayMax > 0) {
      arrayStep = arrayMax;  // البدء من آخر وضعية مسجلة
    }
    arrayStep += 1;
    arrayMax = arrayStep;
    record();
    Taster = 0;
    playmode = false;
    Serial.print("تسجيل الخطوة: "); Serial.println(arrayStep);
    digitalWrite(13, HIGH);
    delay(100);
    digitalWrite(13, LOW);
    saveDataToEEPROM();
    while (digitalRead(7) == LOW) {}  // الانتظار حتى يتم رفع الزر
    delay(200);  // تأخير لمنع التبديل السريع
  }
    if (digitalRead(6) == LOW) {
    delay(50);  // Short delay to avoid multiple triggers
      loadDataFromEEPROM();  // Load data stored in EEPROM
      playmode = true;
      Taster = 0;
      for (int i = 0; i < arrayMax; i++) {
        Step = i + 1;
        Serial.print("Now playing position number: ");
        Serial.println(Step);
        Serial.print("Servo 0 angle: ");
        Serial.println(joint0[i]);
        Serial.print("Servo 1 angle: ");
        Serial.println(joint1[i]);
        Serial.print("Servo 2 angle: ");
        Serial.println(joint2[i]);
        Serial.print("Servo 3 angle: ");
        Serial.println(joint3[i]);
        
        // Move servos to the stored positions
        servo_0.write(joint0[i]);
        servo_1.write(joint1[i]);
        servo_2.write(joint2[i]);
        servo_3.write(joint3[i]);
        
        delay(1000);  // Delay between each position
      }
      playmode = false;
      digitalWrite(13, LOW);
    while (digitalRead(6) == LOW) {}  // Wait for button release
    delay(200);  // Delay to prevent rapid switching
  }
    
  if (digitalRead(8) == LOW) {
    delay(50);  // Short delay to avoid multiple triggers
    clear_positions();
    for (int i = 0; i < EEPROM.length(); i++) {
      EEPROM.write(i, 0);  // Clear all EEPROM data
    }
    arrayStep = 0;
    arrayMax = 0;
    Serial.println("All positions cleared from EEPROM. Ready to record new positions.");
    digitalWrite(13, HIGH);  // Turn on LED to indicate cleared state
    delay(500);
    digitalWrite(13, LOW);
    while (digitalRead(8) == LOW) {}  // Wait for button release
    delay(200);  // Delay to prevent rapid switching
  }

    
   
    /*if (Taster == 3)
    {
      // ++ arrayStep
      // playmode = 1;
      Taster = 0;
      Serial.println("Clear ");
    }*/
    if (currentMillis - previousMillis3 > 2000) // button Status clear
    {
      Taster = 0;
      //Serial.println("restart ");
    }
}