#include <Servo.h>
#include <stdlib.h>
#include <stdio.h>
#include <ctype.h>
Servo Myservo;
Servo Myservo1;
const byte numChars = 50;
char receivedChars[numChars];
char tempChars[numChars];  
int caso = 0;
char messageFromPC[numChars] = {0};
int integerFromPC = 0;
int delay_led_1 = 0;
int delay_led_2 = 0;
int pos_servo_1 = 0;
int pos_servo_2 = 0;
unsigned long current_time = 0;
unsigned long current_time_2 = 0;
unsigned long current_stepper = 0;
bool led1 = LOW;
bool led2 = LOW;
bool stepper = LOW;
float floatFromPC = 0.0;
boolean newData = false;
int i = 0;
int a = 0;
int stepper_steps = 0;
char dir[10];
bool direction = LOW;
int POT1 = 0;
int POT2 = 0;
int slider = 0;
long global_steps = 0;

void Initial_menu (void);
void Read_data (void);
boolean isValidNumber(char * str);
void send_vals (void);
void sep_vals (char * str);


void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(13, OUTPUT);
  pinMode(12, OUTPUT);
  pinMode(2, OUTPUT);
  digitalWrite(2,LOW);
  pinMode(3, OUTPUT);
  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
  pinMode(A2, INPUT);
  Myservo.attach(9);
  Myservo1.attach(10);

}

