/*
LCD specifications:
  lcd has pcd8544 SPI driver IC
  lcd is 84x48 monochrome pixels
  lcd is not compatible with nrf24L01+ while using hardware SPI
  lcd is separated to 6 rows of 8x84 pixels
  lcd library can display monochrome bitmaps as "static const byte "array_name"[]"
  lcd library can only print characters inside rows, but not in between them
  lcd is addressed like this:
      Y:                                                      
    X:0 1 2 3 4 5 6 7 ... 83 (because pixels counted from 0)  
      1 
      2
      3
      4
      5 
     (because rows are counted from 0)
     
      to print charracter in bottom right corner:
        lcd.setCursor(77,5); (83(lcd width in pixels)-6(character width)=77,5 - because we are printing in 6th row) 

Input specifications:
  analog sticks are read by ADCs (analog to digital converters) and return 4 byte integer value (0 - 1023)
  buttons are read by pulled-up digital pins and return boolean value of 0 or 1

Radio specifications:
  radio has nrf24L01+ chip with additional aplifier IC
  radio can work as receiver and transmitter
  radio can receive ACK packets after a transmission
*/

#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"
#include <PCD8544.h>
RF24 radio(9,10);
static PCD8544 lcd;
static const byte radar[] = {
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x40, 0x40, 0x20, 0x20, 0x10, 0x08, 0x08, 0x08,
0x04, 0x04, 0x04, 0x02, 0x02, 0x02, 0x02, 0x02, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
0x01, 0x01, 0x01, 0x01, 0x02, 0x02, 0x02, 0x02, 0x02, 0x04, 0x04, 0x04, 0x08, 0x08, 0x08, 0x10,
0x20, 0x20, 0x40, 0x40, 0x80, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x80, 0x80, 0x40, 0x40, 0x20, 0x20, 0x10, 0x10, 0x10, 0x08, 0x08, 0x08, 0x04, 0x04, 0x04, 0x04,
0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x08, 0x08, 0x08, 0x10, 0x10, 0x10, 0x20, 0x20,
0x40, 0x40, 0x80, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x04, 0x08, 0x10, 0x20,
0x40, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0xC0, 0x30, 0x0C, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x40, 0x20, 0x10,
0x08, 0x04, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x40, 0x40,
0x40, 0x40, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x40, 0x40, 0x40, 0x80, 0x80,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x0C, 0x30, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x80, 0x70, 0x0E, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xC0, 0x30, 0x0C, 0x03,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x60, 0x10, 0x08, 0x04, 0x02, 0x02, 0x01,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x02, 0x04, 0x08, 0x10, 0x60, 0x80, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x03, 0x0C, 0x30, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
0x0E, 0x70, 0x80, 0x00, 0xF0, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xF0, 0x0E,
0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xC0, 0x3C, 0x03, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x80, 0x60, 0x18, 0x04, 0x02, 0x02, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x01, 0x01, 0x02, 0x02, 0x0C, 0x10, 0x60, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x03, 0x3C, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x1E, 0xE0, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0xF0, 
};

const uint64_t pipe = 0xE8E8F0F0E1LL;

float X;
float Y;

int TXDATA[5];
int RXDATA[2];

int menu;
int distance;
int tab;
int wehicleType = 1;
int channel;
int analog1State;
int analog2State;
int lastAnalog1State;
int lastAnalog2State;
int selected1;
int selected2;
int pointer1 = 1;
int pointer2;
int lastPointer1;
int lastPointer2;

bool detected[14][6];

bool done;
bool autoACK;
bool state=1; //0 = controll 1 = menu
bool button1State;
bool button2State;
bool lastButton1State;
bool lastButton2State;

void setup(void){
  pinMode(17,OUTPUT);
  digitalWrite(17,LOW);
  pinMode(A0,INPUT);
  pinMode(A1,INPUT);
  pinMode(A2,INPUT);
  pinMode(A3,INPUT);
  pinMode(1,INPUT_PULLUP);
  pinMode(0,INPUT_PULLUP);
  lcd.begin(84,48);
  lcd.setContrast(59);
//  lcd.createChar(0, glyph);
  radio.begin();
  radio.openWritingPipe(pipe);
  //radio.setPayloadSize(2);
  //radio.setRetries(15,15);
//  radio.setAutoAck(1);
//  autoACK = 1;
//  radio.enableAckPayload();
  radio.setPALevel(RF24_PA_MAX);
  radio.setDataRate(RF24_250KBPS);
  radio.setChannel(125);
  channel = 125;
}

