#include "hardware.h"

// Initialise the hardware
void appInitHardware(void) {
    initHardware();
}
// Initialise the software
TICK_COUNT appInitSoftware(TICK_COUNT loopStart){
    //PRINTF(stdout,"Ready..\r\n");

    display.print("2xPWM,2xSR04,4x20LCD");
    display. setXY(0,1);
    display.print("D0=    cm; D1=    cm");
    display. setXY(0,2);
    display.print("V0=    V ; V1=    V ");
    display. setXY(0,3);
    display.print("A0=    A ; A1=    A ");

    return 0;
}
// This is the main loop
TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart) {

    {
        // Read the Devantech SRF04 sonar and store the results
        sonar0.read();
        DISTANCE_TYPE cm = sonar0.getDistance();
        display.setXY(4,1);
        display.print(cm);
        delay_ms(180); // freezes up without a delay
    }
    {
        // Read the Devantech SRF04 sonar and store the results
        sonar1.read();
        DISTANCE_TYPE cm = sonar1.getDistance();
        display.setXY(15,1);
        display.print(cm);
        delay_ms(180); // freezes up without a delay
    }
    {
        // Read the Analogue Input and store results
        uint16_t val = a2dConvert10bit(adc0);
        display.horizGraph(4,2,val,400,4);
    }

    {
        // Read the Analogue Input and store results
        uint16_t val = a2dConvert10bit(adc1);
        display.horizGraph(4,3,val,40,4);
    }

    {
        // Read the Analogue Input and store results
        uint16_t val = a2dConvert10bit(adc2);
        display.horizGraph(15,2,val,400,4);
    }

    {
        // Read the Analogue Input and store results
        uint16_t val = a2dConvert10bit(adc3);
        display.horizGraph(15,3,val,40,4);
    }

    // This example will move the motors back and forth using the loopStart time:
    TICK_COUNT ms = loopStart / 1000;       // Get current time in ms
    int16_t now = ms % (TICK_COUNT)10000;   // 10 sec for a full swing
    if(now >= (int16_t)5000){               // Goes from 0ms...5000ms
        now = (int16_t)10000 - now;         // then 5000ms...0ms
    }
    // Map it into DRIVE_SPEED range
    DRIVE_SPEED speed = interpolate(now, 0, 5000, DRIVE_SPEED_MIN, DRIVE_SPEED_MAX);
    // Set speed for all motors/servos
    motor0.setSpeed(speed);
    motor1.setSpeed(speed);
    // -------- End   Actuators -------
    return 0;
}