/*
  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
  Modified by Henry M. Walker for MyroC.3.0

  Testing for the following general MyroC commands:

  rTurnLeft
  rTurnRight
  TurnSpeed
  rForward
  rFastForward
  rBackward
  rMotors
  rStop
  rHardStop 

 *
 * This program and all MyroC software is licensed under the Creative Commons
 * Attribution-Noncommercial-Share Alike 3.0 United States License.
 * Details may be found at http://creativecommons.org/licenses/by-nc-sa/3.0/us/
 */

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

#ifdef __APPLE__
#include <libc.h>           // needed for sleep command
#else
#include <unistd.h>
#endif

int main(){
  int motion_number = 0;

  //rConnect ("/dev/rfcomm0");
  rConnect ("/dev/tty.Fluke2-0958-Fluke2");
  rBeep (2, 880);
  rSetForwardness("fluke-forward");
  eSpeakConnect();
  eSpeakSetGender("female");
  sleep(1);
  
  printf("Test 1 turn left for given duration\n");
  eSpeakTalk("Test 1 turn left for given duration");
  /* turns Scribbler left for a specified speed and time */

  printf ("starting motion %d - blocking\n", motion_number++);
  rTurnLeft(0.4,1.0);
  rBeep (1, 880);

  printf ("starting motion %d - blocking\n", motion_number++);
  rTurnLeft(0.4,1.0);
  rBeep (1, 880);

  printf ("starting motion %d - blocking\n", motion_number++);
  rTurnLeft(2.0,0.1);
  rBeep (1, 880);

  printf ("starting motion %d - blocking\n", motion_number++);
  rTurnLeft(0.2,1.0);
  sleep(3);
  eSpeakTalk("Timed, non blocking tests");

  printf ("starting motion %d - non-blocking, stop during first beep\n", motion_number++);
  rTurnLeft(1.0,-1);
  rBeep(2,910);
  rBeep(2,950);


  printf ("starting motion %d - non-blocking, stop during second beep\n", motion_number++);
  rTurnLeft(3.0,-1);
  rBeep(2,910);
  rBeep(2,950);


  printf ("starting motion %d - non-blocking, terminated by forward command\n", motion_number++);
  rTurnLeft(3.0,-1);
  rBeep(2,910);

  printf ("starting motion %d - blocking\n", motion_number++);
  rForward (1.0, 1.0);
  rBeep(2,950);

  eSpeakTalk("Untimed, non blocking tests, press enter to stop");

  printf ("starting motion %d\n", motion_number++);
  rTurnLeft(1.0,-1);
  rBeep(1,880);
  rBeep(1,990);
  getchar();
  rStop();
  sleep(3);

  
  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);
  sleep(3);
eSpeakTalk("Non blocking test, press enter to stop");
  rTurnRight(1.0,-1);
  rBeep(1,880);
  rBeep(1,990);
  getchar();
  rStop();
  sleep(4);

  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);
  sleep(1);
  printf("turn left for 1 speed, at 1 second\n");
  rTurnSpeed("left",1,1) ;
  sleep(1);
  printf("turn right for 0.2 speed, at 2 second\n");
  rTurnSpeed("right",0.2,2);
  sleep(1);
  printf("turn left for 0.22 speed, at 2 second\n");
  rTurnSpeed("left",0.2,2);
  sleep(1);

  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);
  sleep(1);
  eSpeakTalk ("Move forward for 3 seconds ");
  rForward(0.1,3.0);
  sleep(1);
  eSpeakTalk ("Move forward for 4 seconds");
  rForward(0.8,4.0);
  sleep(1);
  eSpeakTalk("Non blocking test, press enter to stop");
  rForward(1.0,-1);
  rBeep(1,880);
  rBeep(1,990);
  getchar();
  rStop();
  sleep(2);

  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);
  sleep(1);
  eSpeakTalk ("Move forward for 2 seconds ");
  rFastForward(2.0);
  sleep(2);
  eSpeakTalk ("Move forward for 0 seconds");
  rFastForward(0);
  sleep(1);
  eSpeakTalk("Non blocking test, press enter to stop");
  rFastForward(-1);
  rBeep(1,880);
  rBeep(1,990);
  getchar();
  rStop();
  sleep(4);

  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 ");
  sleep(1);
  rBackward(0.5,2.0);
  eSpeakTalk ("Move  Backward for 3 seconds ");
  sleep(1);
  rBackward(0.1,3.0);
  sleep(1);
  eSpeakTalk ("Move  Backward for 4 seconds");
  rBackward(0.8,4.0);
  sleep(1);
  eSpeakTalk("Non blocking test, press enter to stop");
  rBackward(1.0,-1);
  rBeep(1,880);
  rBeep(1,990);
  getchar();
  rStop();
  sleep(3);

  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(2);
  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);
  sleep(3);

  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);
  sleep(1);
  
  eSpeakTalk ("Reset fluke forward and move forward 1 second\n");
  rSetForwardness ("fluke-forward");
  printf ("done with rSetForwardness\n");
  rForward (1.0, 1.0);
  sleep(1);
  
  rBeep (2, 880);
  sleep(2);
  rDisconnect();
  eSpeakDisconnect();

  //done
  return 0;
}
