/*
  |NRF24L01 PIN        ESP32 |
  |--------------------------|
  | VCC         =     3.3V   |
  | CSN         =     D5     |
  | MOSI        =     D23    |
  | GND         =     GND    |
  | CE          =     D4     |
  | SCK         =     D18    |
  | MISO        =     D19    |
  |--------------------------|

  | OLED I2C   ESP32  |
  |-------------------|
  | VCC     = 3.3V    |
  | GND     = GND     |
  | SDA     = D21     |
  | SCL     = D22     |
  |-------------------|

  GIMBAL KANAN      ESP32
  pot vertikal    = D34
  pot horizontal  = D35

  GIMBAL KIRI        ESP32
  pot vertikal    = D36
  pot horizontal  = D39

  button 1 kanan  = D14
  button 2 kanan  = D13

  button 1 kiri   = D27
  button 2 kiri   = D12

  switch kanan    = D26
  switch kiri     = D25

  potensio kanan  = D33
  potensio kiri   = D32
  
NOTE : MASIH PERBAIKAN DI BAGIAN NRF24L01
*/



// library
#include <esp_now.h>
#include <WiFi.h>

#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
// #include <Adafruit_SH110X.h>
#include <Adafruit_SSD1306.h>

// LIBRARY NRF24L01
// #include <SPI.h>
// #include <nRF24L01.h>
// #include <RF24.h>

#define i2c_Address 0x3c  // inisialisasi adress oled

#define SCREEN_WIDTH 128  // lebar oled dalam pixel
#define SCREEN_HEIGHT 64  // tinggi oled dalam pixel
#define OLED_RESET -1     //   QT-PY / XIAO

//Adafruit_SH1106G display = Adafruit_SH1106G(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);  // membuat objek untuk oled SH110X
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);  // membuat objek untuk oled SSD1306

// strucut data yang di transfer
struct PacketData {
  // data joystick
  byte lxAxisValue;
  byte lyAxisValue;
  byte rxAxisValue;
  byte ryAxisValue;

  // data potensio
  byte lPotValue;
  byte rPotValue;

  // data tombol dan toggle
  byte LTValue;
  byte RTValue;
  byte LB1Value;
  byte LB2Value;
  byte RB1Value;
  byte RB2Value;
};

PacketData data;  // objek untuk pocket data

// // definisikan pin
#define PIN_LX 39
#define PIN_LY 36
#define PIN_RX 35
#define PIN_RY 34

#define PIN_LPOT 32
#define PIN_RPOT 33

#define PIN_LT 25
#define PIN_RT 26

#define PIN_LB1 27
#define PIN_LB2 12

#define PIN_RB1 14
#define PIN_RB2 13



uint8_t receiverMacAddress[] = { 0x8C, 0xAA, 0xB5, 0x0D, 0xDF, 0xC9 };  //(8C:AA:B5:0D:DF:C9) alamat mac receiver

esp_now_peer_info_t peerInfo;

// panggil balik saat data dikirim
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  Serial.print("\r\nLast Packet Send Status:\t");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
}

int mapping180(int value) {  // fungsi untuk mengambil nilai tengah, bila joystick tidak presisi
  // percabangan untuk mapping nilai tengah sampai atas
  if (value >= 9280) {
    value = map(value, 8840, 17680, 90, 180);
  } else if (value <= 8390) {
    value = (value == 0 ? 0 : map(value, 8840, 0, 90, 0));
  } else {
    value = 90;
  }
  return value;
}
int mapping255(int value) {  // fungsi untuk mengambil nilai tengah, bila joystick tidak presisi
  // percabangan untuk mapping nilai tengah sampai atas
  if (value >= 9280) {
    value = map(value, 8900, 14700, 127, 254);
    if (value >= 254) value = 254;
  } else if (value <= 8390) {
    value = (value == 0 ? 0 : map(value, 8900, 2400, 127, 0));
    if (value <= 0) value = 0;
  } else {
    value = 127;
  }
  return value;
}

int mapping_adc(int value, int limit_min, int limit_max) {  // fungsi untuk mengambil nilai tengah, bila joystick tidak presisi
  // percabangan untuk mapping nilai tengah sampai atas
  if (value >= limit_max) {
    value = map(value, 2000, 3500, 127, 254);
    if (value >= 254) value = 254;
  } else if (value <= limit_min) {
    value = (value == 0 ? 0 : map(value, 2000, 400, 127, 0));
    if (value <= 0) value = 0;
  } else {
    value = 127;
  }
  return value;
}

void setup(void) {
  Serial.begin(9600);   // memulai serial monitor
  WiFi.mode(WIFI_STA);  // set eifi ke mode sta

  display.begin(i2c_Address, true);                  // mulai adress untuk SH110X
  display.begin(SSD1306_SWITCHCAPVCC, i2c_Address);  // mulai adress untuk SSD1306

  display.clearDisplay();  // berishkan dispay oled

  greeting();

  // memulai esp now
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  } else {
    Serial.println("Succes: Initialized ESP-NOW");
  }

  // dapatkan status paket yang Ditransmisikan
  esp_now_register_send_cb(OnDataSent);

  // // // Register peer
  // esp_now_peer_info_t peerInfo;
  memcpy(peerInfo.peer_addr, receiverMacAddress, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;

  // // // Add peer
  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Failed to add peer");
    return;
  }


  // SET PIN
  pinMode(PIN_LB1, INPUT_PULLUP);
  pinMode(PIN_LB2, INPUT_PULLUP);
  pinMode(PIN_LT, INPUT_PULLUP);

  pinMode(PIN_RB1, INPUT_PULLUP);
  pinMode(PIN_RB2, INPUT_PULLUP);
  pinMode(PIN_RT, INPUT_PULLUP);
}

