/*
 * Final Project -- referee.cc
 */

#define PREDATOR_X_MAX -7
#define PREDATOR_X_MIN -10.5
#define PREDATOR_Y_MAX 5
#define PREDATOR_Y_MIN -2
#define PREY_X_MAX -1
#define PREY_X_MIN -4.5
#define PREY_Y_MAX 5
#define PREY_Y_MIN -2
// Minimum starting separation of X and Y, pioneer2dx size is [0.44,0.33]
#define MIN_PRED_SEPARATION 0.45 

#define CAPTURE_RANGE 2

#define RAND(a,b) rand() * ((double)b-a) / (double)RAND_MAX + (double)a 

#include <libplayerc++/playerc++.h>
#include <iostream>
#include <math.h>
#include <unistd.h>
#include <time.h>
#include <cstdlib>
#include "InputMap.h"
using namespace PlayerCc;
using namespace std;

#include "args.h"


bool clearPath(double x_s, double y_s, double x_e, double y_e);

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

  try
  {
    PlayerClient robot(gHostname, gPort);
    SimulationProxy state(&robot);
	Graphics2dProxy draw(&robot);


    double pred1X, pred1Y, pred2X, pred2Y, preyX, preyY, buf;
	double pred1Heading, pred2Heading;
    double pred1Dist, pred2Dist;
    time_t start, stop;

	inputMap_v2(0, (char*)"autolab.pnm");

    srand(time(NULL));
    cout << "Connected - Press enter to start simulation...\n";
    while (!getchar());
    start = time(NULL); 
    
    state.SetPose2d((char *)string("prey").c_str(), 
	RAND(PREY_X_MIN,PREY_X_MAX), 
	RAND(PREY_Y_MIN,PREY_Y_MAX), 
	RAND(-M_PI, M_PI));
    state.SetPose2d((char *)string("predator1").c_str(), 
	RAND(PREDATOR_X_MIN,PREDATOR_X_MAX), 
	RAND(PREDATOR_Y_MIN,PREDATOR_Y_MAX), 	
	RAND(-M_PI, M_PI));
    state.SetPose2d((char *)string("predator2").c_str(), 
	RAND(PREDATOR_X_MIN,PREDATOR_X_MAX), 
	RAND(PREDATOR_Y_MIN,PREDATOR_Y_MAX), 	
	RAND(-M_PI, M_PI));

    // don't put the predators on top of each other
    state.GetPose2d((char *)string("predator1").c_str(), pred1X, pred1Y,
			pred1Heading);
    state.GetPose2d((char *)string("predator2").c_str(), pred2X, pred2Y,
			pred2Heading);
    state.GetPose2d((char *)string("prey").c_str(), preyX, preyY, buf);


    while(fabs(pred1X-pred2X) < MIN_PRED_SEPARATION 
			&& fabs(pred1Y - pred2Y) < MIN_PRED_SEPARATION) {
		state.SetPose2d((char *)string("predator1").c_str(),
				RAND(PREY_X_MIN, PREY_X_MAX),
				RAND(PREY_Y_MIN, PREY_Y_MAX),
				RAND(-M_PI, M_PI));
		state.SetPose2d((char *)string("predator2").c_str(),
				RAND(PREY_X_MIN, PREY_X_MAX),
				RAND(PREY_Y_MIN, PREY_Y_MAX),
				RAND(-M_PI, M_PI));
	    state.GetPose2d((char *)string("predator1").c_str(), 
				pred1X, pred1Y, pred1Heading);
	    state.GetPose2d((char *)string("predator2").c_str(), 
				pred2X, pred2Y, pred2Heading);
   }


    cout << "Predator 1 starting orientation:\t(" 
		<< pred1X << "," << pred1Y << ")\n";	
    cout << "Predator 2 starting orientation:\t(" 
		<< pred2X << "," << pred2Y << ")\n";	
    cout << "Prey starting orientation:\t\t(" 
		<< preyX << "," << preyY << ")\n";	
    // go into read-think-act loop
    for(;;)
    {
      state.GetPose2d((char *)string("prey").c_str(), 
			  preyX, preyY, pred1Heading);
      state.GetPose2d((char *)string("predator1").c_str(), 
			  pred1X, pred1Y, pred2Heading);
      state.GetPose2d((char *)string("predator2").c_str(), 
			  pred2X, pred2Y, buf);
      
      pred1Dist = sqrt(pow(pred1X-preyX,2)+pow(pred1Y-preyY,2));
      pred2Dist = sqrt(pow(pred2X-preyX,2)+pow(pred2Y-preyY,2));

	  bool pred1Clear = clearPath(pred1X, pred1Y, preyX, preyY);
	  bool pred2Clear = clearPath(pred2X, pred2Y, preyX, preyY);
  
      if ((pred1Dist <= CAPTURE_RANGE) && (pred2Dist <= CAPTURE_RANGE) 
			  && pred1Clear && pred2Clear) {
	     stop = time(NULL);
	     cout << "----Capture!-----\n";    
         cout << "\tPredator 1\t(" << pred1X << "," << pred1Y << ")\n";
         cout << "\tPredator 2\t(" << pred2X << "," << pred2Y << ")\n";
         cout << "\tPrey\t\t(" << preyX << "," << preyY << ")\n";
         cout << "\tPredator 1 Distance\t" << pred1Dist << endl;
         cout << "\tPredator 2 Distance\t" << pred2Dist << endl;
	     cout << "Capture occurred after " << stop - start << " seconds\n";
	     cout << "-----------------\n";    
	     exit(stop-start);
      }
    }
  }
  catch (PlayerCc::PlayerError e)
  {
    std::cerr << e << std::endl;
    return -1;
  }
}

#define xToMapX(cell_size, x) (x/cell_size) + width/(2*SCALE_MAP)
#define yToMapY(cell_size, y) height/(2*SCALE_MAP) - (y/cell_size)

bool isCellOccupied(int x, int y) {
	return gridMap[y][x] == 1.0;
}

bool clearPath(double x_s, double y_s, double x_g, double y_g) {

	double x_cell_size = 24.0/width * SCALE_MAP;
	double y_cell_size = 24.0/height * SCALE_MAP;

	//convert to map coordinates.
	int map_x_s = xToMapX(x_cell_size, x_s);
	int map_y_s = yToMapY(y_cell_size, y_s);
	int map_x_g = xToMapX(x_cell_size, x_g);
	int map_y_g = yToMapY(y_cell_size, y_g);

	bool steep = abs(map_y_g - map_y_s) > abs(map_x_g - map_x_s);

	int temp = 0;


	if(steep) {
		temp = map_x_g;
		map_x_g = map_y_g;
		map_y_g = temp;

		temp = map_x_s;
		map_x_s = map_y_s;
		map_y_s = temp;
	}

	if( map_x_s > map_x_g ) {
		temp = map_x_s;
		map_x_s = map_x_g;
		map_x_g = temp;

		temp = map_y_s;
		map_y_s = map_y_g;
		map_y_g = temp;
	}

	int deltax = map_x_g - map_x_s;
	int deltay = abs(map_y_g - map_y_s);
	int error = deltax/2;
	int ystep = (map_y_s < map_y_g) ? 1 : -1;
	int y = map_y_s;

	for(int x = map_x_s; x < map_x_g; x++) {
		if(steep) {
			if(isCellOccupied(y,x)) {
				return false;
			}
		}
		else if(isCellOccupied(x,y)) {
			return false;
		}

		error -= deltay;
		if(error < 0) {
			y += ystep;
			error += deltax;
		}
	}
	return true;
}