// NRF24 based radio controller for radio controlled tractors
// by modelleicher
// NRF24 Library: https://github.com/TMRh20/RF24
// Inspiration by: "RC Tractor Guy" on youtube https://www.youtube.com/user/magicalmachines/videos
// Also great thanks to Antony Cartwright for the best NRF24 tutorial on youtube (bare basics explanation) https://www.youtube.com/watch?v=Qn2YLSYz3WM

// work in progress, just a start for now.
// February 2017


#include <SPI.h>
#include "RF24.h"


// 12 possible bytes for now.. Always send one more byte than the receiver awaits, for some reason.. Otherwise it doesn't work!?
uint8_t dataPackage[12];

// the raw input values, analogRead will be stored in those.
int raw_X, raw_Y, raw_RX, raw_RY = 512;

// speed range, use three possible speed ranges for finer control
byte drive_speedRanges[] = {100, 50, 0};
byte drive_curSpeedRange = 2;

int final_X, final_RY = 512;

//
// 0 - 255
// 50 - 205
// 100 - 155

// byte to store the raw adress values in
byte raw_address = 0;
// byte to store the raw values of up to 16 buttons
byte raw_buttonByte1, raw_buttonByte2 = 0;

// analogRead value for the 6 buttons
int anRead = 0;

// final state (on/off of the buttons) and the variables needet for software debounce
byte final_buttonByte1, final_buttonByte2 = 0;
byte lastButtonOnTime1[8];
byte lastButtonOnTime2[8];
byte lastFinal_buttonByte1, lastFinal_buttonByte2;


int dt = 0;
int lastMillis = 0;

// initialize the RF24 library. Pin 9 and 10 are used for CE and CSN.
RF24 radio(9, 10);

// Most codes use some crazy 64 bit long weird number as a pipe. But "1" works just fine..
const uint64_t pipe = 1;


void setup() {
  // serial is only used for debugging. Uncomment if no debugging is needed.
  Serial.begin(9600);
  // begin radio
  radio.begin();
  // i use PA_MIN for now.. It is good for at least 2m.. Only use higher levels if your power supply can support it and you have a proper Capacitor across the VCC and GND pins on the NRF module. (I suggest at least 100µF)
  radio.setPALevel(RF24_PA_MIN);
  // debug
  Serial.println("started..");
  // open the writing pipe since we are a sender
  radio.openWritingPipe(pipe);
  // debug
  Serial.println("opened..");

  pinMode(5, INPUT_PULLUP);
  pinMode(6, INPUT_PULLUP);
  pinMode(7, INPUT_PULLUP);
  pinMode(8, INPUT_PULLUP);

}

