#include "NXT++.h"
#include <iostream>
#include <fstream>
#include <string>
#include <sstream>
#include <windows.h>	// Use for Sleep() in milliseconds
#include <time.h>

#define MOVE_TIME 500
#define ROTATE_TIME 500
#define MOTOR_POWER 30
#define MOTOR_POWER_2 60

using namespace std;

void DriveForward();
void DriveBackward();
void TurnPivotLeft();
void TurnPivotRight();
void TurnInPlaceLeft();
void TurnInPlaceRight();
void DriveStop();

Comm::NXTComm comm;

int main()
{
	srand(time(NULL));
	 //initialize the NXT and continue if it suceeds Use.Open() or OpenBT()
	if(NXT::OpenBT(&comm))
	{
		ifstream infile;
		string input;
		{
			infile.open("orders.txt");
			if(infile.good())
			{
				getline(infile, input);
				infile.close();
				if(input.compare("0") == 0)
				{
					DriveForward();
					cout<<"forward";
				}
				else
				{
					DriveBackward();
					if(rand() % 2 == 0)
					{
						TurnInPlaceLeft();
					}
					else
					{
						TurnInPlaceRight();
					}
				}
			}
		}
	}
}

void DriveForward()
{
	NXT::Motor::SetForward(&comm, OUT_B, MOTOR_POWER);
	NXT::Motor::SetForward(&comm, OUT_C, MOTOR_POWER);
	Wait(MOVE_TIME);
	DriveStop();
}

void DriveBackward()
{
	NXT::Motor::SetReverse(&comm, OUT_B, MOTOR_POWER);
	NXT::Motor::SetReverse(&comm, OUT_C, MOTOR_POWER);
	Wait(MOVE_TIME);
	DriveStop();
}

void TurnPivotLeft()
{
	NXT::Motor::SetForward(&comm, OUT_B, MOTOR_POWER);
	Wait(ROTATE_TIME);
	DriveForward();
	//DriveStop();
}

void TurnPivotRight()
{
	NXT::Motor::SetForward(&comm, OUT_C, MOTOR_POWER);
	Wait(ROTATE_TIME);
	DriveForward();
	//DriveStop();
}

void TurnInPlaceLeft()
{
	NXT::Motor::SetForward(&comm, OUT_B, MOTOR_POWER);
	NXT::Motor::SetReverse(&comm, OUT_C, MOTOR_POWER);
	//Wait(ROTATE_TIME);
	Wait(rand() % 1500);
	DriveForward();
	DriveForward();
	//DriveStop();
}

void TurnInPlaceRight()
{
	NXT::Motor::SetReverse(&comm, OUT_B, MOTOR_POWER);
	NXT::Motor::SetForward(&comm, OUT_C, MOTOR_POWER);
	//Wait(ROTATE_TIME);
	Wait(rand() % 1500);
	DriveForward();
	DriveForward();
	//DriveStop();
}

void DriveStop()
{
	NXT::Motor::Stop(&comm, OUT_B, true);
	NXT::Motor::Stop(&comm, OUT_C, true);
}