//DP JI 2023

//-------------------------- VARIABLES SET UP -------------------------

int potval = 0;
float measval = 0;
int potPin = A1;
int measPin = A0;

const int numReadings   = 10;   //smoothinh
int readings [numReadings];
int readIndex   = 0;
long total  = 0;
//Variables
int aisVal  = 0;
int smoothval = 0;

const int directionPin = 3;
const int pulsePin = 4;
const int enablePin = 2;
int micross = 3000;
int micros_to_target = 100;                                       // base stepper motor speed

unsigned long previousMicros = 0UL;
unsigned long previousMicros1 = 0UL;

const int button_1_Pin = 5; 
const int button_2_Pin = 6; 
const int toggle_switch_pin = 7;

// variables will change:
int button_1_State = 0;
int button_2_State = 0;
int toggle_switch_state = 0;

int rele1pin = 8;
int rele2pin = 9;

bool dir = true, enable = false, dir_gui_manual = false, manual_performed = true;

unsigned long int target_val = 0; //[N]

int F_sila;
//Serial reading variables
int no_of_cycles, cycle_min_val, cycle_max_val, cycle_duration;
String send_to_serial, state, button_1, button_2, button_3, force_meas, demand_force; 
String terminal_input, mode = "X", goUP, goDOWN;
char mode_char;

//cylce variables set up
#define nPoints_cycle  100
float F_cycle_required[nPoints_cycle];
bool F_cycle_required_reached = false, cycling_Stop = false, new_cycle = false;
int cycle_done = 0;
int micros_cycle = 50;


//------------------------- ARDUINO SET UP -----------------------------------------------

void setup() {
  
  pinMode(directionPin, OUTPUT);
  pinMode(pulsePin, OUTPUT);
  pinMode(enablePin, OUTPUT);
  pinMode(button_1_Pin, INPUT);
  pinMode(button_2_Pin, INPUT);
  pinMode(toggle_switch_pin, INPUT_PULLUP);
  pinMode(measPin, INPUT);
  pinMode(rele1pin, OUTPUT);
  pinMode(rele2pin, OUTPUT);
  
  digitalWrite(directionPin, dir);
  digitalWrite(enablePin, HIGH);
  digitalWrite(rele1pin, HIGH);
  digitalWrite(rele2pin, HIGH);
 
  Serial.begin(500000);
  while (!Serial) {
    ; }                                     // wait for serial port to connect
  Serial.println("Start");
}

//----------------------------- MAIN LOOP ---------------------------------------------
// main loop

