//Controller Code for the 3D Printed Arduino Servo Platform With Robotic Arm 

#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

unsigned long previousMillis2 = 0;
const unsigned long time2 = 90; //timing for task to send out data

RF24 radio(8, 9); //CE, CSN
const byte address[6] = "00002";

typedef struct { //declare stucture
  int Lxj; //members
  int Lyj;
  int Rxj;
  int Ryj;
  int RJBState;
}Data;
Data value; //value is object

int RJB = 7;           //right joystick button pin

int ackData[5] = { 1, 1, 1, 0, 90}; //{HUD value, i, j, k, Joint[]}

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
// The pins for I2C are defined by the Wire-library. 
#define SCREEN_WIDTH 128      // OLED display width, in pixels
#define SCREEN_HEIGHT 64      // OLED display height, in pixels
#define OLED_RESET     -1     // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3D   ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

char *HUD[] = {
  "Connected", "Not Connected", "Not Moving", 
  "Forward", "Backward", 
  "Clockwise", "Counterclockwise", 
  "Arc Right Forward", "Arc Left Forward",
  "Arc Right Backward", "Arc Left Backward"
};

void setup() {

  Serial.begin(9600);

  pinMode (RJB, INPUT_PULLUP);   // Enable the right joystick button pin as input with a PULLUP resistor 

  radio.begin();
  radio.setDataRate( RF24_250KBPS );
  radio.enableAckPayload();
  radio.setRetries(5,5);
  radio.openWritingPipe(address);
  radio.setPALevel(RF24_PA_MIN);
  radio.stopListening();

  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);                            // Don't proceed, loop forever
  }
  
  // Show initial display buffer contents on the screen --
  // the library initializes this with an Adafruit splash screen.
  display.display();
  delay(2000);                          // Pause for 2 seconds

  // Clear the buffer
  display.clearDisplay();
  // Draw a single pixel in white
  display.drawPixel(10, 30, SSD1306_WHITE);
  display.setTextSize(1.5);               // Draw 2X-scale text
  display.setTextColor(SSD1306_WHITE);          //(SSD1306_INVERSE, SSD1306_WHITE , SSD1306_BLACK)
  display.setCursor(30, 40);
  display.println(F("HELLO!:D"));
  display.display();
  delay(2000);
}

void loop() {

  unsigned long currentMillis= millis();

//Do Read analog inputs
  value.Lxj = analogRead(A2);//Read left joystick values from the X-axis
  value.Lyj = analogRead(A3);//Read left joystick values from the Y-axis
  value.Rxj = analogRead(A0);//Read Right joystick values from the X-axis
  value.Ryj = analogRead(A1);//Read Right joystick value from Y-axis
  value.RJBState = digitalRead(RJB);//Read the digital value of the right joystick

//display text to OLED
  testtext(); 

//Send signal out
  if (currentMillis - previousMillis2 >= time2) {
    Serial.println("Doing Task2");
    
    bool rslt;
    rslt = radio.write(&value, sizeof(value)); //Send data out
    if(rslt) {
      if ( radio.isAckPayloadAvailable() ) {
        radio.read(&ackData, sizeof(ackData));
        Serial.println(" Data pass through ");
      } 
      
    }
    else {
          ackData[1] = 0;
          ackData[2] = 1;
          ackData[0] = 2;
          Serial.println(" Data did not pass through ");
    }
    
      Serial.print( " Sending ");
      previousMillis2 = currentMillis;

  }

    //Print values for troubleshooting
    Serial.print("  RJB: ");
    Serial.print(value.RJBState);
    Serial.print("  ackData[0]: ");
    Serial.print(ackData[0]);
    Serial.print("  ackData[2]: ");
    Serial.println(ackData[2]);

}

void testtext(void) { //Function for displaying text on OLED screen
  display.clearDisplay();

  display.setTextSize(1.5); // Draw 1.5X-scale text
  display.setTextColor(SSD1306_WHITE); //(white, yellow, blue, yellow + blue)

  display.setCursor(30, 0);
  display.println(HUD[ackData[2]]);

  display.setCursor(115, 1);
  display.println(ackData[0]);

  display.setCursor(15, 25);
  display.println(HUD[ackData[0]]);

  if (ackData[1] == 1) {
    display.drawPixel(20, 42, SSD1306_WHITE);
  }
  if (ackData[1] == 2) {
    display.drawPixel(40, 42, SSD1306_WHITE);
  }

  display.setCursor(15, 45);
  display.println("J");
  display.setCursor(21, 45);
  display.println(ackData[3]);
  display.setCursor(25, 45);
  display.println(":");
  display.setCursor(35, 45);
  display.println(ackData[4]);

  display.display();      // Show initial text

}