#include <stdint.h>
#include "msp.h"
#include "..\inc\bump.h"
#include "..\inc\Clock.h"
#include "..\inc\SysTick.h"
#include "..\inc\LaunchPad.h"
#include "..\inc\MotorSimple.h"
#include "..\inc\Reflectance.c"

// Driver test
void Pause(void){
    while(LaunchPad_Input()==0);  // wait for touch
    while(LaunchPad_Input());     // wait for release
}

uint8_t Data;
int32_t Position;

int main(void){ // Program12_4, RSLK version 1.1
    Clock_Init48MHz();
    LaunchPad_Init();   // built-in switches and LEDs
    Bump_Init();        // bump switches
    Motor_InitSimple(); // initialization
    Reflectance_Init();

    while(1){

        Data = Reflectance_Read(1900);
        Position = Reflectance_Position(Data);

        if(Position <60 && Position > -60){
            LaunchPad_Output(0x02);
            Motor_ForwardSimple(4200,2);
            Clock_Delay1ms(10);
        }

        else if (Position > 60) {
            LaunchPad_Output(0x02);
            Motor_LeftSimple(4000, -5);
            Clock_Delay1ms(10);
            Motor_ForwardSimple(3000,2);

        }

        else if ( Position < -60 ) {
            LaunchPad_Output(0x02);s
            Motor_RightSimple(4000, 5);
            Clock_Delay1ms(10);
            Motor_ForwardSimple(3000,2);
        }

        // else

        if(Bump_Read()){
            LaunchPad_Output(0x01);
            Motor_BackwardSimple(4000,5);
            Clock_Delay1ms(10);
        }
    }
}