int LEFT_MOTOR=0;
int RIGHT_MOTOR=3;
int leftTurn = 0;
int rightTurn = 1;

void forward()
{
    motor(LEFT_MOTOR,50);
    motor(RIGHT_MOTOR,50);
}

void backward()
{
    motor(LEFT_MOTOR,-50);
    motor(RIGHT_MOTOR, -50);
}

void right()
{
    motor(LEFT_MOTOR,50);
    motor(RIGHT_MOTOR,-50);
}

void left()
{
    motor(RIGHT_MOTOR,50);
    motor(LEFT_MOTOR,-50);
}

void stop()
{
    off(LEFT_MOTOR);
    off(RIGHT_MOTOR);
}

void startup(){
    tone(800.,0.2);
    tone(1000.,0.2);
}

void shutdown(){
    tone(1567.98,0.1);
    tone(1479.98,0.1);
    tone(1244.51,0.1);
    tone(880.00,0.1);
    tone(830.61,0.1);
    tone(1318.,0.1);
    tone(1661.22,0.1);
    tone(2093.00,0.1);
}

void turnForDegrees(float degrees, int direction) {
    float timeToTurn = (2.68/360.)*degrees;
    restartProgram();
    if(direction == 0)
      {
        left();
    }
    else
      {
        right();
    }
    sleep(timeToTurn);
    stop();
    sleep(0.05);
}

void goForward(float distance) {
    float travelTime = distance/15.;
    restartProgram();
    forward();
    sleep(travelTime);
    backward();
    sleep(0.045);
    stop();
    sleep(0.05);
}

void goBackward(float distance) {
    float travelTime = distance/15.;
    restartProgram();
    backward();
    sleep(travelTime);
    forward();
    sleep(0.045);
    stop();
    sleep(0.05);
}

void restartProgram() {
    if(stop_button() == 1){
        main();
        printf("Restarting! \n"); sleep(0.5);
    }
}

void main() {
    while(stop_button() == 0)
      {
        printf("Ready to go! \n");
    }
    startup();
    goForward(17.);
    turnForDegrees(37.,leftTurn);
    goForward(20.);
    turnForDegrees(90.,rightTurn);
    goForward(12.); //scores the first puck
    goBackward(7.);
    turnForDegrees(191.,leftTurn);
    goForward(9.);
    turnForDegrees(20., rightTurn);
    goForward(11.);
    turnForDegrees(18., leftTurn);
    goForward(10.);
    turnForDegrees(79., rightTurn);
    goForward(20.); //scores second puck
    goBackward(8.);
    turnForDegrees(65., rightTurn);
    goForward(23.);
    turnForDegrees(39., leftTurn);
    goForward(12.);
    turnForDegrees(33., leftTurn);
    goForward(11.); //scores third puck (if david is nice)
    goBackward(24.);
    turnForDegrees(20., leftTurn);
    goForward(26.);
    goBackward(8.);
    stop(); sleep(20.);
    shutdown();
    while(1){
        restartProgram();
    }
}