// Prueba para comunicarme con el software que hice en Matlab App Designer.
// Envía por BT un número natural que se incrementa en uno una vez por segundo.
// También lee el puerto BT e imprime la string completa (que le llega por el ppuerto BT) por el puerto serie USB.
// Made by Juan 02/2025.

#include <arduino-timer.h>
// https://deepbluembedded.com/arduino-timer-library/#google_vignette
// https://github.com/contrem/arduino-timer
Timer<1, millis> MyTimer;

#include <SoftwareSerial.h>
const byte rxPin = 2;
const byte txPin = 3;
// Set up a new SoftwareSerial object
SoftwareSerial BTSerial (rxPin, txPin);

constexpr byte numChars = 64;
char readCstring[numChars];
boolean finished_reading_serial = false;

float Kp = 0.1;
float Ki = 0.1;
float Kd = 0.1;

void setup() {
  MyTimer.every(1000, Task_time_up_Handler);
  // Define pin modes for TX and RX
  pinMode(rxPin, INPUT);
  pinMode(txPin, OUTPUT);

  Serial.begin(9600);                     //  Start the harware serial port for USB connections.
  BTSerial.begin(115200);                 //  Start the software port for bluetooth connections.
  delay(1000);

  Serial.println("Communication starting.");
}

void loop() {
  MyTimer.tick();                             // Every second a number is sent through BT.
  
  check_and_read_1_char_serial_BT();
  if (finished_reading_serial == true) {
    Serial.print("Recieved:");
    Serial.println(readCstring);
    update_variables_from_serial_BT();
    print_PID();
    finished_reading_serial = false;
  }   
}

void check_and_read_1_char_serial_BT()
// Respeta lo que dice:
// https://forum.arduino.cc/t/serial-input-basics-updated/382007/2
// By Juan 02/2025.
{
  static byte i = 0;
  char endMarker = '\n';      // endMarker = New Line (NL). En Matlab es el LF (line feed).
  char recievedChar;
  while (BTSerial.available() > 0 && finished_reading_serial == false) {
        recievedChar = BTSerial.read();
        if (recievedChar != endMarker) {
            readCstring[i] = recievedChar;
            i++;
            if (i >= numChars) {
                i = numChars - 1;
                Serial.println("Error, buffer overflow.");
            }
        }
        else {
            readCstring[i] = '\0'; // terminate the string
            i = 0;
            finished_reading_serial = true;
        }
  }
}

void update_variables_from_serial_BT()
{
  if(readCstring[0] == 'K') {
    switch(readCstring[1]) {
      case 'p':
        Kp = atof(readCstring + 2);
        break;
      case 'i':
        Ki = atof(readCstring + 2);
        break;
      case 'd':
        Kd = atof(readCstring + 2);
        break;
      default:
        Serial.println("Error: not Kp, Ki, or Kd.");
        break;
    }
  }
  else {
    Serial.println("Error: no K.");
  }
  //finished_reading_serial = false;
}

void print_PID() {  
  //PID.setK(Kp, Ki, Kd);
  Serial.print("Kp=");
  Serial.print(Kp);
  Serial.print(" , Ki=");
  Serial.print(Ki);
  Serial.print(" , Kd=");
  Serial.println(Kd);
}

bool Task_time_up_Handler(void *)
{
  static int i = 0;     // i is initialized with 0 only once, after thatn it retains its current value.
  BTSerial.write(i);    // write passes the whole number in binary. https://arduino.stackexchange.com/questions/10088/what-is-the-difference-between-serial-write-and-serial-print-and-when-are-they
  BTSerial.write('\n')
  //BTSerial.println(i);  // print passes every digit as a character in binary.
  i++;
  return true;          // Always return true.
}