void loop() {
  
  getTargetValSerial();
  
  micross = 3000;
  
  digitalWrite(rele1pin, HIGH);
  digitalWrite(rele2pin, HIGH);
  digitalWrite(enablePin, HIGH);
  
  measval = analogRead(measPin);
  
  button_1_State = digitalRead(button_1_Pin);
  button_2_State = digitalRead(button_2_Pin);
  toggle_switch_state = !digitalRead(toggle_switch_pin);
    
  F_sila = getForce(smooth()); 
  state = "STANDS STILL";
  sendStatusToSerial();

  // -------------------- MANUAL ACTION -------------------------
  //If button 2 is pressed -> Motor go down
  while(button_2_State == 1){
    micross = 500;
    dir = false;
    digitalWrite(directionPin, dir);
    button_2_State = digitalRead(button_2_Pin);
    digitalWrite(enablePin, LOW);
    digitalWrite(rele1pin, LOW);
    digitalWrite(rele2pin, LOW);
    stepMotor(pulsePin, micross);
    measval = analogRead(measPin);
    smoothval = smooth();
    F_sila = getForce(smoothval);
    state = "BUTTON DOWN";
    sendStatusToSerial();
  }
  
  while(button_1_State == 1){
    micross = 2000;
    dir = true;
    digitalWrite(directionPin, dir);
    button_1_State = digitalRead(button_1_Pin);
    digitalWrite(enablePin, LOW);
    digitalWrite(rele1pin, LOW);
    digitalWrite(rele2pin, LOW);
    stepMotor(pulsePin, micross);
    measval = analogRead(measPin);
    smoothval = smooth();
    F_sila = getForce(smoothval);
    state = "BUTTON UP";
    sendStatusToSerial();
  }

  //-------------- M: mode Manual > go up/down based on user request -------------- 
  if(mode == "M" && manual_performed == false){
    digitalWrite(enablePin, LOW);
    digitalWrite(rele1pin, LOW);
    digitalWrite(rele2pin, LOW);
    
    if(dir_gui_manual == true){ 
       dir = true;
       digitalWrite(directionPin, dir);
        
        for(int i = 0; i < 1000; i++){
          getTargetValSerial();
          readVal_button_meas(); 
          F_sila = getForce(smooth());
          state = "USER pressed UP"; 
          sendStatusToSerial();
          
          stepMotor(pulsePin, micross);     
        }
    }

    if(dir_gui_manual == false){ 
       dir = false;
       digitalWrite(directionPin, dir);
        
        for(int i = 0; i < 1000; i++){
          getTargetValSerial();
          readVal_button_meas(); 
          F_sila = getForce(smooth());
          state = "USER pressed DOWN"; 
          sendStatusToSerial();
          
          stepMotor(pulsePin, micross);     
        }
    }
    manual_performed = true;
  }

  //-------------- A: mode Auto > go to force -------------- 
  while(mode == "A" && button_2_State == 0){                         
    getTargetValSerial();    
    readVal_button_meas();
    button_1_State = digitalRead(button_1_Pin);
    button_2_State = digitalRead(button_2_Pin);
    toggle_switch_state = !digitalRead(toggle_switch_pin);
    potval = analogRead(potPin);

    sendStatusToSerial();
    
    while(toggle_switch_state == 1 && mode == "A" && button_2_State == 0){
      
      smoothval = smooth();
      F_sila = getForce(smoothval);
      toggle_switch_state = !digitalRead(toggle_switch_pin);
      
      while(F_sila < 0.99*target_val && toggle_switch_state == 1 && button_2_State == 0 && mode == "A"){
  
        if(F_sila < 0.75*target_val){micross = micros_to_target;}
        else if(F_sila >= 0.75*target_val && F_sila < 0.90*target_val) {micross = micros_to_target * 2;}
        else if(F_sila >= 0.90*target_val){micross = micros_to_target * 4;};
  
        getTargetValSerial();             
        dir = true;
        digitalWrite(directionPin, dir);  
        button_2_State = digitalRead(button_2_Pin);
        toggle_switch_state = !digitalRead(toggle_switch_pin);
        digitalWrite(enablePin, LOW);
        digitalWrite(rele1pin, LOW);
        digitalWrite(rele2pin, LOW);
        
        stepMotor(pulsePin, micross);
        measval = analogRead(measPin);
        F_sila = getForce( smooth());
        state = "UP AUTO";
        sendStatusToSerial();
      }
    
      while(F_sila > 1.01*target_val && toggle_switch_state == 1 && button_2_State == 0 && mode == "A"){
        
        if(F_sila > 1.25*target_val){micross = micros_to_target;}
        else if(F_sila <= 1.25*target_val && F_sila > 1.1*target_val) {micross = micros_to_target *2;}
        else if(F_sila <= 1.1*target_val){micross = micros_to_target *4;};
  
        getTargetValSerial();
        
        dir = false;
        digitalWrite(directionPin, dir);
        button_2_State = digitalRead(button_2_Pin);
        toggle_switch_state = !digitalRead(toggle_switch_pin);
        digitalWrite(enablePin, LOW);
        digitalWrite(rele1pin, LOW);
        digitalWrite(rele2pin, LOW);
        stepMotor(pulsePin, micross);
        F_sila = getForce(smooth());
        state = "DOWN AUTO";
        sendStatusToSerial();
      }
  
      while(F_sila > 0.98*target_val && F_sila < 1.02 *target_val && toggle_switch_state == 1 && button_2_State == 0 && mode == "A"){
  
        getTargetValSerial();
        button_2_State = digitalRead(button_2_Pin);
        toggle_switch_state = !digitalRead(toggle_switch_pin);
        digitalWrite(enablePin, HIGH);
        digitalWrite(rele1pin, LOW);
        digitalWrite(rele2pin, LOW);
        F_sila = getForce(smooth());
        state = "TARGET REACHED";
        sendStatusToSerial();
       // mode = "X";
      }
    }
  }
  
  //-------------- C: mode cycling ------------------ 

 while(mode == "C" && toggle_switch_state == 1 && cycling_Stop == false && new_cycle == true){

   cycle_done = 0;
   new_cycle = false;
   digitalWrite(enablePin, LOW);
   digitalWrite(rele1pin, LOW);
   digitalWrite(rele2pin, LOW);
   toggle_switch_state = !digitalRead(toggle_switch_pin);
   getTargetValSerial();
   state = "CYCLING"; 
            
   for(int i = 0; i <= nPoints_cycle; i++){
      //CREATE CYCLE
      if( i < nPoints_cycle/2){
          F_cycle_required[i] = cycle_min_val + (i) * float(cycle_max_val - cycle_min_val) / (0.5* float(nPoints_cycle)); 
          //Serial.println(F_cycle_required[i]);
      }
      else{
         F_cycle_required[i] = cycle_max_val - ((i) - nPoints_cycle/2) * float(cycle_max_val - cycle_min_val) / (0.5* float(nPoints_cycle)); 
         //Serial.println(F_cycle_required[i]);  
      }
    }
    //DO DESIRED AMOUNT OF CYCLES
    for(int k = 1; k <= no_of_cycles; k++){

      //FOLLOW FORCE REQUIREMENT FOR WHOLE PATH
      for(int j = 0; j < nPoints_cycle; j++){
        
        getTargetValSerial();
        toggle_switch_state = !digitalRead(toggle_switch_pin);
        
        F_cycle_required_reached = false;
        F_sila = getForce(smoothval);

        //GO TO DESIRED FORCE VALUE
        while(F_cycle_required_reached == false && mode == "C" && cycling_Stop == false && new_cycle == false){
            getTargetValSerial();
            toggle_switch_state = !digitalRead(toggle_switch_pin);
            
            if(F_sila < F_cycle_required[j]){
              dir = true;
              digitalWrite(directionPin, dir); 
              state = "CYCLING UP"; 
                   
            }
            if(F_sila > F_cycle_required[j]){
              dir = false;
              digitalWrite(directionPin, dir); 
              state = "CYCLING DOWN";       
            }
            if(toggle_switch_state == true){
              stepMotor(pulsePin, micros_cycle);
              F_sila = getForce(smooth());
              
              if(F_sila > 0.96*F_cycle_required[j] && F_sila < 1.04 * F_cycle_required[j]){
                F_cycle_required_reached = true;
                
              }
            }
            target_val = F_cycle_required[j];
            sendStatusToSerial();   
        }   
      }
      cycle_done = k;
    }
    if(new_cycle == false){
      mode = "X";
    }
  }
   
}

