// 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;

//
// 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);

  // map the raw values and store them in the dataPackage
  dataPackage[1] = map(raw_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(raw_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_adress, 0, !digitalRead(5));
  bitWrite(raw_adress, 1, !digitalRead(6));
  bitWrite(raw_adress, 2, !digitalRead(7));
  bitWrite(raw_adress, 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;
    }

  }


  // apply the max speed value change (use least significant bit of buttonByte2 for now)
  if (final_buttonByte2 != lastFinal_buttonByte2)
  {
    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[4]);
  //Serial.println(raw_adress);
  // wait for a short amount of time.. Not sure if needed.
  delay(10);
  // debug



}