/*
  movement-test.c
  MyroC general commands testing
  Written by Jordan Yuan
  Written on 11-1 2013
  Modified by Anita DeWitt, Nick Knoebber, Jason Liu, Vasilisa Bashlovkina

  Testing for the following general MyroC commands:

  rTurnLeft
  rTurnRight
  TurnSpeed
  rForward
  rFastForward
  rBackward
  rMotors
  rStop
  rHardStop 

  Compile using
     gcc -lbluetooth -lMyroCDev -leSpeakPackage -o movement-test movement-test.c

*/

#include "MyroC.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>


int main(){

 rConnect ("/dev/rfcomm0");
  rBeep (2, 880);
  rSetForwardness("fluke-forward");
  eSpeakConnect();
  eSpeakSetGender("female");
  //rSetForwardness ("fluke-forward");
  
  printf("Test 1 turn left\n");
  eSpeakTalk("Test 1 turn left");
  /* turns Scribbler left for a specified speed and time */
  rTurnLeft(0.4,1.0);
  rTurnLeft(0.4,1.0);
  rTurnLeft(2.0,0.1);
  rTurnLeft(0.2,1.0);
  eSpeakTalk("Non blocking test, press enter to stop");
  rTurnLeft(1.0,-1);
  rBeep(1,880);
  rBeep(1,990);
  getchar();
  rStop();

  
  printf("Test 2 turn right\n");
  eSpeakTalk("Test 2 turn right");
  /* turns Scribbler right for a specified speed and time */
  rTurnRight(0.4, 1.0);
  rTurnRight(0.4, 1.0);
  rTurnRight(2.0,0.1);
  rTurnRight(0.2,1.0);
  eSpeakTalk("Non blocking test, press enter to stop");
  rTurnRight(1.0,-1);
  rBeep(1,880);
  rBeep(1,990);
  getchar();
  rStop();

  printf("Test 3 turn speed\n");
  eSpeakTalk("Test 3 turn speed");
  /* turns Scribbler in direction for a specified speed at a 
     specified time
     -1 = left.
     1 = right.
  */
  printf("turn right for 1 speed, at 1 second\n");
  rTurnSpeed("right",1,1);
  printf("turn left for 1 speed, at 1 second\n");
  rTurnSpeed("left",1,1) ;
  printf("turn right for 0.2 speed, at 2 second\n");
  rTurnSpeed("right",0.2,2);
  printf("turn left for 0.22 speed, at 2 second\n");
  rTurnSpeed("left",0.2,2);

  printf("Test 4 rForward\n");
  eSpeakTalk("Test 4 R forward");
  /* moves Scribbler forward for a specified speed and time */
  eSpeakTalk ("Move forward for 2 seconds ");
  rForward(0.5,2.0);
  eSpeakTalk ("Move forward for 3 seconds ");
  rForward(0.1,3.0);
  eSpeakTalk ("Move forward for 4 seconds");
  rForward(0.8,4.0);
  eSpeakTalk("Non blocking test, press enter to stop");
  rForward(1.0,-1);
  rBeep(1,880);
  rBeep(1,990);
  getchar();
  rStop();

  printf("Test 5 rFastForward\n");
  eSpeakTalk("Test 5 R fast forward");
  /* moves Scribbler forward at maximum speed for a specified time */
  eSpeakTalk ("Move forward for 1 second ");
  rFastForward(1.0);
  eSpeakTalk ("Move forward for 2 seconds ");
  rFastForward(2.0);
  eSpeakTalk ("Move forward for 0 seconds");
  rFastForward(0);
  eSpeakTalk("Non blocking test, press enter to stop");
  rFastForward(-1);
  rBeep(1,880);
  rBeep(1,990);
  getchar();
  rStop();

  printf("Test 6 rBackward\n");
  eSpeakTalk("Test 6 R backward");
  /* moves Scribbler backward for a specified speed and time */
  eSpeakTalk ("Move  Backward for 2 seconds ");
  rBackward(0.5,2.0);
  eSpeakTalk ("Move  Backward for 3 seconds ");
  rBackward(0.1,3.0);
  eSpeakTalk ("Move  Backward for 4 seconds");
  rBackward(0.8,4.0);
  eSpeakTalk("Non blocking test, press enter to stop");
  rBackward(1.0,-1);
  rBeep(1,880);
  rBeep(1,990);
  getchar();
  rStop();

  printf("Test 7 rMotor + RStop\n");
  eSpeakTalk("Test 7 R motor and hard stop");
  /* directs robot motion with given speeds for the left and right motors
     continues until given another motion command or disconnected*/
  printf("set both left and right motor to 1\n");
  rMotors(1,1);
  sleep(3);
  printf("stop movement\n");
  /* directs robot to stop movement */
  rHardStop();

  printf ("Test 8 rSetForwardness and rGetForwardness\n");
  eSpeakTalk ("Test 8 rSetForwardness and rGetForwardness\n");
  eSpeakTalk ("Set fluke forward and move forward 1 second\n");
  rSetForwardness ("fluke-forward");
  printf ("done with rSetForwardness\n");
  rBeep (2, 880);
  rForward (1.0, 1.0);

  printf ("Scribbler set to %s\n", rGetForwardness());
  eSpeakTalk ("Scribbler direction set to");
  eSpeakTalk (rGetForwardness());
  
  eSpeakTalk ("Set scribbler forward and move forward 1 second\n");
  rSetForwardness ("scribbler-forward");
  printf ("Scribbler direction set to %s\n", rGetForwardness());
  eSpeakTalk ("Scribbler set to");
  eSpeakTalk (rGetForwardness());
  rForward (1.0, 1.0);
  
  eSpeakTalk ("Reset fluke forward and move forward 1 second\n");
  rSetForwardness ("fluke-forward");
  printf ("done with rSetForwardness\n");
  rForward (1.0, 1.0);
  
  rBeep (2, 880);
  rDisconnect();
  eSpeakDisconnect();

  //done
  return 0;
}