void loop() {
  // delta time, time since last loop
  dt = millis() - lastMillis;
  lastMillis = millis();


  // read the 4 analog values and save them in the variables
  raw_X = analogRead(1);
  raw_Y = analogRead(0);
  raw_RX = analogRead(3);
  raw_RY = analogRead(2);

  // some very basic inertia simulation so the vehicle doesn't launch forward/backward and to limit the steering speed since the servo with a 2S Lipo is really fast
  // instead of the raw value the final value is sent to the vehicle. The final value is pretty much just a smoothed out version of the raw value. The higher the
  // difference between raw and final value is the faster the servo/motor accelerates
  // change the following two values to adjust, the lower the value the quicker the final value will follow the raw value.
  #define steeringInertiaValue 6;
  #define drivingInertialValue 9;
  // steering:
  if (raw_X >= final_X) // if the raw value is lower than the final value
  {
    int diff = raw_X - final_X; // get the difference between both values
    diff = diff / steeringInertiaValue; // the difference gets divided by the "inertia value"
    min(final_X = final_X + diff, raw_X); // now the difference is added to the final value

 }
  else if (raw_X <= final_X) // this is pretty much the same as the above just in the other direction
  {
    int diff = final_X - raw_X;
    diff = diff / steeringInertiaValue;
    max(final_X = final_X -diff, raw_X);

  }
  // driving:
  if (raw_RY >= final_RY)
  {
    int diff = raw_RY - final_RY;
    diff = diff / drivingInertialValue;
    min(final_RY = final_RY + diff, raw_RY);

 }
  else if (raw_RY <= final_RY)
  {
    int diff = final_RY - raw_RY;
    diff = diff / drivingInertialValue;
    max(final_RY = final_RY -diff, raw_RY);

  }
  //

  // debug
  Serial.println(final_X);

  // map the raw values and store them in the dataPackage
  dataPackage[1] = map(final_X, 0, 1023, 0, 255);
  dataPackage[2] = map(raw_Y, 0, 1023, 0, 255);
  dataPackage[3] = map(raw_RX, 0, 1023, 0, 255);
  dataPackage[4] = map(final_RY, 0, 1023, 0 + drive_speedRanges[drive_curSpeedRange], 255 - drive_speedRanges[drive_curSpeedRange]);

  // get the current adress from the jumpers (pin 5 to 8) and write to the adress byte
  // since they use internal pullup we invert the digitalRead value
  bitWrite(raw_address, 0, !digitalRead(5));
  bitWrite(raw_address, 1, !digitalRead(6));
  bitWrite(raw_address, 2, !digitalRead(7));
  bitWrite(raw_address, 3, !digitalRead(8));

  dataPackage[0] = raw_address;

  // get the Joystick buttons
  bitWrite(raw_buttonByte1, 0, digitalRead(2));
  bitWrite(raw_buttonByte1, 1, digitalRead(3));

  // get 6 buttons from one analog pin using voltage dividers and analog read.
  anRead = analogRead(A7);

  if (anRead < 560 && anRead > 490) {
    raw_buttonByte2 = B00000000;
  }
  else if (anRead < 20) {
    raw_buttonByte2 = B00000001;
  }
  else if (anRead < 270 && anRead > 200) {
    raw_buttonByte2 = B00000010;
  }
  else if (anRead < 450 && anRead > 390) {
    raw_buttonByte2 = B00000100;
  }
  else if (anRead < 660 && anRead > 585) {
    raw_buttonByte2 = B00001000;
  }
  else if (anRead < 820 && anRead > 720) {
    raw_buttonByte2 = B00010000;
  }
  else if (anRead > 900) {
    raw_buttonByte2 = B00100000;
  }

  // button debouncing for both button bytes and storing them in on/off variables
  for (byte i = 0; i < 8; i++)
  {
    if (bitRead(raw_buttonByte1, i) == HIGH)
    {
      lastButtonOnTime1[i] = lastButtonOnTime1[i] + dt;
      if (lastButtonOnTime1[i] > 200)
      {
        bitWrite(final_buttonByte1, i, !bitRead(final_buttonByte1, i));
        lastButtonOnTime1[i] = 0;
      }
    }
    else
    {
      lastButtonOnTime1[i] = 0;
    }

  }

  for (byte i = 0; i < 8; i++)
  {
    if (bitRead(raw_buttonByte2, i) == HIGH)
    {
      lastButtonOnTime2[i] = lastButtonOnTime2[i] + dt;
      if (lastButtonOnTime2[i] > 200)
      {
        bitWrite(final_buttonByte2, i, !bitRead(final_buttonByte2, i));
        lastButtonOnTime2[i] = 0;
      }
    }
    else
    {
      lastButtonOnTime2[i] = 0;
    }

  }

  //
  dataPackage[5] = final_buttonByte1;
  dataPackage[6] = final_buttonByte2;


  // apply the max speed value change (use least significant bit of buttonByte2 for now)
  if (bitRead(final_buttonByte2, 0) != bitRead(lastFinal_buttonByte2, 0))
  {
    drive_curSpeedRange = drive_curSpeedRange + 1;
    if (drive_curSpeedRange > 2) {
      drive_curSpeedRange = 0;
    }
  }


  // update the "last" values
  lastFinal_buttonByte1 = final_buttonByte1;
  lastFinal_buttonByte2 = final_buttonByte2;

  // send the dataPackage
  radio.write(dataPackage, sizeof(dataPackage));
  //debug
  //Serial.println(final_buttonByte2, BIN);
  //Serial.println(dataPackage[0]);
  //Serial.println(raw_adress);
  // wait for a short amount of time.. Not sure if needed.
  delay(10);
  // debug



}