void loop(void) {
  // mengisi data struct dengan mapping joystick
  data.lyAxisValue = mapping_adc(analogRead(PIN_LY), 1750, 1950);
  data.lxAxisValue = mapping_adc(analogRead(PIN_LX), 1825, 2025);
  data.ryAxisValue = mapping_adc(analogRead(PIN_RY), 1900, 2100);
  data.rxAxisValue = mapping_adc(analogRead(PIN_RX), 1900, 2100);
  // data potensio
  data.lPotValue = map(analogRead(PIN_LPOT), 0, 4095, 0, 255);
  data.rPotValue = map(analogRead(PIN_RPOT), 0, 4095, 0, 255);
  // data togle
  data.LTValue = !digitalRead(PIN_LT);
  data.RTValue = !digitalRead(PIN_RT);
  // data togle
  data.LB1Value = !digitalRead(PIN_LB1);
  data.LB2Value = !digitalRead(PIN_LB2);
  data.RB1Value = !digitalRead(PIN_RB1);
  data.RB2Value = !digitalRead(PIN_RB2);

  connection();

  display_var(data.lyAxisValue, data.lxAxisValue, data.ryAxisValue, data.rxAxisValue);  // fungsi display di oled
}

void connection() {
  esp_err_t result = esp_now_send(receiverMacAddress, (uint8_t *)&data, sizeof(data));  // mengirim data ke receiver
  // // feedback kondisi data
  if (result == ESP_OK) {
    Serial.println("Sent with success");
  } else {
    Serial.println("Error sending the data");
  }
}

void greeting() {
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(2);
  display.setCursor(20, 10);
  display.print("NUG TX");  // variable y left
  display.setCursor(0, 35);
  display.print("Ver 0.0.12");  // variable y left
  display.display();
  delay(2000);
  display.clearDisplay();
}

void display_var(byte LY, byte LX, byte RY, byte RX) {
  char buffer[50];

  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);

  // bagian atas
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.print("T:");
  display.print(data.LTValue);                    // variable toggle left
  display.drawLine(19, 0, 19, 9, SSD1306_WHITE);  // vertikal pertama
  display.setCursor(22, 0);
  display.print("P:");
  sprintf(buffer, "%.3d", data.lPotValue);
  display.print(buffer);  // variable pot left

  display.setCursor(77, 0);
  sprintf(buffer, "%.3d", data.rPotValue);
  display.print(buffer);  // variable pot right
  display.setCursor(94, 0);
  display.print(":P");
  display.drawLine(107, 0, 107, 9, SSD1306_WHITE);  // vertikal kedua
  display.setCursor(116, 0);
  display.print(":T");
  display.setCursor(110, 0);
  display.print(data.RTValue);                    // variable toggle right
  display.drawLine(0, 9, 128, 9, SSD1306_WHITE);  // garis hrizontal atas

  // bagian bawah
  display.drawLine(0, 54, 128, 54, SSD1306_WHITE);  // garis horizontal atas

  display.setCursor(0, 57);
  display.print("B1:");
  display.print(data.LB1Value);                     // variable B1 left
  display.drawLine(25, 54, 25, 68, SSD1306_WHITE);  // garis vertikal pertama
  display.setCursor(28, 57);
  display.print("B2:");
  display.print(data.LB2Value);  // varibale B2 left

  display.setCursor(76, 57);
  display.print(data.RB2Value);  // variable B2 right
  display.setCursor(82, 57);
  display.print(":B2");
  display.drawLine(101, 54, 101, 68, SSD1306_WHITE);  // garis vertikal kedua
  display.setCursor(104, 57);
  display.print(data.RB1Value);  // variable B1 right
  display.setCursor(110, 57);
  display.print(":B1");

  // bagian tengah
  display.setCursor(0, 14);
  display.print("Y:");
  display.setCursor(0, 37);
  display.print("X:");

  display.setCursor(116, 14);
  display.print(":Y");
  display.setCursor(116, 37);
  display.print(":X");

  display.setTextSize(2);
  display.setCursor(12, 14);
  sprintf(buffer, "%.3d", LY);
  display.print(buffer);  // variable y left
  display.setCursor(12, 37);
  sprintf(buffer, "%.3d", LX);
  display.print(buffer);  // variable x left

  display.setCursor(81, 14);
  sprintf(buffer, "%.3d", RY);
  display.print(buffer);  // variable y right
  display.setCursor(81, 37);
  sprintf(buffer, "%.3d", RX);
  display.print(buffer);  // variable x right

  display.display();
}

void kalibrasi_joystic() {
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.print("LX :");
  display.print(analogRead(PIN_LX));
  display.setCursor(0, 30);
  display.print("LY :");
  display.print(analogRead(PIN_LY));
  display.setCursor(64, 0);
  display.print("RX :");
  display.print(analogRead(PIN_RX));
  display.setCursor(64, 30);
  display.print("RY :");
  display.print(analogRead(PIN_RY));
  display.display();
}