void loop(void){
  //read all input from buttons and analog sticks
  TXDATA[0]=analogRead(A2);
  TXDATA[1]=analogRead(A3);
  TXDATA[2]=analogRead(A0);
  TXDATA[3]=analogRead(A1);
  button1State = !digitalRead(1);
  button2State = !digitalRead(0);
  bitWrite(TXDATA[4],0,button1State);
  bitWrite(TXDATA[4],1,button2State);
  
  //check for a state button press
  if (button1State != lastButton1State){
    if (button1State == HIGH) {
      state = !state; //change state
      pointer1 = 1; //reset values
      tab = 0;
      lcd.clear(); //clear lcd
    } 
    delay(50); //allow some time for debounce
  }
  lastButton1State = button1State; //lock in button state
  
  if (state == 0){
    printControll(); //print action menu or radar scope
  }
  if (state == 1){
    printSettings(); //print settings menu
  }
}

void printControll(){
  if (button2State != lastButton2State) {
    if (button2State == HIGH) {
      lcd.setCursor(0,0);
      lcd.drawBitmap(radar,84,48);
    }
    delay(50);
  }
  lastButton2State = button2State;
  radio.write(&TXDATA,sizeof(TXDATA));  //send some data
  if (radio.isAckPayloadAvailable()){   //check if some data is returned after sending
    radio.read(&RXDATA,sizeof(RXDATA)); //if data is returned - read it
  }
  distance=map(constrain(RXDATA[1],0,200),0,200,0,41); //constrain and map returned distance to LCD
  X = RXDATA[1] * cos((PI / 180 * RXDATA[0]))+41;      //calculate X coordinate
  Y = -RXDATA[1] * sin((PI / 180 * RXDATA[0]))+41;     //calculate Y coordinate
  
  //check X and Y coordinates for a match on a 13x6 grid
  for (int x = 0; x <= 13; x++){                               
    for (int y = 0; y <= 5; y++){
      if ((X>=x*6)&&(X<x*6+6)&&(Y>=y*8)&&(Y<y*8+8)){
        detected[x][y]=1;     //if found on grid, set as detected in boolean array
        lcd.setCursor(x*6,y); //set LCD cursor to x and y values (*6 is for aligning to the grid since the character is 6 pixels wide)
        lcd.print(0);         //print '0' in that position
      }
    }
  }
}

