#include <cstdlib>
#include <iostream>
#include <string>
#include <conio.h>
#include "nxt.h"
using namespace std;

//set up the NXT
Connection *connection = new Bluetooth();
Motor *motorA = new Motor(OUT_A, connection);
Motor *motorB = new Motor(OUT_B, connection);
Motor *motorC = new Motor(OUT_C, connection);

int main()
{
  try{
    cout << "Try to connect to the NXT" << endl;
    connection->connect(40);
    motorC->reset_rotation();
    cout << "Motor C rotation: " << motorC->get_rotation() << endl; //print out the rotation value;
    motorA->on(85); //make motor A turn forward with speed 85
    motorC->on(-85); //make motor A turn backwards with speed 85
    motorB->on(30,400);
    Sleep(2000);
    motorC->off();//make it coast
    motorA->stop();//break the motor
    cout << "Motor C rotation:" << motorC->get_rotation() << endl; //print out the rotation value;
    connection->disconnect();
  }
  catch (Nxt_exception& e){
    //some error occurred - print it out
    cout << e.what() << endl;
    cout << "error code: " << e.error_code() << endl;
    cout << "error type: " << e.error_type() << endl;
    cout << e.who() << endl;
    connection->disconnect();
  }
  system("pause");
  return 0;
}