#include "Arduino.h"
#include "AxisJoystick.h"
#include "Servo.h"


// Pin Definitions
#define JOYSTICK_1_PIN_SW  2
#define JOYSTICK_1_PIN_VRX  A3
#define JOYSTICK_1_PIN_VRY  A1
#define JOYSTICK_2_PIN_SW 3
#define JOYSTICK_2_PIN_VRX  A5
#define JOYSTICK_2_PIN_VRY  A4
#define SERVO360MICRO1_1_PIN_SIG  4
#define SERVO360MICRO2_2_PIN_SIG  5



// Global variables and defines

// object initialization
AxisJoystick joystick_1(JOYSTICK_1_PIN_VRX, JOYSTICK_1_PIN_VRY, JOYSTICK_1_PIN_SW);
AxisJoystick joystick_2(JOYSTICK_2_PIN_VRX, JOYSTICK_2_PIN_VRY, JOYSTICK_2_PIN_SW);
Servo servo360Micro1_1;
Servo servo360Micro2_2;


// define vars for testing menu
const int timeout = 10000;       //define timeout of 10 sec
char menuOption = 0;
long time0;

// Setup the essentials for your circuit to work. It runs first every time your circuit is powered with electricity.
void setup()
{
  // Setup Serial which is useful for debugging
  // Use the Serial Monitor to view printed messages
  Serial.begin(9600);
  while (!Serial) ; // wait for serial port to connect. Needed for native USB
  Serial.println("start");


  menuOption = menu();

}

// Main logic of your circuit. It defines the interaction between the components you selected. After setup, it runs over and over again, in an eternal loop.
void loop()
{


  if (menuOption == '1') {
    // PS2 X Y Axis Joystick Module #1 - Test Code
    // Read Joystick X,Y axis and press
    int joystick_1X =  joystick_1.xAxis();
    int joystick_1Y =  joystick_1.yAxis();
    int joystick_1SW =  joystick_1.readSW();
    Serial.print(F("X: ")); Serial.print(joystick_1X);
    Serial.print(F("\tY: ")); Serial.print(joystick_1Y);
    Serial.print(F("\tSW: ")); Serial.println(joystick_1SW);

  }
  else if (menuOption == '2') {
    // PS2 X Y Axis Joystick Module #2 - Test Code
    // Read Joystick X,Y axis and press
    int joystick_2X =  joystick_2.xAxis();
    int joystick_2Y =  joystick_2.yAxis();
    int joystick_2SW =  joystick_2.readSW();
    Serial.print(F("X: ")); Serial.print(joystick_2X);
    Serial.print(F("\tY: ")); Serial.print(joystick_2Y);
    Serial.print(F("\tSW: ")); Serial.println(joystick_2SW);

  }
  else if (menuOption == '3') {
    // Continuous Rotation Micro Servo - FS90R #1 - Test Code
    // The servo will rotate CW in full speed, CCW in full speed, and will stop  with an interval of 2000 milliseconds (2 seconds)
    servo360Micro1_1.attach(SERVO360MICRO1_1_PIN_SIG);         // 1. attach the servo to correct pin to control it.
    servo360Micro1_1.write(180);  // 2. turns servo CW in full speed. change the value in the brackets (180) to change the speed. As these numbers move closer to 90, the servo will move slower in that direction.
    delay(2000);                              // 3. waits 2000 milliseconds (2 sec). change the value in the brackets (2000) for a longer or shorter delay in milliseconds.
    servo360Micro1_1.write(0);    // 4. turns servo CCW in full speed. change the value in the brackets (0) to change the speed. As these numbers move closer to 90, the servo will move slower in that direction.
    delay(2000);                              // 5. waits 2000 milliseconds (2 sec). change the value in the brackets (2000) for a longer or shorter delay in milliseconds.
    servo360Micro1_1.write(90);    // 6. sending 90 stops the servo
    delay(2000);                              // 7. waits 2000 milliseconds (2 sec). change the value in the brackets (2000) for a longer or shorter delay in milliseconds.
    servo360Micro1_1.detach();                    // 8. release the servo to conserve power. When detached the servo will NOT hold it's position under stress.
  }
  else if (menuOption == '4') {
    // Continuous Rotation Micro Servo - FS90R #2 - Test Code
    // The servo will rotate CW in full speed, CCW in full speed, and will stop  with an interval of 2000 milliseconds (2 seconds)
    servo360Micro2_2.attach(SERVO360MICRO2_2_PIN_SIG);         // 1. attach the servo to correct pin to control it.
    servo360Micro2_2.write(180);  // 2. turns servo CW in full speed. change the value in the brackets (180) to change the speed. As these numbers move closer to 90, the servo will move slower in that direction.
    delay(2000);                              // 3. waits 2000 milliseconds (2 sec). change the value in the brackets (2000) for a longer or shorter delay in milliseconds.
    servo360Micro2_2.write(0);    // 4. turns servo CCW in full speed. change the value in the brackets (0) to change the speed. As these numbers move closer to 90, the servo will move slower in that direction.
    delay(2000);                              // 5. waits 2000 milliseconds (2 sec). change the value in the brackets (2000) for a longer or shorter delay in milliseconds.
    servo360Micro2_2.write(90);    // 6. sending 90 stops the servo
    delay(2000);                              // 7. waits 2000 milliseconds (2 sec). change the value in the brackets (2000) for a longer or shorter delay in milliseconds.
    servo360Micro2_2.detach();                    // 8. release the servo to conserve power. When detached the servo will NOT hold it's position under stress.
  }

  if (millis() - time0 > timeout)
  {
    menuOption = menu();
  }

}



// Menu function for selecting the components to be tested
// Follow serial monitor for instrcutions
char menu()
{

  Serial.println(F("\nWhich component would you like to test?"));
  Serial.println(F("(1) PS2 X Y Axis Joystick Module #1"));
  Serial.println(F("(2) PS2 X Y Axis Joystick Module #2"));
  Serial.println(F("(3) Continuous Rotation Micro Servo - FS90R #1"));
  Serial.println(F("(4) Continuous Rotation Micro Servo - FS90R #2"));
  Serial.println(F("(menu) send anything else or press on board reset button\n"));
  while (!Serial.available());

  // Read data from serial monitor if received
  while (Serial.available())
  {
    char c = Serial.read();
    if (isAlphaNumeric(c))
    {

      if (c == '1')
        Serial.println(F("Now Testing PS2 X Y Axis Joystick Module #1"));
      else if (c == '2')
        Serial.println(F("Now Testing PS2 X Y Axis Joystick Module #2"));
      else if (c == '3')
        Serial.println(F("Now Testing Continuous Rotation Micro Servo - FS90R #1"));
      else if (c == '4')
        Serial.println(F("Now Testing Continuous Rotation Micro Servo - FS90R #2"));
      else
      {
        Serial.println(F("illegal input!"));
        return 0;
      }
      time0 = millis();
      return c;
    }
  }
}