#include "eyebot.h"

void turnLeft(int);
void turnRight(int);
void moveForward(int);
void moveBackward(int);
void pairing();
void pushButton(int, int);

#define RIGHT 6 //physical pin 22, wiring pin 6
#define LEFT 4 //physical pin 16, wiring pin 4
#define PARK 21 //physical pin 29, wiring pin 21
#define CON 3 //physical pin 15, wp 3
#define REVERSE 0 //physical 11, wp 0
#define FORWARD 26 //physical 32, wiring pin 26
#define SPEED 2 //physical pin 13, wiring pin 2

#define SECOND 1000000

int main() {
        wiringPiSetup();
        pinMode(RIGHT, OUTPUT); //Turn right
        pinMode(LEFT, OUTPUT); //Turn left
        pinMode(PARK, OUTPUT); //Park
        pinMode(CON, OUTPUT); //Pairing/remote on
        pinMode(REVERSE, OUTPUT); //Down
        pinMode(FORWARD, OUTPUT); //Up
        pinMode(SPEED, OUTPUT); //Speed
        
        //This is a demo of the different ways to control the car

        //This pairing process only has to be done once and 
        //and then the car will always remember the remote
        //Pushing the pairing button for 4 seconds
        pushButton(CON, 4);
        //Waiting for 3 seconds
        usleep(3*SECOND);      
        
        //Pushing reverse for three seconds.
        pushButton(REVERSE, 3);

        //Pushing forwards for three seconds.
        pushButton(FORWARD, 3);

        //Pushing left for three seconds.
        pushButton(LEFT, 3);

        //Pushing right for three seconds.
        pushButton(RIGHT, 3);
        
        //Pushing park toggles the parking mode on.
        //Pushing park for 1 second
        pushButton(PARK, 1);
        //The car is now parked and will not move.

        //Pushing park for 1 second.
        pushButton(PARK, 1);
        //The car is no longer parked and will now move.
        
       //The speedmode button cycles through each speedmode.
       //Each press increases the speed by 1, from 1 - 3.
       //Pressing it while it is at 3 simply brings it back
       //down to 1.
       
       //Push speed for one second, assuming we are at 1
       pushButton(SPEED, 1);
       //We are now in speedmode 2
       //Push speed for one second
       pushButton(SPEED, 1);
       //We are now in speedmode 3

       //Push speed for one second
       pushMode(SPEED, 1);
       //We are now back in speedmode 1

       }

//Pushes the given button for the given duration (in seconds)
void pushButton(int inPin, int duration) {
       digitalWrite(inPin, HIGH);
       usleep(duration*SECOND);
       digitalWrite(inPin, LOW);
}