/*  NAVIGATION

- Goal: to get the RoboDuck 3000 from A to B.
- Output: 2 motors via PWM on different GPIO pins
- Input: Info from geo-location sensor via UART
- Power: via `Server/ brain` (5V, GND, I2C)
- Input: Commands from `Server/ brain` via I2C

Commands:

- in: `STATUS_` -> out: `READY_` (before the start), `OK_` or `NOK_` (during)
- in: `NAVLEFT_`, `NAVRIGHT_` --> out: `ACK_` (free navigation during game)
- in: `WAYPOINT_<coordinate>` --> out: `ACK_` (coordinates of a waypoint)
- in: `SPEED_<number>` --> out: `ACK_` (speed in range of [0,100]; 0 no speed; 100 max speed)
- in: `GEOFENCE_<coordinate>_<radius>` --> out: `ACK_` (coordinate of circle with radius in meters)
- in: `STOP_` --> out: `ACK_` (stops everything)
- in: `HOME_` --> out: `ACK_` (goes to the HOME position)
- in: `RC_` --> out: `ACK_` (free navigation during this mode)

Tasks:

- startup: when the geo-location sensor provides a (reliable) position store it as (default) `HOME` position
- it loops and scans for incoming messages
- in the mean time it keeps track of where it is, how fast it's going over ground (SOG) and course over ground (COG)
- it calculates the thrust for both motors based on the direction and speed it has to reach
- it does not move outside it's geofenced area (circle based)
- if the last received position has been reached it will stay there (margin is 5 meters)
- IF no request for update was received in the last minute THEN it will just STOP!

Interrupts:
Important reference for this code https://docs.arduino.cc/language-reference/en/functions/external-interrupts/attachInterrupt/
Why? Because it explains why I just read the command after a message from the brain via i2C. The code then just flags that a command has
been received and processes it as soon as possible not allowing any intterupts at that time. 
I had to do this because I tried to do all this work within the interrupt of the I2C; bad idea
Arduino also says keep it short in https://docs.arduino.cc/language-reference/en/functions/communication/wire/onRequest/
*/

#include "RDgps.h"

#include <Arduino.h>
#include <SoftwareSerial.h>
#include <Wire.h> // for I2C


/* DEFINITIONS */
const int MOTOR_LEFT  = D3; // left motor
const int MOTOR_RIGHT = D9; // right motor
int MAX_VALUE_MOTORS = 255;

#define I2C_ADDRESS 0x10 // the address of this unit
#define MAX_SIZE_I2C_BUFFER 96 // to include coordinates later on

volatile bool newI2cCommandAvailable = false; // flag that's set true when new message from brain needs to be processed
char pendingCommand[MAX_SIZE_I2C_BUFFER]; // any command from the brain

// The serial connection to the GPS device
SoftwareSerial ss(1, 0); // RX/TX

/* ***** MOTOR HANDLING  ******** */
int CURRENT_MAX_POWER = 50; // for safety we set it to 50%
int CURRENT_LEFT_POWER = 0; // we start with no power on the motors; this max is the same for both motors
int CURRENT_RIGHT_POWER = 0;

// Yesterday I found out that each motor will start at a different duty cycle because of frictions
// I therefore want to have a seperate minimal duty setting per motor so I can compensate later on
int MIN_LEFT_PWM_DUTY = 60; // the left motor runs a bit smoother than the right and therefore will start rotation earlier
int MIN_RIGHT_PWM_DUTY = 75; // the right motor needs more duty on its GPIO pin

int CURRENT_LEFT_PWM_DUTY = 0; // can be maximum 255
int CURRENT_RIGHT_PWM_DUTY = 0;

int newPwmLeft = 0; // just to be local now global; means the setting we want it to be
int newPwmRight = 0; // 
unsigned long lastRamp = 0; // to check whether it's time to adjust pwm again
int PWM_ADJUSTMENT_DELAY = 5; // 5 msec between updates to prevent spikes in current

// Navigating left or right means I'll adjust a general counter between -100 and 100.
// -100 means all the way left, 0 means centre , 100 means all the way right
int CURRENT_HEADING = 0; // I use 0 as centre. 

