#include <libplayerc++/playerc++.h>
#include <iostream>
#include <iomanip>
#include <stdlib.h>
#include <string.h>
#include <fstream>
#include <math.h>
#include <time.h>
#include "vec.h"
#include "behaviors.h"

#include "args.h"



using namespace std;

char host[256] = "localhost";
int port = PLAYER_PORTNUM;
int device_index = 0;


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

  parse_args(argc,argv);
  using namespace PlayerCc;

  extern char *optarg;
  char *pEnd;
  char filename[50] = "weights.txt";

  bool fromFile = 0;
  int c, i;


  ifstream fin;
  int behWeights[NUM_BEHAVIORS];


  PlayerClient robot(host, port);
  Position2dProxy pp(&robot, gIndex);
  LaserProxy lp(&robot, gIndex);
  SonarProxy sp(&robot, gIndex);


  vec totalVector(0,0);
  vec randVec(0,0),tmpVec;
  double newTheta,newTurnRate,newSpeed,random;
  int favoredSide;

  double elapsed;
  time_t start;
  time_t end;


  // Initialize all behavior weights with zero
  for (i=0; i < NUM_BEHAVIORS; i++)
    behWeights[i] = 0;

//   double travWeight = 3.0;
//   double fidWeight = 200.0;
//   double lasWeight = 1.0;
//   double sonWeight = 60.0;
//   double randWeight = 300.0;
//   double wallWeight = 1.0;

//   while((c = getopt(argc,argv,"i")) != EOF)
//     {
//       switch(c)
// 	{
// 	case 'i':
// 	    {
// 	      cout <<"here 4"<<endl;

// 	      strcpy(filename,optarg);
// 	      cout <<"here 5"<<endl;

// 	      fromFile = 1;
// 	    }
// 	default:
// 	  break;
// 	}
//     }


  
  time(&start);
  srand(time(NULL));
  favoredSide = rand()%2;
  


  double sonarAngles[8] = {-130,-150,-170,170,150,130};

  int behId;
  fin.open(filename);
  while(!fin.eof())
    {
      fin>>behId;
      fin>>behWeights[behId];
    }
  fin.close();
  
  while(1)
    {
      robot.Read();

      totalVector.mag = totalVector.theta = 0;
      
      // Laser avoid behavior
      if (behWeights[LASER_AVOID_ID])
	{
	  tmpVec = laserAvoid(&lp,behWeights[LASER_AVOID_ID]);
	  tmpVec.mag = tmpVec.mag * behWeights[LASER_AVOID_ID];
	  totalVector = totalVector + tmpVec ;
	}
  
      // Rear sonar avoid behavior
      if (behWeights[SONAR_AVOID_ID])
	{
	  tmpVec = sonarAvoid(&sp,sonarAngles,behWeights[SONAR_AVOID_ID]);
	  tmpVec.mag = tmpVec.mag * behWeights[SONAR_AVOID_ID];
	  totalVector = totalVector + tmpVec ;
	}


//       time(&end);
//       elapsed = difftime(end,start);
//       if(elapsed >= 3.0)
// 	{	      
// 	  time(&start);
// 	  favoredSide = rand()%2;
// 	  //Which side will the robot favor?  Or rather, which
// 	  //way will the robot turn if faced with a binary choice?
// 	  //favoredSide is the variable that determines that:
// 	  //0 = right, 1 = left.
// 	  randVec = randomVector(randWeight,favoredSide);
	  
// 	}

      // Random vector behavior
      if (behWeights[RANDOM_VECTOR_ID])
	{
	  tmpVec = randomVector(behWeights[RANDOM_VECTOR_ID], 1);
	  tmpVec.mag = tmpVec.mag * behWeights[RANDOM_VECTOR_ID];
	  totalVector = totalVector + tmpVec ;
	}


      // Fiducial attract behavior
      if (behWeights[WANDER_ID])
	{
	  tmpVec = wander(&lp,behWeights[WANDER_ID]);
	  tmpVec.mag = tmpVec.mag * behWeights[WANDER_ID];
	  totalVector = totalVector + tmpVec ;
	}


      
      //The following code interprets the sum vector (totalVector)
      //as rotational and lateral velocity commands.
      if(totalVector.mag > 0)
	{
	  if(totalVector.theta >=-45 && totalVector.theta <= 45)
	    {
	      newSpeed = .5;
	    }
	  else if(totalVector.theta > 45 && totalVector.theta <= 135 || 
		  totalVector.theta < -45 && totalVector.theta > -135)
	    {
	      newSpeed = 0;
	    }
	  else if(totalVector.theta >135 || totalVector.theta < -135)
	    {
	      newSpeed = -.2;
	      totalVector.reverse();
	    }
	}
      else
	newSpeed = 0;

      //Restrict the turnrate such that it falls between the max & min
      if(totalVector.mag > 0)
	{
	  newTurnRate = totalVector.theta;
	  
	  if(newTurnRate > 40)
	    newTurnRate = 40;
	  else if(newTurnRate < -40)
	    newTurnRate = -40;
	}
      else
	newTurnRate = 0;
      
      //set the speed and output some debug info (totalVector)
      pp.SetSpeed(newSpeed,DTOR(newTurnRate));
      cout<<setprecision(5)<<totalVector.mag<<'\t'<<totalVector.theta<<endl;

    }
  return 0;
}