#include "hardware.h"
#include "armbotbuild.h"
#include <Gait/GaitRunner.h>

#define SCHEDULER_MAX_JOBS 20 // Set the max number of scheduled jobs

int zeroAlreadyPressed = 0;
int oneAlreadyPressed = 0;
int twoAlreadyPressed = 0;

int lasttime = 0;
int * data = 0;
int x = 0;
ACTUATOR_LIST PROGMEM all[] = {&GripperGrabber.actuator,&GripperRotate.actuator,&GripperSideways.actuator,&GripperUpDown.actuator,&ElbowUpDown.actuator,&ShoulderUpDown.actuator,&ShoulderRotate.actuator };

G8_RUNNER gait = MAKE_G8_RUNNER(all, animations);

void appInitHardware(void) {
        initHardware();
        gaitRunnerInit(&gait);
}

TICK_COUNT appInitSoftware(TICK_COUNT loopStart){
        rprintf("Robot Started\r\n");
        gaitRunnerPlay(&gait, 2, 5000, 10, 1);
        return 1000000;
}

//void checkSonar(void * data, TICK_COUNT lasttime, TICK_COUNT overflow){
//      rprintf("about to check sonar\r\n");
//      distanceRead(sonar);
//      rprintf("about to display sonar\r\n");
//      rprintf("Distance=%u\r\n",sonar.distance.cm);
//      scheduleJob(&checkSonar,data,lasttime,1000000);
//      rprintf("sonar rescheduled for 1 second\r\n");
//}

TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart) {

        //scheduleJob(&checkSonar,&data,lasttime, 0);

        if(SWITCH_pressed(&button0)){
                if (zeroAlreadyPressed == 0) {
                rprintf("Button 0 Pressed\r\n");
                gaitRunnerPlay(&gait, 0, 5000, 30, 2);
                gaitRunnerProcess(&gait);
                zeroAlreadyPressed = 1;
                }
        }

        if(SWITCH_released(&button0)){
                zeroAlreadyPressed = 0;
        }

        if(SWITCH_pressed(&button1)){
                if (oneAlreadyPressed == 0) {
                rprintf("Button 1 Pressed\r\n");
                gaitRunnerPlay(&gait, 1, 5000, 75, 1);
                gaitRunnerProcess(&gait);
                oneAlreadyPressed = 1;
                }
        }

        if (x >= 3){
            distanceRead(sonar);
            rprintf("Distance=%u\r\n",sonar.distance.cm);
            x++;
            }

        if(SWITCH_released(&button1)){
                oneAlreadyPressed = 0;
        }

        if(SWITCH_pressed(&button2)){
                if (twoAlreadyPressed == 0) {
                rprintf("Button 2 Pressed\r\n");
                gaitRunnerPlay(&gait, 3, 5000, 22, 10);
                gaitRunnerProcess(&gait);
                twoAlreadyPressed = 1;
                }
        }

        if(SWITCH_released(&button2)){
                twoAlreadyPressed = 0;
        }

        return 0;
}