void adjust2MotorsPWM () {

  if (millis() - lastRamp < PWM_ADJUSTMENT_DELAY) return; // if not 5 msec gone by then return
  lastRamp = millis();
  
  //Serial.print("actual PWM left=");
  //Serial.print(CURRENT_LEFT_PWM_DUTY);
  //Serial.print(" right=");
  //Serial.println(CURRENT_RIGHT_PWM_DUTY);

  // First change the left motor
  if (CURRENT_LEFT_PWM_DUTY < newPwmLeft) { // increase if the new value is higher
    CURRENT_LEFT_PWM_DUTY++;
  } else if (CURRENT_LEFT_PWM_DUTY > newPwmLeft) { // else decrease
    CURRENT_LEFT_PWM_DUTY--;
  }
  
  // Then we change the right motor's PWM
  if (CURRENT_RIGHT_PWM_DUTY < newPwmRight) {
    CURRENT_RIGHT_PWM_DUTY++;
  } else if (CURRENT_RIGHT_PWM_DUTY > newPwmRight) {
    CURRENT_RIGHT_PWM_DUTY--;
  }

  // adjust the pwm's almost at the same time
  analogWrite(MOTOR_LEFT, CURRENT_LEFT_PWM_DUTY);
  analogWrite(MOTOR_RIGHT, CURRENT_RIGHT_PWM_DUTY); 
}

void setMotors(int leftPower, int rightPower) {
  // Input parameters (leftPower, rightPower) are values in [0, 100]%

  // First calculate the safety-capped maximum PWM limit for each motor based on CURRENT_MAX_POWER.
  // Because when max power is set to 50% for instance then the max PWM can be no more than 126
  int maxPwmLeft = MIN_LEFT_PWM_DUTY + ((MAX_VALUE_MOTORS - MIN_LEFT_PWM_DUTY) * CURRENT_MAX_POWER / 100);
  int maxPwmRight = MIN_RIGHT_PWM_DUTY + ((MAX_VALUE_MOTORS - MIN_RIGHT_PWM_DUTY) * CURRENT_MAX_POWER / 100);

  // 2. Calculate PWM for the Left Motor
  if (leftPower > 0) {
    // Map the incoming 0-100% to the PWM range for the left motor
    newPwmLeft = map(leftPower, 0, 100, MIN_LEFT_PWM_DUTY, maxPwmLeft);
  } else {
    newPwmLeft = 0; // just to be sure we then stop the motor
  }

  // do the same for right motot
  if (rightPower > 0) {
    newPwmRight = map(rightPower, 0, 100, MIN_RIGHT_PWM_DUTY, maxPwmRight);
  } else {
    newPwmRight = 0; // Explicitly force 0 PWM so the motor completely cuts power and stops
  }

  // Save the new power values for later
  CURRENT_LEFT_POWER = leftPower;
  CURRENT_RIGHT_POWER = rightPower;

  // Smoothly adjust the pwm for both motors
  // adjust2MotorsPWM(newPwmLeft, newPwmRight); 

  Serial.print("leftPower=");
  Serial.print(leftPower);
  Serial.print(" rightPower=");
  Serial.print(rightPower);
  Serial.print(" maxPower=");
  Serial.print(CURRENT_MAX_POWER);
  Serial.print(" pwmLeft=");
  Serial.print(newPwmLeft);
  Serial.print(" pwmRight=");
  Serial.println(newPwmRight);
}

void applyCurrentHeading() {
  // now we have a new heading in mind; let's translate this to power on the motors
 
  int leftPower = 100; // always try to go to the fullest
  int rightPower = 100;

   // let's say direction 0; then left will be 100 and so will right
   // if direction is -45 it means left will be get 100-45 power and right stays the same

   if (CURRENT_HEADING < 0) { // but to turn left reduce the power on the left motor
    leftPower = 100 + CURRENT_HEADING;
  } else if (CURRENT_HEADING > 0) { // and vice versa
    rightPower = 100 - CURRENT_HEADING;
  }

  // power doesn't mean full 255 on the PWM ... because it also depends on the max speed that is applied and difference in motors
  setMotors(leftPower, rightPower);
}

void changeSpeed(int newMaxPower) {
  // make sure it stays in range [0,100]
  // user can say 0, 10, 30, 50, 70 or 100
  if (newMaxPower < 0)   newMaxPower = 0;
  if (newMaxPower > 100) newMaxPower = 100;

  // Update the limit
  CURRENT_MAX_POWER = newMaxPower;

  if (CURRENT_MAX_POWER == 0) {
    setMotors(0,0);
    return; // this is to prevent that this algoritme will still stake the base values
  }

  // Then recalculate the motor values again based on current heading
  // So we don't change direction but we just change PWM's
  applyCurrentHeading();
  
}

