#include <Adafruit_SSD1306.h>
#include <Servo.h>
#include <Adafruit_GFX.h>
#include <Wire.h>
#include <SPI.h>


// Pin Definitions
#define LEDB_PIN_VIN	0
#define LEDR_PIN_VIN	2
#define PUSHBUTTONMOMENTARY_1_PIN_2	4
#define PUSHBUTTONMOMENTARY_2_PIN_2	16
#define PUSHBUTTONMOMENTARY_3_PIN_2	17
#define PUSHBUTTONMOMENTARY_4_PIN_2	5
#define PUSHBUTTONMOMENTARY_5_PIN_2	18
#define SERVOMD1_1_PIN_SIG	32
#define SERVOMD2_2_PIN_SIG	33



// Global variables and defines
const int servoMD1_1RestPosition   = 20;  //Starting position
const int servoMD1_1TargetPosition = 150; //Position when event is detected
const int servoMD2_2RestPosition   = 20;  //Starting position
const int servoMD2_2TargetPosition = 150; //Position when event is detected
// object initialization
QMC5883L compass;
LED ledB(LEDB_PIN_VIN);
LED ledR(LEDR_PIN_VIN);
#define SSD1306_LCDHEIGHT 32
Adafruit_SSD1306 oLed128x32(OLED128X32_PIN_RST);
Button pushButtonMomentary_1(PUSHBUTTONMOMENTARY_1_PIN_2);
Button pushButtonMomentary_2(PUSHBUTTONMOMENTARY_2_PIN_2);
Button pushButtonMomentary_3(PUSHBUTTONMOMENTARY_3_PIN_2);
Button pushButtonMomentary_4(PUSHBUTTONMOMENTARY_4_PIN_2);
Button pushButtonMomentary_5(PUSHBUTTONMOMENTARY_5_PIN_2);
Servo servoMD1_1;
Servo servoMD2_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");
    
    // Set declination angle on your location and fix heading
    // You can find your declination on: http://magnetic-declination.com/
    // (+) Positive or (-) for negative
    // For declination angle 4'36E (positive)
    compass.begin(+4,36);
    oLed128x32.begin(SSD1306_SWITCHCAPVCC, 0x3C);  // initialize with the I2C addr 0x3C (for the 128x32)
    oLed128x32.clearDisplay(); // Clear the buffer.
    oLed128x32.display();
    pushButtonMomentary_1.init();
    pushButtonMomentary_2.init();
    pushButtonMomentary_3.init();
    pushButtonMomentary_4.init();
    pushButtonMomentary_5.init();
    servoMD1_1.attach(SERVOMD1_1_PIN_SIG);
    servoMD1_1.write(servoMD1_1RestPosition);
    delay(100);
    servoMD1_1.detach();
    servoMD2_2.attach(SERVOMD2_2_PIN_SIG);
    servoMD2_2.write(servoMD2_2RestPosition);
    delay(100);
    servoMD2_2.detach();
    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')
    {
    // Disclaimer: The Ublox NEO-6M GPS Module is in testing and/or doesn't have code, therefore it may be buggy. Please be kind and report any bugs you may find.
    }
    else if(menuOption == '2') {
    // HMC5883L - Magnetometer (Compass) - Test Code
    //Read heading value from compass
    float compassHeading = compass.getHeadingDeg();
    Serial.print(F("Heading: ")); Serial.print(compassHeading); Serial.println(F("[°]"));

    }
    else if(menuOption == '3') {
    // LED - Basic Blue 5mm - Test Code
    // The LED will turn on and fade till it is off
    for(int i=255 ; i> 0 ; i -= 5)
    {
        ledB.dim(i);                      // 1. Dim Led 
        delay(15);                               // 2. waits 5 milliseconds (0.5 sec). Change the value in the brackets (500) for a longer or shorter delay in milliseconds.
    }                                          
    ledB.off();                        // 3. turns off
    }
    else if(menuOption == '4') {
    // LED - Basic Red 5mm - Test Code
    // The LED will turn on and fade till it is off
    for(int i=255 ; i> 0 ; i -= 5)
    {
    ledR.dim(i);                      // 1. Dim Led 
    delay(15);                               // 2. waits 5 milliseconds (0.5 sec). Change the value in the brackets (500) for a longer or shorter delay in milliseconds.
    }                                          
    ledR.off();                        // 3. turns off
    }
    else if(menuOption == '5') {
    // Monochrome 128x32 I2C OLED graphic display - Test Code
    oLed128x32.setTextSize(1);
    oLed128x32.setTextColor(WHITE);
    oLed128x32.setCursor(0, 10);
    oLed128x32.clearDisplay();
    oLed128x32.println("Circuito.io Rocks!");
    oLed128x32.display();
    delay(1);
    oLed128x32.startscrollright(0x00, 0x0F);
    delay(2000);
    oLed128x32.stopscroll();
    delay(1000);
    oLed128x32.startscrollleft(0x00, 0x0F);
    delay(2000);
    oLed128x32.stopscroll();
    }
    else if(menuOption == '6') {
    // Momentary Push Button - Panel Mount (Red) #1 - Test Code
    //Read pushbutton state. 
    //if button is pressed function will return HIGH (1). if not function will return LOW (0). 
    //for debounce funtionality try also pushButtonMomentary_1.onPress(), .onRelease() and .onChange().
    //if debounce is not working properly try changing 'debounceDelay' variable in Button.h
    bool pushButtonMomentary_1Val = pushButtonMomentary_1.read();
    Serial.print(F("Val: ")); Serial.println(pushButtonMomentary_1Val);
    }
    else if(menuOption == '7') {
    // Momentary Push Button - Panel Mount (Red) #2 - Test Code
    //Read pushbutton state. 
    //if button is pressed function will return HIGH (1). if not function will return LOW (0). 
    //for debounce funtionality try also pushButtonMomentary_2.onPress(), .onRelease() and .onChange().
    //if debounce is not working properly try changing 'debounceDelay' variable in Button.h
    bool pushButtonMomentary_2Val = pushButtonMomentary_2.read();
    Serial.print(F("Val: ")); Serial.println(pushButtonMomentary_2Val);
    }
    else if(menuOption == '8') {
    // Momentary Push Button - Panel Mount (Red) #3 - Test Code
    //Read pushbutton state. 
    //if button is pressed function will return HIGH (1). if not function will return LOW (0). 
    //for debounce funtionality try also pushButtonMomentary_3.onPress(), .onRelease() and .onChange().
    //if debounce is not working properly try changing 'debounceDelay' variable in Button.h
    bool pushButtonMomentary_3Val = pushButtonMomentary_3.read();
    Serial.print(F("Val: ")); Serial.println(pushButtonMomentary_3Val);
    }
    else if(menuOption == '9') {
    // Momentary Push Button - Panel Mount (Red) #4 - Test Code
    //Read pushbutton state. 
    //if button is pressed function will return HIGH (1). if not function will return LOW (0). 
    //for debounce funtionality try also pushButtonMomentary_4.onPress(), .onRelease() and .onChange().
    //if debounce is not working properly try changing 'debounceDelay' variable in Button.h
    bool pushButtonMomentary_4Val = pushButtonMomentary_4.read();
    Serial.print(F("Val: ")); Serial.println(pushButtonMomentary_4Val);
    }
    else if(menuOption == '10') {
    // Momentary Push Button - Panel Mount (Red) #5 - Test Code
    //Read pushbutton state. 
    //if button is pressed function will return HIGH (1). if not function will return LOW (0). 
    //for debounce funtionality try also pushButtonMomentary_5.onPress(), .onRelease() and .onChange().
    //if debounce is not working properly try changing 'debounceDelay' variable in Button.h
    bool pushButtonMomentary_5Val = pushButtonMomentary_5.read();
    Serial.print(F("Val: ")); Serial.println(pushButtonMomentary_5Val);
    }
    else if(menuOption == '11') {
    // Standard Size - High Torque - Metal Gear Servo - MG995 #1 - Test Code
    // The servo will rotate to target position and back to resting position with an interval of 500 milliseconds (0.5 seconds) 
    servoMD1_1.attach(SERVOMD1_1_PIN_SIG);         // 1. attach the servo to correct pin to control it.
    servoMD1_1.write(servoMD1_1TargetPosition);  // 2. turns servo to target position. Modify target position by modifying the 'ServoTargetPosition' definition above.
    delay(500);                              // 3. waits 500 milliseconds (0.5 sec). change the value in the brackets (500) for a longer or shorter delay in milliseconds.
    servoMD1_1.write(servoMD1_1RestPosition);    // 4. turns servo back to rest position. Modify initial position by modifying the 'ServoRestPosition' definition above.
    delay(500);                              // 5. waits 500 milliseconds (0.5 sec). change the value in the brackets (500) for a longer or shorter delay in milliseconds.
    servoMD1_1.detach();                    // 6. release the servo to conserve power. When detached the servo will NOT hold it's position under stress.
    }
    else if(menuOption == '12') {
    // Standard Size - High Torque - Metal Gear Servo - MG995 #2 - Test Code
    // The servo will rotate to target position and back to resting position with an interval of 500 milliseconds (0.5 seconds) 
    servoMD2_2.attach(SERVOMD2_2_PIN_SIG);         // 1. attach the servo to correct pin to control it.
    servoMD2_2.write(servoMD2_2TargetPosition);  // 2. turns servo to target position. Modify target position by modifying the 'ServoTargetPosition' definition above.
    delay(500);                              // 3. waits 500 milliseconds (0.5 sec). change the value in the brackets (500) for a longer or shorter delay in milliseconds.
    servoMD2_2.write(servoMD2_2RestPosition);    // 4. turns servo back to rest position. Modify initial position by modifying the 'ServoRestPosition' definition above.
    delay(500);                              // 5. waits 500 milliseconds (0.5 sec). change the value in the brackets (500) for a longer or shorter delay in milliseconds.
    servoMD2_2.detach();                    // 6. 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) Ublox NEO-6M GPS Module"));
    Serial.println(F("(2) HMC5883L - Magnetometer (Compass)"));
    Serial.println(F("(3) LED - Basic Blue 5mm"));
    Serial.println(F("(4) LED - Basic Red 5mm"));
    Serial.println(F("(5) Monochrome 128x32 I2C OLED graphic display"));
    Serial.println(F("(6) Momentary Push Button - Panel Mount (Red) #1"));
    Serial.println(F("(7) Momentary Push Button - Panel Mount (Red) #2"));
    Serial.println(F("(8) Momentary Push Button - Panel Mount (Red) #3"));
    Serial.println(F("(9) Momentary Push Button - Panel Mount (Red) #4"));
    Serial.println(F("(10) Momentary Push Button - Panel Mount (Red) #5"));
    Serial.println(F("(11) Standard Size - High Torque - Metal Gear Servo - MG995 #1"));
    Serial.println(F("(12) Standard Size - High Torque - Metal Gear Servo - MG995 #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 Ublox NEO-6M GPS Module - note that this component doesn't have a test code"));
    		else if(c == '2') 
    			Serial.println(F("Now Testing HMC5883L - Magnetometer (Compass)"));
    		else if(c == '3') 
    			Serial.println(F("Now Testing LED - Basic Blue 5mm"));
    		else if(c == '4') 
    			Serial.println(F("Now Testing LED - Basic Red 5mm"));
    		else if(c == '5') 
    			Serial.println(F("Now Testing Monochrome 128x32 I2C OLED graphic display"));
    		else if(c == '6') 
    			Serial.println(F("Now Testing Momentary Push Button - Panel Mount (Red) #1"));
    		else if(c == '7') 
    			Serial.println(F("Now Testing Momentary Push Button - Panel Mount (Red) #2"));
    		else if(c == '8') 
    			Serial.println(F("Now Testing Momentary Push Button - Panel Mount (Red) #3"));
    		else if(c == '9') 
    			Serial.println(F("Now Testing Momentary Push Button - Panel Mount (Red) #4"));
    		else if(c == '10') 
    			Serial.println(F("Now Testing Momentary Push Button - Panel Mount (Red) #5"));
    		else if(c == '11') 
    			Serial.println(F("Now Testing Standard Size - High Torque - Metal Gear Servo - MG995 #1"));
    		else if(c == '12') 
    			Serial.println(F("Now Testing Standard Size - High Torque - Metal Gear Servo - MG995 #2"));
            else
            {
                Serial.println(F("illegal input!"));
                return 0;
            }
            time0 = millis();
            return c;
        }
    }
}