void loop() {
  // put your main code here, to run repeatedly:
  switch (caso){
    case 0:
      Initial_menu();
      caso = 1;
      break;
    case 1:
      Read_data();
      if (newData == true){
        newData = false;
        switch(atoi(receivedChars)){
        case 0:
            Serial.println("Unicamente puede ingresar numeros");
            memset(receivedChars, 0, sizeof(receivedChars));
            caso = 0;
            break;
        case 1:
          Serial.println("Ingrese el LED que desea cambiar <1> o <2>");
          memset(receivedChars, 0, sizeof(receivedChars));
          caso = 2;
          break;
        case 2:
          Serial.println("Ingrese 1 para cambiar pasos y 2 para cambiar direccion");
          caso = 8;
          memset(receivedChars, 0, sizeof(receivedChars));
          break;
        case 3:
          Serial.println("Ingrese el servo que desea cambiar <1> o <2>");
          memset(receivedChars, 0, sizeof(receivedChars));
          caso = 5;
          break;
        case 4:
          send_vals();
          memset(receivedChars, 0, sizeof(receivedChars));
          caso = 0;
          break;
        case 5:
          Serial.println("Ingrese los valores en formado LED1=T,LED2=T,SERVO1=POS,SERVO2=POS,STEPS=STEPS,DIR=DIR");
          memset(receivedChars, 0, sizeof(receivedChars));
          caso = 11;
          break;
      }
      }
      break;
    case 2:
      Read_data();
      if (newData == true){
        newData = false;
        switch(atoi(receivedChars)){
          case 0:
            Serial.println("Unicamente puede ingresar numeros");
            memset(receivedChars, 0, sizeof(receivedChars));
            caso = 0;
            break;
          case 1:
            Serial.println("Ingrese el periodo del LED 1 en formato <periodo en ms>");
            memset(receivedChars, 0, sizeof(receivedChars));
            caso = 3;
            break;
          case 2:
            Serial.println("Ingrese el periodo del LED 2 en formato <periodo en ms>");
            memset(receivedChars, 0, sizeof(receivedChars));
            caso = 4;
            break;
        }
          
      }
      break;
    case 3:
      Read_data();  
      if (newData == true){
        if (isValidNumber(receivedChars)){
          delay_led_1 = atoi(receivedChars);
        }else{
          Serial.println("Unicamente pueden ingresar numeros");
        }
        
        memset(receivedChars, 0, sizeof(receivedChars));
        caso = 0;
        newData = false;
      }
      break;
    case 4:
      Read_data();  
      if (newData == true){
        if (isValidNumber(receivedChars)){
          delay_led_2 = atoi(receivedChars);
        }else{
          Serial.println("Unicamente pueden ingresar numeros");
        }
        memset(receivedChars, 0, sizeof(receivedChars));
        caso = 0;
        newData = false;
      }
      break;
    case 5:
      Read_data();
      if (newData == true){
        newData = false;
        switch(atoi(receivedChars)){
          case 0:
            Serial.println("Unicamente puede ingresar numeros");
            memset(receivedChars, 0, sizeof(receivedChars));
            caso = 0;
            break;
          case 1:
            Serial.println("Ingrese los grados del servo 1 <grados>");
            memset(receivedChars, 0, sizeof(receivedChars));
            caso = 6;
            break;
          case 2:
            Serial.println("Ingrese los grados del servo 2 <grados>");
            memset(receivedChars, 0, sizeof(receivedChars));
            caso = 7;
            break;
        }
      }
      break;
    case 6:
      Read_data();  
      if (newData == true){
        if (isValidNumber(receivedChars)){
          pos_servo_1 = atoi(receivedChars);
          if (pos_servo_1 >= 0 && pos_servo_1 <= 180){
          Myservo.write(pos_servo_1);
          }else{
            Serial.println("El valor ingresado no esta entre 0 y 180 grados");
          }
        }else{
          Serial.println("Unicamente pueden ingresar numeros");
        }
        
        memset(receivedChars, 0, sizeof(receivedChars));
        caso = 0;
        newData = false;
      }
      break;   
    case 7:
      Read_data();  
      if (newData == true){
        if (isValidNumber(receivedChars)){
          pos_servo_2 = atoi(receivedChars);
          if (pos_servo_2 >= 0 && pos_servo_2 <= 180){
          Myservo1.write(pos_servo_2);
          }else{
            Serial.println("El valor ingresado no esta entre 0 y 180 grados");
          }
        }else{
          Serial.println("Unicamente pueden ingresar numeros");
        }
        memset(receivedChars, 0, sizeof(receivedChars));
        caso = 0;
        newData = false;
      }
      break; 
    case 8:
      Read_data();
      if (newData == true){
        newData = false;
        switch(atoi(receivedChars)){
          case 0:
            Serial.println("Unicamente puede ingresar numeros");
            memset(receivedChars, 0, sizeof(receivedChars));
            caso = 0;
            break;
          case 1:
            Serial.println("Ingrese el valor de steps");
            memset(receivedChars, 0, sizeof(receivedChars));
            caso = 9;
            break;
          case 2:
            Serial.println("Ingrese la direccion <0> derecha y <1> izquierda");
            memset(receivedChars, 0, sizeof(receivedChars));
            caso = 10;
            break;
        }
      }
      break;
    case 9:
    Read_data();  
      if (newData == true){
        if (isValidNumber(receivedChars)){
          stepper_steps = atoi(receivedChars);
        }
        memset(receivedChars, 0, sizeof(receivedChars));
        caso = 0;
        newData = false;
      }
      break;
    case 10:
    Read_data();  
      if (newData == true){
        if (isValidNumber(receivedChars)){
          if (atoi(receivedChars)){
            direction = HIGH;
          }else{
            direction = LOW;
          }
          Serial.println(direction);
        }else{
          Serial.println("Unicamente pueden ingresar numeros");
        }
        memset(receivedChars, 0, sizeof(receivedChars));
        caso = 0;
        newData = false;
      }
      break;
    case 11:
    Read_data();
    if (newData == true){
      sep_vals(receivedChars);
      Myservo.write(pos_servo_1);
      Myservo1.write(pos_servo_2);
      memset(receivedChars, 0, sizeof(receivedChars));
      caso = 0;
      newData = false;
    }
      break;
  }
  if ((millis() - current_time) >= delay_led_1){
    if (led1){
      digitalWrite(2,LOW);
      led1 = LOW;
    }else{
      led1 = HIGH;
      digitalWrite(2,HIGH);
    }
    current_time = millis();
  }
  if ((millis() - current_time_2) >= delay_led_2){
    if (led2){
      digitalWrite(12,LOW);
      led2 = LOW;
    }else{
      led2 = HIGH;
      digitalWrite(12,HIGH);
    }
    current_time_2 = millis();
  }

  
digitalWrite(13,direction);
if (a < stepper_steps){
  
  if (i< 10) {
    if ((micros() - current_stepper) >= 2000){
    if (stepper){
      digitalWrite(3,LOW);
      stepper = LOW;
    }else{
      stepper = HIGH;
      digitalWrite(3,HIGH);
    }
    current_stepper = micros();
   }
   i++;
  }else{
    i = 0;
    a++;
  }
}else{
  a = 0;
  if (direction){
    global_steps = global_steps + stepper_steps;
  }else{
    global_steps = global_steps - stepper_steps;
  }
  stepper_steps = 0;
}
POT1 = analogRead(A0);
POT2 = analogRead(A1);
slider = analogRead(A2);
}

