#include"saphira.h"
#include<stdio.h>


const int SUSP = 1;
const int ACTIV = 0;
const int NOTIMEOUT = 0;
const int HIGHPRI = 0;
const int MEDPRI = 1;
const int LOWPRI = 2;
 
char *avoidColl = "avoid";
char *stopColl = "stopcoll";
char *stop = "stop";
char *move = "move";

void ConnectInit()
{
  sfInitControlProcs();
  sfInitInterpretationProcs();
  sfInitRegistrationProcs();
}  // end ConnectInit()

void StartObstacle()
{
  sfStartBehavior(sfAvoidCollision, avoidColl,
		  NOTIMEOUT,
		  HIGHPRI,
		  ACTIV,
		  2,     /* front sensitivity: range .5 to 3 */
		  2,     /* side sensitivity: range .5 to 3 */
		  5,     /* how fast turns away: range 4 to 10 */
		  5000); /* standoff, how far in mm to stay away */
  sfStartBehavior(sfStopCollision, stopColl,
		  NOTIMEOUT,
		  HIGHPRI,
		  ACTIV,
		  2,     /* front sensitivity: range .5 to 3 */
		  2,     /* side sensitivity: range .5 to 3 */
		  5000); /* standoff, how far in mm to stay away */
} // end StartObstacle()


void ResetObstacle()
{
  sfInterruptTask(avoidColl);
  sfInterruptTask(stopColl);
  sfRemoveTask(avoidColl);
  sfRemoveTask(stopColl);
  sfStartBehavior(sfAvoidCollision, avoidColl,
		  NOTIMEOUT,
		  HIGHPRI,
		  ACTIV,
		  1,     /* front sensitivity: range .5 to 3 */
		  1,     /* side sensitivity: range .5 to 3 */
		  10,     /* how fast turns away: range 4 to 10 */
		  10000); /* standoff, how far in mm to stay away */
  sfStartBehavior(sfStopCollision, stopColl,
		  NOTIMEOUT,
		  HIGHPRI,
		  ACTIV,
		  1,     /* front sensitivity: range .5 to 3 */
		  1,     /* side sensitivity: range .5 to 3 */
		  10000); /* standoff, how far in mm to stay away */
} // end ResetObstacle()

		  

void StopObstacle()
{
  sfInterruptTask(avoidColl);
  sfInterruptTask(stopColl);
  sfRemoveTask(avoidColl);
  sfRemoveTask(stopColl);
} // end StopObstacle()



void StartStop()
{
  sfStartBehavior(sfStop, stop,
		  NOTIMEOUT,
		  HIGHPRI,
		  ACTIV);     /* priority value */
} // end StartStop()


void StopStop()
{
  sfInterruptTask(stop);
  sfRemoveTask(stop);
} // end StopStop()


void StartMovement(float startSpeed)
{
  sfStartBehavior(sfConstantVelocity, move,
		  NOTIMEOUT,
		  HIGHPRI,
		  ACTIV,
		  startSpeed);    /* speed of robot */
} // end StartMovement()


void ResetMovement(float newSpeed)
{
  sfInterruptTask(move);
  sfRemoveTask(move);
  sfStartBehavior(sfConstantVelocity, move,
		  NOTIMEOUT,
		  HIGHPRI,
		  ACTIV,
		  newSpeed);    /* speed of robot */
} // end ResetMovement()

		  


void StopMovement()
{
  sfInterruptTask(move);
  sfRemoveTask(move);
} // end StopMovement()


void Shutdown()
{
  sfDisconnectFromRobot();
}


void userControl()
{
  float speed = 200;
  while (speed != 0) {
    printf("Enter new speed for robot: ");
    scanf("%f", &speed);
    ResetMovement(speed);
  }  // end while
}

int main()
{
  sfAddConnectHandler(&ConnectInit, sfFIRST);
  sfStartup(1);  /* start up Saphira server and return control here */

  sfLoadMapFile("olin.map");
  sfMoveRobot(2500,  15240, 0);


  /* Connect to the robot */
  sfConnectToRobot(sfTTYPORT, sfCOM1); 
  
  /* wait until connected to robot */
  while(!sfIsConnected) sfPause(100);

  /* start motors */
  sfRobotComInt(4, 1);

  printf("Startup complete\n");
  StartObstacle();
  printf("Started obstacle avoidance\n");
  StartStop();
  printf("Started stop\n");
  StartMovement(200);
  printf("Started movement\n");
  ResetObstacle();
  printf("Reset obstacle avoidance\n");
  printf("Robot absolute location: %d  %d\n", sfRobot.ax, sfRobot.ay);
  sfPause(30000);
  printf("Robot absolute location: %d  %d\n", sfRobot.ax, sfRobot.ay);

  userControl();
  /*  ResetMovement(50);
  printf("Reset movement\n");
  sfPause(30000);
  printf("Robot absolute location: %d  %d\n", sfRobot.ax, sfRobot.ay); */
  printf("Stopping movement\n");
  StopMovement();
  printf("Stopping obstacle avoidance");
  StopObstacle();
  printf("Shutting down system");
  Shutdown();
} // end main()