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

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

    int lasttime = 0;
    int * data = 0;

    int i = 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 0;
    }

    //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");
                                    distanceRead(sonar);
                                    rprintf("Distance=%u\r\n",sonar.distance.cm);
                    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");
                                    distanceRead(sonar);
                                    rprintf("Distance=%u\r\n",sonar.distance.cm);
                    gaitRunnerPlay(&gait, 1, 5000, 75, 1);
                    gaitRunnerProcess(&gait);
                    oneAlreadyPressed = 1;
                    }
            }

    //              {
    //      int i;
    //              for (i = 0; i < 5; i++)
    //              {
    //                      delay_ms(50);
    //              }

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

    //      return 0;
    //              }

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

            if(SWITCH_pressed(&button2)){
                    if (twoAlreadyPressed == 0) {
                    rprintf("Button 2 Pressed\r\n");
                                    distanceRead(sonar);
                                    rprintf("Distance=%u\r\n",sonar.distance.cm);
                    gaitRunnerPlay(&gait, 3, 5000, 22, 10);
                    gaitRunnerProcess(&gait);
                    twoAlreadyPressed = 1;
                    }
            }

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

            return 0;
    }