// ARduino - REmote

//LCD config
#include <Wire.h>
//There are several libraries with similar name, we are using the one made by Frank de Brabander
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 20, 4);
      



//Radio config
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
RF24 radio(4, 8); // CE, CSN
const byte robot[6] = "00001";
const byte remote_control[6] = "00002";
const byte race_controller[6] = "00003";

//Gyro & Accelerometer 
//#include<MPU6050.h>    
//MPU6050 mpu;
//int16_t ax, ay, az;
//int16_t gx, gy, gz;

//Global varaibles
char text[15];
int vibration_counter = 0;

//Calibrate the servo, change SERVO_CENTER value until the wheels will be straight
//Higher number will turn the wheels left, lower number will turn them right 
#define SERVO_CENTER 90

/*** REMOTE CONTROL SETTINGS ***/
#define ButtonL_PIN 8
#define ButtonR_PIN 6
#define pot_PIN A7
#define Vibration_PIN 6
#define ButtonArray_PIN A6
#define INVERT_LEFT_STICK 0
#define INVERT_RIGHT_STICK 1

#define LEFT_JOYSTICK_X_PIN A0
#define LEFT_JOYSTICK_Y_PIN A1
#define RIGHT_JOYSTICK_Y_PIN A2
#define RIGHT_JOYSTICK_X_PIN A3


#define ButtonM_VALUE 820
#define Button1_VALUE 765
#define Button2_VALUE 679
#define Button3_VALUE 10
#define Button4_VALUE 504
/*** END REMOTE CONTROL SETTINGS ***/



void setup()
{
  
  //There are two types of those LCDs one type has address 0x3F and the other 0x27
  //We guessed that our LCD is 0x27 , now we will verify it actually works
  //If there is an error, we will change the address  to 0x3F
  Wire.begin();
  Wire.beginTransmission(0x27);
  byte error = Wire.endTransmission();
  if (error != 0)
      LiquidCrystal_I2C lcd(0x3F, 20, 4);

  //Configure pins
  pinMode(Vibration_PIN, OUTPUT);

  //We are using pin 7 for the vibration motor, but it is also onnected to right joystick button
  //so if you want to use the button instead of the vbration botor, unomment the following line
  //pinMode(ButtonR_PIN, INPUT_PULLUP);

  //Left joystick button
  pinMode(ButtonL_PIN, INPUT_PULLUP);

  //The following pins are using for joysticks x,y values
  pinMode(LEFT_JOYSTICK_X_PIN, INPUT);
  pinMode(LEFT_JOYSTICK_Y_PIN, INPUT);
  pinMode(RIGHT_JOYSTICK_Y_PIN, INPUT);
  pinMode(RIGHT_JOYSTICK_X_PIN, INPUT);

  //internal led
  pinMode(13, OUTPUT);
  digitalWrite(13, LOW);

  //Potentiometer
  pinMode (pot_PIN, INPUT);

  pinMode(ButtonArray_PIN, INPUT);

  digitalWrite(2, LOW);

  //Turn on serial
  Serial.begin(57600);
  Serial.println("Starting");
  
  //Init accelerometer

  //mpu.initialize();

  //Display message on LCD screen
  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("-=PlayRobotics=-");

  //Setup Radio
  radio.begin();
  radio.setPALevel(RF24_PA_HIGH);
  //radio.setPALevel(RF24_PA_MIN);
  radio.setRetries(15, 15);  // Set max retries and delay time on retries 15 max
  radio.setAutoAck(true);

  radio.openReadingPipe(1, remote_control);

  digitalWrite(Vibration_PIN, HIGH);
  delay(500);
  digitalWrite(Vibration_PIN, LOW);
}