void Initial_menu (void){
  Serial.println("<1> Cambiar LEDs");
  Serial.println("<2> Cambiar stepper");
  Serial.println("<3> Cambiar servo");
  Serial.println("<4> Obtener valores del sistema");
  Serial.println("<5> Cargar valores al sistema completo");
}
void Read_data (void){
  static byte ndx = 0;
  char endMarker = '\n';
  char rc;

  while (Serial.available() > 0 && newData == false) {
      rc = Serial.read();
      if (rc != endMarker) {
          receivedChars[ndx] = rc;
          ndx++;
          if (ndx >= numChars) {
              ndx = numChars - 1;
          }
      }
      else {
          receivedChars[ndx] = '\0'; // terminate the string
          ndx = 0;
          newData = true;
      }
  }
  
}
void sep_vals(void){
  char * strtokIndx; // this is used by strtok() as an index

  strtokIndx = strtok(tempChars,",");      // get the first part - the string
  strcpy(messageFromPC, strtokIndx); // copy it to messageFromPC

  strtokIndx = strtok(NULL, ","); // this continues where the previous call left off
  integerFromPC = atoi(strtokIndx);     // convert this part to an integer

  strtokIndx = strtok(NULL, ",");
  floatFromPC = atof(strtokIndx);     // convert this part to a float
}

boolean isValidNumber(char* str){
  int check = 0;
  for(byte i=0;i<sizeof(str);i++)
  {
  if(isalpha(str[i])) check++;
  }
  if (check>0) {
    return false;
  } else {
    return true;
  }
}

void send_vals (void){
  Serial.print("Servo1=");
  Serial.print(pos_servo_1);
  Serial.print(",");
  Serial.print("Servo2=");
  Serial.print(pos_servo_2);
  Serial.print(",");
  Serial.print("LED1=");
  Serial.print(delay_led_1);
  Serial.print(",");
  Serial.print("LED2=");
  Serial.print(delay_led_2);
  Serial.print(",");
  Serial.print("Stepper steps=");
  Serial.print(global_steps);
  Serial.print(",");
  Serial.print("Stepper dir=");
  Serial.println(direction);
  Serial.print("POT1=");
  Serial.print(POT1);
  Serial.print(",");
  Serial.print("POT2=");
  Serial.print(POT2);
  Serial.print(",");
  Serial.print("slider=");
  Serial.println(slider);
}
void sep_vals (char * str){
  char * aux_str;
  char * aux_str_2;
  char * aux_str_3;
  char * aux_str_4;
  char * aux_str_5;
  char * aux_str_6;
  aux_str = strtok(str,",");
  aux_str_2 = strtok(NULL,",");
  aux_str_3 = strtok(NULL,",");
  aux_str_4 = strtok(NULL,",");
  aux_str_5 = strtok(NULL,",");
  aux_str_6 = strtok(NULL,",");
  strtok(aux_str,"=");
  delay_led_1 = atoi(strtok(NULL,"="));
  strtok(aux_str_2,"=");
  delay_led_2 = atoi(strtok(NULL,"="));
  strtok(aux_str_3,"=");
  pos_servo_1 = atoi(strtok(NULL,"="));
  strtok(aux_str_4,"=");
  pos_servo_2 = atoi(strtok(NULL,"="));
  strtok(aux_str_5,"=");
  stepper_steps = atoi(strtok(NULL,"="));
  strtok(aux_str_6,"=");
  if (atoi(strtok(NULL,"="))==1){
   direction = HIGH;
  }else{
    direction = LOW;
  }

}