#include "config.h"

void setup() {
  system_setup();
  SD_setup();
  motor_setup();
  MPU_setup();
  SR04_setup();
  alarm_setup();
  display_setup();
  task_setup();
}

void loop() {
  runnerTask.execute();
  MPU_reading();
  SR04_reading();

  control();

  if (state_debug) { serial_debug(); }
  if (state_read) { readFromSD(); state_read=false; }
  if (state_par_wr) {

    int str_len = str_data.length();
    char buff[str_len];
    str_data.toCharArray(buff,str_len);
    
    if (StrContains(buff,"i")){
      int idx1 = str_data.indexOf('i');
      String geti = str_data.substring(idx1+1);
      INTERVAL = geti;
    } else if (StrContains(buff,"d")){
      int idx2 = str_data.indexOf('d');
      String getd = str_data.substring(idx2+1);
      DISTANCE = getd;
    } else if (StrContains(buff,"t")){
      int idx3 = str_data.indexOf('t');
      String gett = str_data.substring(idx3+1);
      TEMP = gett;
    }

    String write_data = INTERVAL+";"+DISTANCE+";"+TEMP+"\n";
    Serial.print(write_data);

    writeParToSD(write_data);
    delay(500);
    readParFromSD();
    state_par_wr = false;
  }
  if (state_par_rd) { 
    readParFromSD();
    state_par_rd = false;
  }
  if (state_help) {
    String help_str;
    help_str += "----------------------------------HELP----------------------------------\n";
    help_str += "help*; -> help\n";
    help_str += "db.on*; -> Enable debug\n";
    help_str += "db.off*; -> Disable debug\n";
    help_str += "log.on*; -> Run recording data\n";
    help_str += "log.off*; -> Stop recording data\n";
    help_str += "log.read*; -> Read recorded data\n";
    help_str += "par.wr*; -> Write parameter value\n";
    help_str += "par.rd*; -> Read parameter value\n";
    help_str += "----------------------------------end-----------------------------------\n";
    Serial.print(help_str);
    state_help = false;
  }

  display_processing();
  
  serial_com();
}