void changeDirection(int deltaDirection) {
  // calculates the new power settings per motor based on the command 
  // input is decrease or increase of the general steering (value between -100 and 100)
  // -100 means that left motor will have 0% and the right will have 100%
  // 0 means that both of them will have 50% of max power
  // 100 means that right motor will have 0% and the left will have 100%
  
  int newDirection = CURRENT_HEADING + deltaDirection;
  
  if (newDirection < -100) newDirection = -100;
  if (newDirection > 100)  newDirection = 100;
  
  CURRENT_HEADING = newDirection;

  Serial.print("New heading: ");
  Serial.println(newDirection);

  if (CURRENT_MAX_POWER == 0) {
    setMotors(0, 0);
    return;
  }

  applyCurrentHeading();

}

void setup_PWM (int resolution, int frequency, int max_value) {

  // resolution and frequency will be set for all GPIO pins =  2 motors
  Serial.print("Setting frequency to: ");
  Serial.println(frequency);
  analogWriteFreq(frequency);

  Serial.print("Setting resolution to: ");
  Serial.print(resolution);
  Serial.println(" bits");
  analogWriteResolution(resolution); // sets the resolution

  MAX_VALUE_MOTORS = max_value;
}

/* ******* COMMANDS *********** */

char last_cmd[MAX_SIZE_I2C_BUFFER];
char last_response[MAX_SIZE_I2C_BUFFER];

void clearBuffer(char* buffer) {
  for(int i=0; i<MAX_SIZE_I2C_BUFFER; i++) {
    buffer[i] = 0;
  }
}

// got compiler warning without the const keyword because compiler now understand I'm passing constant strings
void setCmdResponse(const char* new_cmd, const char* new_response) {
  clearBuffer(last_cmd);
  strcpy(last_cmd,new_cmd);
  clearBuffer(last_response);
  strcpy(last_response, new_response);
}
/*

- in: `STATUS_` -> out: `READY_` (before the start), `OK_` or `NOK_` (during)
- in: `NAVLEFT_`, `NAVRIGHT_` --> out: `ACK_` (free navigation during game)
- in: `WAYPOINT_<coordinate>` --> out: `ACK_` (coordinates of a waypoint)
- in: `SPEED_<number>` --> out: `ACK_` (speed in range of [0,100]; 0 no speed; 10 max speed)
- in: `GEOFENCE_<coordinate>_<radius>` --> out: `ACK_` (coordinate of circle with radius in meters)
- in: `STOP_` --> out: `ACK_` (stops everything)
- in: `HOME_` --> out: `ACK_` (goes to the HOME position)
- in: `RC_` --> out: `ACK_` (free navigation during this mode)
*/

bool isStatusCmd(char* token) {
  if(strcmp(token,"STATUS") == 0) {
    setCmdResponse("STATUS", "RDY_");
    return true;
  }
  return false;
}

bool isNavCmd(char* token) {

  if (strcmp(token,"NAVLEFT") == 0) {
    Serial.println("nav command: go left");
    changeDirection(-10);

    setCmdResponse("NAVLEFT","ACK_");
    return true;
  } else if (strcmp(token,"NAVRIGHT") == 0) {
    Serial.println("nav command: go right");
    changeDirection(10);

    setCmdResponse("NAVRIGHT","ACK_");
    return true;
  }
  return false;
}

bool isStopCmd(char* token) {
  if (strcmp(token,"STOP") == 0) {
    CURRENT_HEADING = 0;
    CURRENT_MAX_POWER = 0;    
    setMotors(0,0);
    setCmdResponse("STOP","ACK_");
    return true;
  }
  return false;
}

bool isHomeCmd(char* token) {
  if (strcmp(token,"HOME") == 0) {
    setCmdResponse("HOME","ACK_");
    return true;
  }
  return false;
}

bool isGeofenceCmd(char* token) {
  if (strcmp(token,"GEOFENCE") == 0) {
    setCmdResponse("GEOFENCE", "ACK_");
    return true;
  }
  return false;
}

bool isSpeedCmd(char* token) {
  // according to documentation 
  // "In C and C++ (Arduino), strtok works by replacing the delimiter with a null terminator (\0) 
  // and returning a pointer to the beginning of the first token.
  // This means I can call strtok() again as it changed the original string
  if (strcmp(token,"SPEED") == 0) {
    // now figure out what speed we are instructed to use
    // behind the underscore can be 00, 10, 30, 50, 70 or 100 (meaning %)
    char* speed = strtok(NULL, "_"); 
    if (speed == NULL) {
      setCmdResponse("SPEED", "NOK_");
      return true;
    }
    Serial.print("Speed request: ");
    Serial.println(speed);

    // now make sure we set that speed
    changeSpeed(atoi(speed));

    setCmdResponse("SPEED", "ACK_");
    return true;
  }
  return false;
}