//----------------------------FUNCTIONS----------------------------------------------------------------------------------------
//fce

void serialFlush(){
  while(Serial.available() > 0) {
    char t = Serial.read();
  }
}

void stepMotor(int pulsePinF, int microssF){
  
  unsigned long currentMicros = micros();   
  if(currentMicros - previousMicros > 4){
    digitalWrite(pulsePinF, 0);
    previousMicros = currentMicros;
  }
  
  unsigned long currentMicros1 = micros();
  if(currentMicros1 - previousMicros1 > microssF + 4){
    digitalWrite(pulsePinF, 1);
    previousMicros1 = currentMicros1;
  } 
}

long smooth() { /* function smooth */
  ////Perform average on sensor readings
  long average_val;
  
  // subtract the last reading:
  total = total - readings[readIndex];
  // read the sensor:
  readings[readIndex] = analogRead(measPin);
  // add value to total:
  total = total + readings[readIndex];
  // handle index
  readIndex = readIndex + 1;
  if (readIndex >= numReadings) {
      readIndex = 0;
  }
  // calculate the average:
  average_val = total / numReadings;
  return average_val;
}

int getForce(int smoothvalval){
  int a_sig_read = smoothvalval;
  int F = (a_sig_read - 10)* 9.4;
  if (F<0){F=0;}
  return F;
}

String sendStatusToSerial(){

  send_to_serial = String(state + "." + button_1_State + "." + button_2_State +"." + toggle_switch_state + "." + F_sila + "." + target_val + "." + millis() + "." + cycle_done  + "." + mode + ".");
  Serial.println(send_to_serial);
  return send_to_serial;
}

String getValue(String data, char separator, int index)
{
  int found = 0;
  int strIndex[] = {  0, -1  };
  int maxIndex = data.length()-1;
  for(int i=0; i<=maxIndex && found<=index; i++){
    if(data.charAt(i)==separator || i==maxIndex){
      found++;
      strIndex[0] = strIndex[1]+1;
      strIndex[1] = (i == maxIndex) ? i+1 : i;
    }
  }
  return found>index ? data.substring(strIndex[0], strIndex[1]) : "";
}

void getTargetValSerial(){
  if(Serial.available() > 0){                                                 //Serial read values
    //V4
    terminal_input = Serial.readString();
    mode = getValue(terminal_input,'.', 0);
    mode_char = mode.charAt(0);
    
    switch(mode_char){
      
      case 'M':
        goUP = getValue(terminal_input,'.', 1);
        goDOWN = getValue(terminal_input,'.', 2);
        if(goUP == "true" && goDOWN == "false"){
          dir_gui_manual = true;
        }
        if(goUP == "false" && goDOWN == "true"){
          dir_gui_manual = false;
        }
        manual_performed = false;
        
        break;
        
      case 'A':
        target_val = getValue(terminal_input,'.', 1).toInt();
        if(target_val>4000){target_val = 4000;}
        Serial.println(target_val);
        break;
        
      case 'C':
        if(getValue(terminal_input,'.', 1) == "XXXXX"){
          cycling_Stop = true;
        }
        else{cycling_Stop = false;
        }
        new_cycle = true;
        
        no_of_cycles = getValue(terminal_input,'.', 1).toInt();
        cycle_min_val = getValue(terminal_input,'.', 2).toInt();
        cycle_max_val = getValue(terminal_input,'.', 3).toInt();
        cycle_duration = getValue(terminal_input,'.', 4).toInt();
        break;
    }
  }
}

void readVal_button_meas(){
    button_1_State = digitalRead(button_1_Pin);
    button_2_State = digitalRead(button_2_Pin);
    toggle_switch_state = !digitalRead(toggle_switch_pin);
    potval = analogRead(potPin);
    
}
