/*
 * File:   Spin_main.c
 * Author: Vivian and Harini
 *
 * Created on July 26, 2016, 10:54 AM
 * Objective: Have the roach spin, stop, and do it 3 times before stopping forever
 *
 */
#include <stdio.h>
#include <stdlib.h>
#include "BOARD.h"
#include "roach.h"
#include "serial.h"
#include "timers.h"
//other includes here
#include "Navigation.h"
#include "Bumper.h"

#define T_360 2400 //time taken for one full revolution (about 2.25 seconds)
#define MAX_SPEED 10

void main(void) {

    //Initiate functions
    SERIAL_Init();
    BOARD_Init();
    Roach_Init();
    TIMERS_Init();

    int spins_remaining = 3;
    int steps = 5; //make the roach spin 3 times
    InitTimer(0, 1);

    while (1) {
        if (IsTimerExpired(0)) {
            switch (steps) {

                case 0:
                    Stop();
                    break;

                case 1:
                    if (steps == 1) {
                        InitTimer(0, T_360);
                        RotateLeft(MAX_SPEED);
                        if (IsTimerExpired(0)) {
                            break;
                        }
                    }

                case 2:
                    if (steps == 2) {
                        InitTimer(0, 1500);
                        Stop();
                        if (IsTimerExpired(0)) {
                            break;
                        }
                    }

                case 3:
                    if (steps == 3) {
                        InitTimer(0, T_360);
                        RotateLeft(MAX_SPEED);
                        if (IsTimerExpired(0)) {
                            break;
                        }
                    }

                case 4:
                    if (steps == 4) {
                        InitTimer(0, 1500);
                        Stop();
                        if (IsTimerExpired(0)) {
                            break;
                        }
                    }

                case 5:
                    if (steps == 5) {
                        InitTimer(0, T_360);
                        RotateLeft(MAX_SPEED);
                        if (IsTimerExpired(0)) {
                            break;
                        }
                    }

            }
            steps--;
        }
    }

}