/* Simple HSI Knobs Sketch for Falcon BMS / DCS / FSX
 *  for Arduino Micro/Leonardo / Sparkfun Pro Micro or equiv. clones
 * by SemlerPDX June2019
 * VETERANS-GAMING.COM
 * ( https://veterans-gaming.com/blogs/entry/32-diy-custom-game-controller-2-dial-hsi-course-and-heading-knobs/ )
 *
 *  Pins:
 *  Rotary Encoder 1 - (OUTA-OUTB-SW) = Arduino Pins (0,1,15)
 *  Rotary Encoder 2 - (OUTA-OUTB-SW) = Arduino Pins (2,3,6)
 *
 *  Encoder Library
 * https://www.pjrc.com/teensy/td_libs_Encoder.html
 *
 *  Joystick Library
 * by Matthew Heironimus
 * https://github.com/MHeironimus/ArduinoJoystickLibrary
 */

#define ENCODER_USE_INTERRUPTS
#define ENCODER_OPTIMIZE_INTERRUPTS
#include <Encoder.h>
#include <Joystick.h>

// Tell the Encoder Library which pins have encoders
Encoder axisXRotation(0, 1);
Encoder axisYRotation(2, 3);

// Rotary Encoder Push Button Pins
int buttonArray[2] = {15, 6};
const int numButtons = sizeof(buttonArray) / sizeof(buttonArray[0]);

// Rotary Encoder Interrupt Pins
int EncoderPin0 = 0;
int EncoderPin1 = 1;
int EncoderPin2 = 2;
int EncoderPin3 = 3;

// Delay Time between loops
int debounceDelay = 260;

// Variables to compare current to old values
int oldX = 0;
int oldY = 0;
int RxAxis_Value = 1;
int RyAxis_Value = 1;

// Intervals for Jump/Warp Speed Rotations
int JumpSpeed = 18;
int WarpSpeed = 30;

// Set generic joystick using id 42 with 2 buttons and 2 axes
Joystick_ Joystick(0x42, 
  0x04, 2, 0,
  false, false, false, true, true, false,
  false, false, false, false, false);  


void setup() { 

  // Set Encoder Pins as Pullups
  pinMode(EncoderPin0, INPUT_PULLUP);
  pinMode(EncoderPin1, INPUT_PULLUP);
  pinMode(EncoderPin2, INPUT_PULLUP);
  pinMode(EncoderPin3, INPUT_PULLUP);

  // Loop through buttons and set them as Pullups
  for (int x = 0; x < numButtons; x++) {
    pinMode(buttonArray[x], INPUT_PULLUP);
  }

  // Set Range of custom Axes
  Joystick.setRxAxisRange(0, 359);
  Joystick.setRyAxisRange(0, 359);

  // Initialize Joystick Library
  Joystick.begin(false);

}


void loop() {

  // Loop through button pin values & set to Joystick
  for (int x = 0; x < numButtons; x++) {
    byte currentButtonState = !digitalRead(buttonArray[x]);
    Joystick.setButton(x, currentButtonState);
  }

  // Read "Heading" X Axis Rotation Encoder Knob
  int newX = axisXRotation.read();
  if (newX != oldX) {
    RxAxis_Value = getAxisVal(newX, oldX, RxAxis_Value);
    Joystick.setRxAxis(RxAxis_Value);
    axisXRotation.write(newX);
    oldX = newX;
  }

  // Read "Course" Y Axis Rotation Encoder Knob
  int newY = axisYRotation.read();
  if (newY != oldY) {
    RyAxis_Value = getAxisVal(newY, oldY, RyAxis_Value);
    Joystick.setRyAxis(RyAxis_Value);
    axisYRotation.write(newY);
    oldY = newY;
  }

  // Send Joystick info through USB
  Joystick.sendState();
  delay(debounceDelay);
}


// Function to return Rotation Value adjusted for the turning speed
int speedVal(int val) {
  if (val >= WarpSpeed) {
    val = WarpSpeed;
  } else if (val >= JumpSpeed) {
    val = JumpSpeed;
  } else {
    val = 1;
  }
  
  return val;
}

// Function to return modified Rotation Axis Value
int getAxisVal(int newVal, int oldVal, int val) {
  if (newVal > oldVal) {
    // Determine speed of increment & set output
    newVal = newVal - oldVal;
    val = val + speedVal(newVal);
  } else if (newVal < oldVal) {
    // Determine speed of decrement & set output
    newVal = oldVal - newVal;
    val = val - speedVal(newVal);
  }

  // Correct Rotation Value within 360 degrees
  val = (val % 360 + 360) % 360;
  return val;
}