void printSettings(){ //settings menu script
  //delete cursor from last position
  lcd.setCursor(77,lastPointer1);
  lcd.print(' ');
  
  //print cursor in new position
  lcd.setCursor(77,pointer1);
  lcd.print('<');
  
  //switch to the tab value corresponding to the actions of selected setting
  switch (tab){
    case 0:{
      //print whole settings menu
      lcd.setCursor(17,0);
      lcd.print("Settings");
      lcd.setCursor(0,1);
      lcd.print("PALevel");
      lcd.setCursor(59,1);
      
      //check Power Amplification level and print corresponding text
      switch (radio.getPALevel()){
        case 0:
          lcd.print("MIN");
        break;
        case 1:
          lcd.print("LOW");
        break;
        case 2:
          lcd.print("MED");
        break;
        case 3:
          lcd.print("MAX");
        break;
      }

      lcd.setCursor(0,2);
      lcd.print("AutoACK");
      
      //check Auto-Acknowledgement state and print corresponding text
      switch (autoACK){
        case 0:{
          lcd.setCursor(59,2);
          lcd.print("OFF");
        }break;
        case 1:{
          lcd.setCursor(65,2);
          lcd.print("ON");
        }break;
      }
      lcd.setCursor(0,3);
      lcd.print("DataRate");
      
      //check transmission speed setting and print corresponding text
      switch (radio.getDataRate()){
        case RF24_250KBPS:{
          lcd.setCursor(59,3);
          lcd.print("LOW");
        }break;
        case RF24_2MBPS:{
          lcd.setCursor(59,3);
          lcd.print("MAX");
        }break;
        case RF24_1MBPS:{
          lcd.setCursor(59,3);
          lcd.print("MED");
        }break;
      }
      lcd.setCursor(0,4);
      lcd.print("Channel");
      
      //check channel setting and print corresponding text in right possition
      if (channel<10){
        lcd.setCursor(71,4);
      }
      else if (channel<100){
        lcd.setCursor(65,4);
      }
      else {
        lcd.setCursor(59,4);
      }
      lcd.print(channel);
      lcd.setCursor(0,5);
      lcd.print("Wehicle");
      lcd.setCursor(59,5);

      //check which wehicle type user wants to controll and print corresponding type
      switch (wehicleType){
        case 1:
          lcd.print("N/A"); //default setting with no additional functionality
        break;
        case 2:
          lcd.print("CAR"); //2 wheel drive and servo steering
        break;
        case 3:
          lcd.print("TNK"); //2 independant wheel drive with no steering
        break;
        case 4:
          lcd.print("BLC"); //2 independant wheel drive with gyro and accellerometter axial stabilization
        break;
        case 5:
          lcd.print("DRN"); //2 independant wheel drive with additional information from sensors
        break;
      }
      getInput(1,5,0,0); //read user input from getInput function
      if (done){
        //reset values and go to selected settings tab
        pointer1 = 1;    
        tab = selected1;
        done=0;
      }
    }break;
    case 1:{  
      //Power Amplification settings menu
      lcd.setCursor(17,0);
      lcd.print("PALevel:");
      lcd.setCursor(0,1);
      lcd.print("MIN");
      lcd.setCursor(0,2);
      lcd.print("LOW");
      lcd.setCursor(0,3);
      lcd.print("MED");
      lcd.setCursor(0,4);
      lcd.print("MAX");
      getInput(1,4,0,0);
      if (done){
        //check which PALevel setting user sellected
        switch (selected1){
          case 1:
            radio.setPALevel(RF24_PA_MIN);
          break;
          case 2:
            radio.setPALevel(RF24_PA_LOW);
          break;
          case 3:
            radio.setPALevel(RF24_PA_HIGH);
          break;
          case 4:
            radio.setPALevel(RF24_PA_MAX);
          break;
        }
        //reset values
        done = 0;
        tab = 0;
      }
    }break;
    case 2:{
      lcd.setCursor(17,0);
      lcd.print("AutoACK:");
      lcd.setCursor(0,1);
      lcd.print("OFF");
      lcd.setCursor(0,2);
      lcd.print("ON");
      getInput(1,2,0,0);
      if (done){
        //check which AutoACK setting user sellected
        switch (selected1){
          case 1:{
            radio.setAutoAck(0);
            autoACK = 0;
        }break;
          case 2:{
            radio.setAutoAck(1);
            autoACK = 1;
          }break;
        }
        //reset values
        done = 0;
        tab = 0;
      }
    }break;
    case 3:{
      //DataRate settings menu
      lcd.setCursor(14,0);
      lcd.print("DataRate:");
      lcd.setCursor(0,1);
      lcd.print("LOW (+dBm)");
      lcd.setCursor(0,2);
      lcd.print("MED (~dBm)");
      lcd.setCursor(0,3);
      lcd.print("MAX (-dBm)");
      getInput(1,3,0,0);
      if (done){
        //check which DataRate setting user sellected
        switch(selected1){
          case 1:
            radio.setDataRate(RF24_250KBPS);
          break;
          case 2:
            radio.setDataRate(RF24_1MBPS);
          break;
          case 3:
            radio.setDataRate(RF24_2MBPS);
          break;
        }
        //reset values
        done = 0;
        tab = 0;
      }
    }break;
    case 4:{
      //Channel settings menu
      lcd.setCursor(17,0);
      lcd.print("Channel:");
      if (pointer2<10){
        lcd.setCursor(32,1);
        lcd.print(' ');
      }
      else if (pointer2<100){
        lcd.setCursor(29,1);
        lcd.print(' ');
      }
      else {
        lcd.setCursor(32,1);
      }
      lcd.print(pointer2);
      lcd.print(' ');
      getInput(1,1,0,125);
      if (done){
        channel = selected2; //lock in user selected channel
        //reset values
        done = 0;
        tab = 0;
      }
    }break;
    case 5:{
      //Not implemented, but in TODO list
    }break;
  }
}
void getInput(int START, int END, int MIN, int MAX){
  if (TXDATA[3]==1023){
    analog1State = 2; //set analog state as in right position
  }
  else if (TXDATA[3]==0){
    analog1State = 0; //set analog state as in left position
  }
  else analog1State = 1; //set analog state as in middle position
  
  if (analog1State != lastAnalog1State) {
    // increment or decrement pointer1 position according to analog1 state
    if (analog1State == 0){
      lastPointer1=pointer1;
      pointer1++;
      if (pointer1>END){
        pointer1 = START;
      }
    }
    if (analog1State == 2){
      lastPointer1=pointer1;
      pointer1--;
      if (pointer1<START){
        pointer1 = END;
      }
    }
  }
  lastAnalog1State = analog1State;
  if (TXDATA[2]==1023){ 
    analog2State = 2; //set analog2 state as in right position
  }
  else if (TXDATA[2]==0){
    analog2State = 0; //set analog2 state as in left position
  }
  else analog2State = 1; //set analog2 state as in middle position

  // increment or decrement pointer2 position according to analog2 state
  if (analog2State == 2){
    lastPointer2=pointer2;
    pointer2++;
  }
  if (analog2State == 0){
    lastPointer2=pointer2;
    pointer2--;
  }
  pointer2 = constrain(pointer2,MIN,MAX); //constrain cursor movement past given boundaries
  if (button2State != lastButton2State) {
    if (button2State == LOW) {
      //lock in pointer positions
      selected1 = pointer1;
      selected2 = pointer2; 
      //reset values 
      pointer1 = 1;         
      pointer2 = 0;
      done = 1;    //mark as user is done
      lcd.clear(); //clear lcd
    }
    delay(50); //allow some time for debounce
  }
  lastButton2State = button2State;
}