/*
Throttle Control Remote
*/

#include <ArduinoJson.h>
#include <assert.h>
#include <SoftwareSerial.h>
#include <stdlib.h>

#define RX 3 //BT
#define TX 2 //BT
#define BTN_PIN 9
#define LED_BUILTIN 13
//#define WAIT 110 // time for JSON to transmit at 2400 BAUD
#define WAIT 500
#define BAUDRATE 9600

// Global Variables 

int ledState = HIGH;
int btnState = LOW;
int lastBtnState = LOW;
int lastDebounceTime = 0;
int debounceDelay = 50;
int reading;
int x = 0;
char c = ' ';
char r1_string[8];
char l1_string[8];

SoftwareSerial BTSerial(RX, TX); // (RX, TX)
uint8_t draw_state = 0;

int enable, last_enable;

int RIGHT, LEFT, RIGHT_OUT, LEFT_OUT ;
int Right, Left;
int offsetRIGHT, offsetLEFT ;

//double deadStick = 0.02*1024; //Deadstick given as a percentage (2%)
double deadStick = 10;

 
void setup(){
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(1,OUTPUT);
  digitalWrite(1,HIGH);
  pinMode(BTN_PIN, INPUT);
  digitalWrite(BTN_PIN, HIGH);                // setup input HIGH 
analogReadResolution(9);
  Serial.begin(BAUDRATE);
  BTSerial.begin(BAUDRATE); // Setup BLUETOOTH serial
  pinMode(RX, INPUT);
  pinMode(TX, OUTPUT);
  digitalWrite(BTN_PIN, HIGH);                // setup input HIGH 
//  u8g_prepare();
 // assign default color value


 }
 
void loop() {
  /* power button */
  readPowerButton();


StaticJsonDocument<64> doc;
  last_enable = enable;
  enable = ledState;
  Right = analogRead(03);
  Left = analogRead(02);
  doc["e"] = enable; // latched 
  doc["l"] = Left; // Read Left Joystick analog
  doc["r"] = Right; // Read Right Joystick analog

  serializeJson(doc, Serial);
  serializeJson(doc, BTSerial); //output through BT
  BTSerial.println();
//  Serial.print(ledState);

if (enable & !last_enable) {
    offsetRIGHT = Right; //Take the initial offset value of your joysticks
    offsetLEFT = Left;
  }
  Serial.println();
  getJoysticks();


 

  delay(WAIT);
}

void readPowerButton() {
  /* power button */
  int reading = digitalRead(BTN_PIN);
  if (reading != lastBtnState) {
    lastDebounceTime = millis();
  }
  if ((millis() - lastDebounceTime) > debounceDelay) {
    if (reading != btnState) {
      btnState = reading;
      if (btnState == HIGH) {
        ledState = !ledState;
        digitalWrite(LED_BUILTIN, ledState);
      }
    }
  }
  lastBtnState = reading;
}


void getJoysticks() {
  if (!enable) {
    RIGHT_OUT = 0;
    LEFT_OUT = 0;
  }
  else
  {
  int RIGHT = Right - offsetRIGHT;                            // read RIGHT joystick
  if (abs(RIGHT) <= deadStick) {
      RIGHT = 0;                                                    //set deadstick
      RIGHT_OUT = 0;                                                //set deadstick
  } 
  else 
  {
    if (RIGHT < -deadStick) {
        RIGHT = map(RIGHT, -deadStick, -offsetRIGHT, 0, 254);        //map bottom range to 0-100
        RIGHT_OUT = RIGHT;
    } 
    else 
    {
      if (RIGHT > deadStick) {
          RIGHT = map(RIGHT, deadStick, 1023-offsetRIGHT, 0, 254);      //map top range to 0-100
          RIGHT_OUT = RIGHT;
        }
      }
    }
  
  LEFT = Left - offsetLEFT;                              // read LEFT joystick
  if (abs(LEFT) <= deadStick) {
      LEFT = 0;                                                     //set deadstick
      LEFT_OUT = 0;
  } 
  else 
  {
    if (LEFT < 0) {
        LEFT = map(LEFT, -deadStick, -offsetLEFT, 0, 254);           //map bottom range to 0-100
        LEFT_OUT = LEFT;
    } 
    else 
    {
      if (LEFT > 0) {
        LEFT = map(LEFT, deadStick, 1023-offsetLEFT, 0, 254);          //map top range to 0 -100
        LEFT_OUT = LEFT;
      }
    }
  }
  }
}