bool isWaypointCmd(char* token) {
  if (strcmp(token,"WAYPOINT") == 0) {
    setCmdResponse("WAYPOINT","ACK_");
    return true;
  }
  return false;
}

bool isRCCmd(char* token) {
  if (strcmp(token,"RC") == 0) {
    setCmdResponse("RC","ACK_");
    return true;
  }
  return false;  
}

void parseCommand(char* cmd) {
  // cmd contains everything that was received
  // if something then continue
  if (!(strlen(cmd) == 0)) {
    //Serial.println(cmd);
    // each command ends with an underscore; so if that's not in there then skip the message

    char* token = strtok(cmd, "_");
    while (token != NULL) {
      Serial.println(token);
      if (isStatusCmd(token)) break;
      if (isNavCmd(token)) break;
      if (isStopCmd(token)) break;
      if (isHomeCmd(token)) break;
      if (isGeofenceCmd(token)) break;
      if (isSpeedCmd(token)) break;
      if (isWaypointCmd(token)) break;
      if (isRCCmd(token)) break;
      Serial.println("UNKNOWN CMD");
      token = strtok(NULL, "_");  // see if there's more
    } 
  }  
}

/* ******* I2C HANDLING ********

  src: https://docs.arduino.cc/learn/communication/wire/

  As this unit will always have to respond to commands the function requestEvent() is the one that will be used. 
  I just implemented an empty function for future purposes
*/

void receiveEvent(int num_bytes_received) {
  
  // this code just flags that the main loop has to take care of the command
  // also stores the command received
  // has to be very fast

  int i = 0;

  while (Wire.available() && i < (MAX_SIZE_I2C_BUFFER-1)) {
    char c = Wire.read();
    pendingCommand[i] = c;
    i++;
  }
  pendingCommand[i]= '\0';  // terminating character
  newI2cCommandAvailable = true;
}


void requestEvent() {
  // special request from the main brain to answer back

  // last command is stored in last_cmd
  // based on that an answer has already been stored in last_response; so just send that
  
  // Wire.write(last_response);
  // There was a bug that RP2040 would send the same characters until the number of requested bytes from the brain/ESP32S3
  // So now it will really just send the lenght of the last response
  Wire.write((const uint8_t*)last_response, strlen(last_response));
}


/* MAIN MC STARTUP */
void setup() {

  Serial.begin(115200); // baud rate of 115200 bits/s over USB with the computer

  Serial.println("starting setting up");

  pinMode(MOTOR_LEFT, OUTPUT); // we'll be sending values not reading from those pins
  pinMode(MOTOR_RIGHT, OUTPUT);

  ss.begin(9600); // communication with the GPS sensor
  // SoftSerial.begin(9600);
  
  Serial.println("I2C setup");
  Wire.begin(I2C_ADDRESS); // start to listen on this address
  Wire.onReceive(receiveEvent); // if this MC hears something then call the receiveEvent function
  Wire.onRequest(requestEvent); // if it's asked for an update call this function

  setup_PWM(8, 20000, 255); // set resolution to 8 bits; frequency of 20Khz to steer the motors via PWM

  // Serial1.begin(9600); // initialize the UART connection on the standard pins via hardware

  // info about TinyGPSPlus
  //Serial.print(F("Testing TinyGPSPlus library v. ")); Serial.println(TinyGPSPlus::libraryVersion());

  Serial.println("set everything up");
  setCmdResponse("STATUS", "RDY_");
}

void loop() {

  if (newI2cCommandAvailable) {
    noInterrupts(); // to make sure that even if new message comes in we'll not deal with it


    char cmd[MAX_SIZE_I2C_BUFFER];
    strncpy(cmd, pendingCommand, MAX_SIZE_I2C_BUFFER - 1);
    cmd[MAX_SIZE_I2C_BUFFER - 1] = '\0';
    
    newI2cCommandAvailable = false;
    interrupts(); // important; has to be set back on to receive I2C messages again and enable other interrupts

    Serial.print("msg received [");
    Serial.print(cmd);
    Serial.println("]");

    parseCommand(cmd);   // now safe to do motor logic
  }

  adjust2MotorsPWM();     // non-blocking PWM ramp of the motors
}
