#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;

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(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;
}