#include "mbed.h"
//#include "OpenChair.h"

//Define variables
char            str[80];
int             i;
bool            TO = false;

//Open serial port
Serial          pc(SERIAL_TX, SERIAL_RX);

//Initialise OpenChair
OpenChair       chair(PC_12,PD_2,PC_10,PC_11,60,150); //60,150
//Define      Peripherals
InterruptIn     emergencyButton(PA_9);
volatile bool   emergencyBtnEnabled = true;
volatile bool   emergencyBtnPressed = false;
InterruptIn     takeOverButton(PB_5);
volatile bool   takeOverBtnEnabled = true;
volatile bool   takeOverBtnPressed = false;
AnalogIn        potentiometerX(PA_0);
AnalogIn        potentiometerY(PA_1);
DigitalOut      notifierLED(PB_13);
DigitalOut      relay(PB_6);
DigitalOut      buzzer(PB_12);  //led pb3
Timeout         debounceBtn;    // button bouncing Timeout

// Enables Emergency button when bouncing is over
void enableEmergencyBtn(void) {
    emergencyBtnEnabled = true;
}

// ISR handling Emergency button press event
void onEmergencyBtnPressed(void) {
    if (emergencyBtnEnabled) { // disabled while the button is bouncing
        emergencyBtnEnabled = false;
        emergencyBtnPressed = true;
        debounceBtn.attach(callback(enableEmergencyBtn), 0.3);  // debounce time = 0.3s
    }
}

// Enables TakeOver button when bouncing is over
void enableTakeOverBtn(void) {
    takeOverBtnEnabled = true;
}

// ISR handling TakeOver button press event
void onTakeOverBtnPressed(void) {
    if (takeOverBtnEnabled) { // disabled while the button is bouncing
        takeOverBtnEnabled = false;
        takeOverBtnPressed = true;
        debounceBtn.attach(callback(enableTakeOverBtn), 0.3);   // debounce time = 0.3s
    }
}

int main(void) {
    emergencyButton.mode(PullUp);
    takeOverButton.mode(PullUp);

    emergencyButton.fall(callback(onEmergencyBtnPressed)); // attach ISR to handle button press event
    takeOverButton.fall(callback(onTakeOverBtnPressed));   // attach ISR to handle button press event

    //State the variables which hold the data for the position of the potentiometers of the addon box.
    pc.baud(115200);

    double  valueX;
    double  valueY;

    //Turn on board and hold the power button after poweroff so that the release of it causes the motherboard to turn off.

    pc.printf("Relay Starting\n");
    wait(1);    // Wait one second before beginning turn on sequence incase system startup is due to someone playing around with the switch.
    relay = 1;
    wait(0.5);
    relay = 0;
    wait(0.5);
    relay = 1;
    pc.printf("Relay Finishing\n");

    // Begin serial communication.
    pc.baud(115200);
    pc.printf("Starting Loop\n");
    while (1) {
        if (emergencyBtnPressed) {
            emergencyBtnPressed = false;
            pc.printf("Emergency mode activated. Shutting down.\n");

            //relay = 0;
            pc.printf("Relay off\n");
        }
        else
        if (takeOverBtnPressed) {
            takeOverBtnPressed = false;
            pc.printf("Takeover button on\n");
            TO = !TO;
            pc.printf("Takeover button toggled\n");
        }
        else
        if (TO == true) {
            pc.printf("Takeover button variable activated and joystick drive enacted\n");
            valueX = (double)(potentiometerX.read() * 2 - 1);
            valueY = (double)(potentiometerY.read() * 2 - 1);
            pc.printf("Potentiometer Values: %.02f    %.02f\n", valueX, valueY);

            chair.drive(valueX, valueY, 1);
            wait_ms(50);
        }
        else
        if (TO == false) {
            // ...
        }
    }   // end while
}   //end main