#ifndef SunOS
#include <signal.h>
#else
#include <siginfo.h>
#include <sys/signal.h>
#endif

#include <pthread.h>
#include <stdio.h>
#include <unistd.h>
#include <iostream.h>

#include "RobotControl.hh"
#include "SerialChannel.hh"
#include "Behavior.hh"
#include "BehaviorManager.hh"

// Behaviors to add...
#include "Explore.hh"		// Random wander
#include "AvoidObject.hh"	// Avoid walls
#include "BumpSensor.hh"	// Retract from contact
#include "GraspReflex.hh"	// Automatically grasp items
#include "BatterySaver.hh"	// Freeze if battery is low

#ifdef VISION
#include "VisionBehavior.hh"	// Compute visual image recognition
#include "V4L.hh"
#endif

//#include "Speak.hh"

RobotControl* robot;
Channel<unsigned char*>* comm;

#ifdef VISION
V4L* camera;
#endif

void quit( int sig );

int main (int argc, char** argv)
{

#ifdef SPEECH
  // initialize the system
  int heap_size = 210000;  // default scheme heap size
  int load_init_files = 1; // we want the festival init files loaded
  festival_initialize( load_init_files, heap_size );
  festival_eval_command("(voice_ked_diphone)");
#endif

  //  SAY( "Welcome to Harvey Mudd College Robotics" );

  pthread_t sipReader, behManager;
  pthread_t avoidObj, explore, batterySaver, bumpsensor, grasp;

#ifdef VISION
  pthread_t vision;
#endif

  signal( SIGSEGV, quit );
  signal( SIGINT, quit );

  cout << "Pioneer starting up..." << endl;

  // Initialize communications channel to the robot
  comm = new SerialChannel<unsigned char*>(1,9600,"PioneerComm");

  // Instantiate robot control class
  robot = new RobotControl( comm, false );

  // Initialize Robot Control class
  if ( !robot->Initialize() )
    {
      // SAY( "Robot failed to initialize. Please check the connection");
      cout << "Robot failed to start" << endl;
      return 0;
    }

  // Set the origin
  robot->SetOrigin();

  cout << "Finished initializing robot" << endl;

  // Start the regular update of robot state from the robot.
  pthread_create ( &sipReader, NULL,
		   RobotControl::UpdateState, (void*)robot);

  // Start up the behavior manager
  BehaviorManager manager( 2000 );
  pthread_create( &behManager, NULL,
		  BehaviorManager::startServer, (void*)&manager);

  usleep(1000);

  //  SAY( "Starting robot in three seconds" );
  int sleep_time = 3;
  cout << "Starting up robot in " << sleep_time << "...";
  flush( cout );
  while ( sleep_time > 0 )
    {
      sleep(1);
      sleep_time--;
      cout << " " << sleep_time << "...";
      flush(cout);
    }
  cout << endl;

  robot->TranslationalVelocity( 200 );

#ifdef VISION
  camera = new V4L( "Robot Camera",
		    "/dev/video0",
		    0,
		    VIDEO_MODE_NTSC,
		    VIDEO_PALETTE_RGB24,
		    320,
		    240,
		    24 );

  // Create the avoid object behavior
  VisionBehavior visionBeh("Vision Behavior",
			   11,
			   robot,
			   camera );
  pthread_create( &vision, NULL,
		  Behavior::Entry, (void*)&visionBeh );
#endif

  // Create the avoid object behavior
  AvoidObject avoidObjBeh("Avoid Object",
			  10,
			  robot );
  pthread_create( &avoidObj, NULL,
		  Behavior::Entry, (void*)&avoidObjBeh );

  // Create the grasp reflex behavior
  GraspReflex graspBeh( "Grasp Reflex",
			5,
			robot);
  pthread_create( &grasp, NULL,
		  Behavior::Entry, (void*)&graspBeh );

  sleep( 100000 );
  // Create the battery saver behavior
  BatterySaver batterySaverBeh( "Battery Saver",
				1,
				robot,
				12.0);
  pthread_create( &batterySaver, NULL,
		  Behavior::Entry, (void*)&batterySaverBeh );


  // Create the bump sensor behavior
  BumpSensor bumpsensorBeh( "Bump Sensor",
			    3,
			    robot);
  pthread_create( &bumpsensor, NULL,
		  Behavior::Entry, (void*)&bumpsensorBeh );

  /*
  // Create the explore behavior
  Explore exploreBeh( "Explore",
		      15,
		      robot);
  pthread_create(&explore, NULL,
		 Behavior::Entry, (void*)&exploreBeh );
		 */

  pthread_join(behManager,   NULL);
  pthread_join(bumpsensor,   NULL);
  pthread_join(avoidObj,     NULL);
  pthread_join(explore,      NULL);
  pthread_join(batterySaver, NULL);

#ifdef VISION
  pthread_join(vision,       NULL);
#endif

  pthread_join(sipReader,    NULL);

  robot->Close();

  delete robot;
  delete comm;

  return 0;
}

void quit( int sig )
{
#ifdef LINUX
  signal( SIGINT, SIG_IGN );
#endif
#ifdef FreeBSD
  signal( SIGINT, SIG_IGN );
#endif
  psignal( sig, "User Quit" );

  robot->Close();

  //  SAY( "Goodbye" );
  delete robot;
  //  delete comm;

  exit(1);
}