void loop()
{
  delay(10);
   
  //Test potantiometer
  int sensorValue = analogRead(pot_PIN);
  //Serial.println(sensorValue);


   
  //===== Check joysticks

  //This is steering
  int left_joystick_X = analogRead(LEFT_JOYSTICK_X_PIN);

  //This is throttle
  int right_joystick_Y = analogRead(RIGHT_JOYSTICK_Y_PIN);
  
  
  //This is optional, but we can replace joystick steering with accelerometer steering

  //Accelerometer sensor can be used to allow steering by turning the remote control in your hands like a steering wheel
  //mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  //Overwrite the joystick with accelerometer value, 
  //we are also mapping to value from the sensor to the same value range as returned by the joystick
  //left_joystick_X = map(ay, -17000, 17000, 0, 1024);  //Send Y axis data


  //Not used for racing game, if you want to use them uncomment the lines below
  //int left_joystick_Y = analogRead(LEFT_JOYSTICK_Y_PIN);
  //int right_joystick_X = analogRead(RIGHT_JOYSTICK_X_PIN);

  //We will be using an array of chars to send the message
  char message_to_send[6];

  // "M" - Movment
  message_to_send[0] = 'M';

  //Speed part 1
  message_to_send[1] = '0';
  //Speed part 2
  message_to_send[2] = '0';
  //Direction 1->forward , 2->back
  message_to_send[3] = '1';

  //Steering
  message_to_send[4] = '0';

  //Center calibration
  message_to_send[5] = '90';


  //Speed handeling
  // Joystick will return a number between 1024 and 0, but we want to map it to a value that will be nagative for backword speed
  int speed = map(right_joystick_Y, 0, 1024, 512, -512);

  int direction = 1; //forward
  if (speed < 0)
  {
    speed = speed * -1;
    //Direction 1->forward , 2->back
    direction = 2;
  }

  //The maximum value of a speed is now 512, but we are using an array of chars to send the data
  //The size of the char datatype is 8 bits, which means the maximum number we can store is 255
  //we will devied the joystick value into two number

  int speed_one = speed / 254;
  int speed_two = speed % 254; //The remaining of the division



  //Steering handeling
  //We will need to map the joystick value to 180 (full left) -> 0 (full right)
  int steering = map(left_joystick_X, 0, 1023, 180, 0);


  //First character of the message -> identifier (M = Movment)
  message_to_send[0] = 'M';
  message_to_send[1] = speed_one;
  message_to_send[2] = speed_two;
  message_to_send[3] = direction;
  message_to_send[4] = steering;
  message_to_send[5] = SERVO_CENTER;

  //Set the radio to transmitter mode
  radio.stopListening();
  radio.openWritingPipe(robot);

  //Send the joystick values to the robot
  if (!radio.write(&message_to_send, sizeof(message_to_send)))
    Serial.println("----====Writing to ROBOT failed====-----");
  else
    Serial.println("----====SENT Joystick to robot");

  //Now lets check if we have incoming mesages
  radio.startListening();
  if (radio.available())
  {
    radio.read(&text, sizeof(text));

    Serial.println((char)text[0]);
    Serial.println((char)text[1]);
    Serial.println((char)text[2]);
    Serial.println((char)text[3]);
    Serial.println((char)text[4]);
    //Check if we got lap time message
    if (text[0] == 'L')
    {
      //We want to get rid of 'L' since it is just message identifier and we don't want to print it
      text[0] = ' ';

      //Erase previous message on LCD
      lcd.setCursor(0, 0);
      lcd.print("                ");

      //Print Lap Time
      lcd.setCursor(0, 0);
      lcd.print("LAP:");
      lcd.print(text);
    }
    //Check if we got BEST lap time message
    if (text[0] == 'B')
    {
      //We want to get rid of 'B' since it is just message identifier and we don't want to print it
      text[0] = ' ';

      //Since best lap is also a lap we will still need to print the time on screen
      //Erase previous message on LCD
      lcd.setCursor(0, 0);
      lcd.print("                ");
      lcd.setCursor(0, 1);
      lcd.print("                ");

      //Print Lap Time
      lcd.setCursor(0, 0);
      lcd.print("LAP:");
      lcd.print(text);

      //Since this is the best lap we will also print the best lap time on the second line of the screen
      //When a regular lap will happen only the first line will be overwritten and the best lap time will still be there
      lcd.setCursor(0, 1);
      lcd.print("BEST:");
      lcd.print(text);

      //Let's make short vibration, so the player will know a best lap was scored
      //We can use delay() function, arduino will not do anything during the delay (will not send remote commands to robot)
      //A better solution then delay will be to start the vibration, and then stop it in one of the next runs of the arduino loop

      digitalWrite(Vibration_PIN, HIGH);
      //delay(200);
      vibration_counter = 5;
    }
  }

  //Check button click
  int x = analogRead(A6);
  //Serial.println("Buttons");
  //Serial.println(x);
  if ((x >= Button1_VALUE - 30) && (x <= Button1_VALUE + 30)) {
    Serial.println("Button Array: Button 1 was clicked");

  }
  else
  {
    //check if button 2 was clicked
    if ((x >= Button2_VALUE - 30) && (x <= Button2_VALUE + 30))
    {
      Serial.println("Button Array: Button 2 was clicked");
      Serial.println("Start Racing game");
      //send message to controller
      radio.stopListening();
      radio.openWritingPipe(race_controller);
      //S means -> Start game 
      char data_to_send[1] = "S";
      if (!radio.write(&data_to_send, sizeof(data_to_send))) {
        Serial.println("Error sending to race controller");

      }
      else
        Serial.println("sent S to race controller");

    }
    else
    {
      //check if button 3 was clicked
      if ((x >= Button3_VALUE - 30) && (x <= Button3_VALUE + 30))
      {
        Serial.println("Button Array: Button 3 was clicked");

      }
      else
      {
        //check if button 4 was clicked
        if ((x >= Button4_VALUE - 30) && (x <= Button4_VALUE + 30))
        {
          Serial.println("Button Array: Button 4 was clicked");

        }
        else
          //check if middle button was clicked
          if ((x >= ButtonM_VALUE - 30) && (x <= ButtonM_VALUE + 30))
          {
            Serial.println("Button Array: Button M was clicked");
            Serial.println("Sending finish game command");
            //send message to controller
            radio.stopListening();
            radio.openWritingPipe(race_controller);
            //S means -> Start game 
            char data_to_send[1] = "F";
            if (!radio.write(&data_to_send, sizeof(data_to_send))) {
              Serial.println("Error sending to race controller");
            }
            else
              Serial.println("sent F to race controller");
          }
      }
    }
  }
  delay(75);

  //We are going to decrease the vibration counter value in every cycle of the loop, until we turn off the vibration
  //This is our way to do vibration without blocking the code using the delay() function (explained above in the best lap section)
  if (vibration_counter == 0)
    digitalWrite(Vibration_PIN, LOW);
  else
    vibration_counter--;
}