/* Modified HSI Knobs Sketch for Falcon BMS / DCS / FSX
 *  with additional 3rd Rotary Encoder (Z Axis)
 *    also with a 3-way Toggle Switch as buttons
 *  for Arduino Leonardo or equiv. clones
 * by SemlerPDX Sep2019
 * VETERANS-GAMING.COM
 * ( in response to reply at:
 *    https://veterans-gaming.com/index.php?/blogs/entry/32-diy-custom-game-controller-2-dial-hsi-course-and-heading-knobs/ )
 *  
 *  Rotary Encoders need 2 Intterupt Pins Each - Choose a board that has enough like Arduino Leonardo (or clones)
 *    info on Interrupts: https://www.arduino.cc/reference/en/language/functions/external-interrupts/attachinterrupt/
 *  
 *  Pins:
 *  Rotary Encoder 1 - (OUTA-OUTB-SW) = Arduino Pins (0,1,15)
 *  Rotary Encoder 2 - (OUTA-OUTB-SW) = Arduino Pins (2,3,6)
 *  Rotary Encoder 3 - (OUTA-OUTB-SW) = Arduino Pins (9,10,7)
 *  
 *  Three-Way Toggle - (Center Post)  = Analog Input (A1)
 *  
 *  Three-Way Toggle Switch Wiring Example:
 * https://veterans-gaming.com/uploads/monthly_2019_09/three_way_toggle_wiring_arduino.png.1524289e8f56690c7ff6d9f321904de9.png
 *  
 *  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>

//Three Way Toggle Init
#define TOGGLE_PIN A1
int previousState;
bool aToggled;

//Rotary Encoder Push Button Pins
//  *further buttons added must increment after these (these are 0,1,2) (buttons 1,2 and 3 on controller)
int buttonArray[3] = {15, 6, 7};

//Set Toggle Positions as Buttons* (buttons start at 0,1,2,3,4,5 for 6 buttons)
int ToggleButton1 = 3; //button 4 on controller
int ToggleButton2 = 4; //button 5 on controller
int ToggleButton3 = 5; //button 6 on controller

//Rotary Encoder Interrupt Pins
int EncoderPin0 = 0;
int EncoderPin1 = 1;
int EncoderPin2 = 2;
int EncoderPin3 = 3;
int EncoderPin4 = 9;   //*Must have board with 6 total interrupt pins
int EncoderPin5 = 10;  //*Must have board with 6 total interrupt pins

//Tell the Encoder Library which pins have encoders
Encoder axisXRotation(EncoderPin0, EncoderPin1);
Encoder axisYRotation(EncoderPin2, EncoderPin3);
Encoder axisZRotation(EncoderPin4, EncoderPin5);

//Delay Time between loops
int debounceDelay = 260;

//Delay Time before button release
int toggleDebounce = 10;

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

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

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


//Function to translate Three-way Toggle Analog Value
int getToggleState(int aVal) {
  if (aVal < 100) {
    aVal = ToggleButton1;
  }else if (aVal < 900) {
    aVal = ToggleButton3;
  }else{
    aVal = ToggleButton2;
  }
  return aVal;
}

//Function to set Rotation value adjusted for the turning speed
int speedVal(int dif, int val, int dir) {
  int increment = 1;
  if (dif >= WarpSpeed) {
    increment = WarpSpeed;
  }else if (dif >= JumpSpeed) {
    increment = JumpSpeed;
  }
  if (dir == 1) {
    val = val + increment;
  }else{
    val = val - increment;
  }
  //Correct Rotation within 360 deg.
  if (val < 0) {
    val = val + 360;
  }else if (val >= 360) {        
    val = val - 360;
  }
  return val;
}


void setup() { 
  
  //Toggle Switch Setup
  previousState = 1000;
  aToggled = false;

  //Loop through Encoder Pins and set them as Pullups
  for (int x = 0; x < 6; x++) {
    pinMode(EncoderPin[x], INPUT_PULLUP);
  }

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

  //Set Range of custom Axes
  Joystick.setRxAxisRange(0, 359);
  Joystick.setRyAxisRange(0, 359);
  Joystick.setRzAxisRange(0, 359);
  
  // Initialize Joystick Library
  Joystick.begin(false);

}


void loop() {

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


  //Read Three Way Toggle
  int analogValue = analogRead(TOGGLE_PIN);
  int actualState = getToggleState(analogValue);
  if (previousState != actualState) {
    //Set Toggle Switch input as Button Press
    Joystick.setButton(actualState, 1);
    previousState = actualState;
    aToggled = true;
    
  }else{
    //Reset button(s) to unpressed state
    if (aToggled) {
      aToggled = false;
      delay (toggleDebounce);
      for (int a = 3; a < 6; a++) {
        Joystick.setButton(a, 0);
      }
    }
  }
  

  // Read "Heading" X Axis Rotation Encoder Knob
  int newX = axisXRotation.read();
  if (newX > oldX) {
    //Determine speed of increment & set output
    int difX = newX - oldX;
    RxAxis_Value = speedVal(difX, RxAxis_Value, 1);
    Joystick.setRxAxis(RxAxis_Value);
    axisXRotation.write(newX);
    oldX = newX;

  }else if (newX < oldX) {
    //Determine speed of decrement & set output
    int difX = oldX - newX;
    RxAxis_Value = speedVal(difX, RxAxis_Value, 0);
    Joystick.setRxAxis(RxAxis_Value);
    axisXRotation.write(newX);
    oldX = newX;
  }


  // Read "Course" Y Axis Rotation Encoder Knob
  int newY = axisYRotation.read();
  if (newY > oldY) {
    //Determine speed of increment & set output
    int difY = newY - oldY;
    RyAxis_Value = speedVal(difY, RyAxis_Value, 1);
    Joystick.setRyAxis(RyAxis_Value);
    axisYRotation.write(newY);
    oldY = newY;

  }else if (newY < oldY) {
    //Determine speed of decrement & set output
    int difY = oldY - newY;
    RyAxis_Value = speedVal(difY, RyAxis_Value, 0);
    Joystick.setRyAxis(RyAxis_Value);
    axisYRotation.write(newY);
    oldY = newY;
  }

 
  // Read "QNH" Z Axis Rotation Encoder Knob
  int newZ = axisZRotation.read();
  if (newZ > oldZ) {
    //Determine speed of increment & set output
    int difZ = newZ - oldZ;
    RzAxis_Value = speedVal(difZ, RzAxis_Value, 1);
    Joystick.setRzAxis(RzAxis_Value);
    axisZRotation.write(newZ);
    oldZ = newZ;

  }else if (newZ < oldZ) {
    //Determine speed of decrement & set output
    int difZ = oldZ - newZ;
    RzAxis_Value = speedVal(difZ, RzAxis_Value, 0);
    Joystick.setRzAxis(RzAxis_Value);
    axisZRotation.write(newZ);
    oldZ = newZ;
  }


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