#include "Joystick.h"

Joystick_ Joystick(JOYSTICK_DEFAULT_REPORT_ID,
                   JOYSTICK_TYPE_MULTI_AXIS, 32, 0,
                   true, true, true, false, false, false,
                   false, true, false, false, false);

const bool testAutoSendMode = true;

#include <Wire.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x20, 20, 4); // set the LCD address to 0x20 for a 16 chars and 2 line display

unsigned long previousMillis = 0;
const long interval = 200;

int xValue = 0;
int yValue = 0;
int throttleInput = 0;
int reverseValue = 0;
int throttleValue = 0;

void setup() {
  Joystick.setXAxisRange(0, 1023);
  Joystick.setYAxisRange(0, 1023);
  Joystick.setThrottleRange(0, 1023);
  Joystick.setZAxisRange(0, 1023);
  Joystick.begin();
  //Serial.begin(9600);
  lcd.init();
  lcd.init();

  lcd.backlight();
}

void loop() {

  readAnalog();

  Joystick.setXAxis(xValue);
  Joystick.setYAxis(yValue);
  Joystick.setThrottle(throttleValue);
  Joystick.setZAxis(reverseValue);

  //serialOutput();

  unsigned long currentMillis = millis();

  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;

    updateLCD();
  }

}

void readAnalog()
{
  xValue = analogRead(A3);
  yValue = 1023 - analogRead(A2);
  throttleInput = analogRead(A1);

  if (throttleInput >= 950)
  {
    throttleValue = 1023;
    reverseValue = 512;
  }
  else if (throttleInput >= 532)
  {
    throttleValue = (throttleInput - 512) * 2;
    reverseValue = 512;
  }
  else if (throttleInput <= 50)
  {
    throttleValue = 0;
    reverseValue = 0;
  }
  else if (throttleInput <= 492)
  {
    throttleValue = 0;
    reverseValue = throttleInput;
  }
  else
  {
    throttleValue = 0;
    reverseValue = 512;
  }
}

void updateLCD()
{
  lcd.setCursor(0, 0);
  lcd.print("ThumbV = ");
  lcd.print(yValue);
  lcd.print("   ");
  lcd.setCursor(0, 1);
  lcd.print("ThumbH = ");
  lcd.print(xValue);
  lcd.print("   ");
  lcd.setCursor(0, 2);
  lcd.print("Throttle = ");
  lcd.print(throttleValue);
  lcd.print("   ");
  lcd.setCursor(0, 3);
  lcd.print("Reverse = ");
  lcd.print(reverseValue);
  lcd.print("   ");
  lcd.setCursor(15, 0);
  lcd.print("    ");
  lcd.setCursor(15, 0);
  lcd.print(throttleInput);
}


void serialOutput()
{
  Serial.print("x = ");
  Serial.print(xValue);
  Serial.print(" y = ");
  Serial.print(yValue);
  Serial.print(" t = ");
  Serial.print(throttleValue);
  Serial.print(" t = ");
  Serial.print(throttleValue);
  Serial.println();
}