#include <math.h>
#include <GL/glut.h>
#include <unistd.h>
#include <stdlib.h>
#include <iostream>
#include <FTGL/ftgl.h>
#include <signal.h>

#include <vector>
#include <Eigen/Eigen>

#include "include/transformation3.hh"
#include "include/transformation2.hh"
#include "include/qc_odometry_messages.h"

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sensor_msgs/Imu.h>
#include <px_comm/OpticalFlow.h>

#define NAV_USING_GROUND_TILE          33
#define NAV_USING_SLAM                 34
#define NAV_USING_OPTICAL_FLOW         35

using namespace std;

struct timeval tvstart;
struct timeval tvend;

double roll, yaw, pitch;

double gametime=0;
float Robot_direction[10], Robot_position_x[10], Robot_position_y[10];
float vehicle_position_x=0.0,vehicle_position_y=0.0,vehicle_position_z=0.0,vehicle_direction=0.0,vehicle_wingangle=0.0;
float position_record_x[20000], position_record_y[20000], position_record_z[20000];
int position_record_state[20000];
float obstacle_position_x[4],obstacle_position_y[4],obstacle_direction[4];
float angle=-1.5775,deltaAngle = 0.0,ratio;
float convertMat[3][3];
float x=11.0f,y=0.5f,z=0.0f;
float lx_FrontBack=-1.0f,ly=-70.0f,lx=70.0f,lz=70.0f,lz_FrontBack=0.0f,lx_LeftRight=0.0f,lz_LeftRight=-1.0f,lx_UpDown=0.0f,ly_UpDown=0.0f,lz_UpDown=-1.0f;
GLint snowman_display_list;
int deltaMove_FrontBack = 0, deltaMove_LeftRight = 0, deltaMove_UpDown=0;
bool overlook=false,fu=false,yang=false,Robot_move=false,vehicle_move=false,vehicle_land=false,vehicle_collision=false,track_clear=false;
int window_width=640, window_height=360;
int move_record=0;

px_comm::OpticalFlow local_optical_data;

void opticalCallback(const px_comm::OpticalFlow& rc_optical_data)
{
  local_optical_data.ground_distance = rc_optical_data.ground_distance;
  local_optical_data.velocity_x = rc_optical_data.velocity_x;
  local_optical_data.velocity_y = rc_optical_data.velocity_y;
  local_optical_data.quality    = rc_optical_data.quality;
  local_optical_data.position_x = rc_optical_data.position_x;
  local_optical_data.position_y = rc_optical_data.position_y;
  local_optical_data.time_step  = rc_optical_data.time_step;

  local_optical_data.state_style = rc_optical_data.state_style;

  vehicle_position_x = 10.0f - local_optical_data.position_x;
  vehicle_position_y = local_optical_data.ground_distance;
  vehicle_position_z = -local_optical_data.position_y;

  if(move_record % 2 == 0){
    position_record_x[move_record/2] = vehicle_position_x;
    position_record_y[move_record/2] = vehicle_position_y;
    position_record_z[move_record/2] = vehicle_position_z;
    position_record_state[move_record/2] = local_optical_data.state_style;
  }
  if(move_record++/2 >= 20000)
    move_record = 0;

  local_optical_data.pitch = rc_optical_data.pitch;
  local_optical_data.yaw = rc_optical_data.yaw;
  local_optical_data.roll  = rc_optical_data.roll;
  pitch = local_optical_data.pitch;
  yaw = -local_optical_data.yaw;
  roll = -local_optical_data.roll;

	convertMat[0][0]=cos(pitch)*cos(yaw);
	convertMat[1][0]=cos(pitch)*sin(yaw);
	convertMat[2][0]=-sin(pitch);
	convertMat[0][1]=sin(pitch)*cos(yaw)*sin(roll)-sin(yaw)*cos(roll);
	convertMat[1][1]=sin(pitch)*sin(yaw)*sin(roll)+cos(yaw)*cos(roll);
	convertMat[2][1]=cos(pitch)*sin(roll);
	convertMat[0][2]=sin(pitch)*cos(yaw)*cos(roll)+sin(yaw)*sin(roll);
	convertMat[1][2]=sin(pitch)*sin(yaw)*cos(roll)-cos(yaw)*sin(roll);
	convertMat[2][2]=cos(pitch)*cos(roll);

}

const float R=0.17;

void change_direction(int id){
	for(int i=0;i<10;i++)
		Robot_direction[i]=Robot_direction[i]>0? (Robot_direction[i]-3.141): (Robot_direction[i]+3.141);
	glutTimerFunc(20000,change_direction,0);
}

void error_direction(int id){
	srand((unsigned)time(0));
	for(int i=0;i<10;i++)
		Robot_direction[i]+=(rand()/(double)(RAND_MAX/(2*3.141/9.0))-3.141/9.0);
	glutTimerFunc(5000,error_direction,0);
}

void changeSize(int w, int h){
	window_width=w;
	window_height=h;
	// Prevent a divide by zero, when window is too short
	// (you cant make a window of zero width).
	if(h == 0)
		h = 1;
	ratio = 1.0f * w / h;
	// Reset the coordinate system before modifying
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	// Set the viewport to be the entire window
	glViewport(0, 0, w, h);
	// Set the clipping volume
	gluPerspective(45,ratio,1,1000);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	gluLookAt(x - 1*lx_FrontBack*sin(0.01*lx), y - 1*sin(0.01*ly), z - 1*lz_FrontBack*sin(0.01*lz),
		      x,y,z,
			  0.0f,1.0f,0.0f);
}

GLuint createDL() {
	GLuint snowManDL;
	// Create the id for the list
	snowManDL = glGenLists(1);
	// start list
	glNewList(snowManDL,GL_COMPILE);
	// call the function that contains the rendering commands
	// endList
	glEndList();
	return(snowManDL);
}

void initScene() {
	glEnable(GL_DEPTH_TEST);
	snowman_display_list = createDL();
}

void RenderBone(float x0, float y0, float z0, float x1, float y1, float z1 ,GLdouble radius)
{
	GLdouble  dir_x = x1 - x0;
	GLdouble  dir_y = y1 - y0;
	GLdouble  dir_z = z1 - z0;
	GLdouble  bone_length = sqrt( dir_x*dir_x + dir_y*dir_y + dir_z*dir_z );
	static GLUquadricObj *  quad_obj = NULL;
	if ( quad_obj == NULL )
		quad_obj = gluNewQuadric();
	gluQuadricDrawStyle( quad_obj, GLU_FILL );
	gluQuadricNormals( quad_obj, GLU_SMOOTH );
	glPushMatrix();
	// ÆœÒÆµœÆðÊŒµã
	glTranslated( x0, y0, z0 );
	// ŒÆËã³€¶È
	double  length;
	length = sqrt( dir_x*dir_x + dir_y*dir_y + dir_z*dir_z );
	if ( length < 0.0001 ) {
		dir_x = 0.0; dir_y = 0.0; dir_z = 1.0;  length = 1.0;
	}
	dir_x /= length;  dir_y /= length;  dir_z /= length;
	GLdouble  up_x, up_y, up_z;
	up_x = 0.0;
	up_y = 1.0;
	up_z = 0.0;
	double  side_x, side_y, side_z;
	side_x = up_y * dir_z - up_z * dir_y;
	side_y = up_z * dir_x - up_x * dir_z;
	side_z = up_x * dir_y - up_y * dir_x;
	length = sqrt( side_x*side_x + side_y*side_y + side_z*side_z );
	if ( length < 0.0001 ) {
		side_x = 1.0; side_y = 0.0; side_z = 0.0;  length = 1.0;
	}
	side_x /= length;  side_y /= length;  side_z /= length;
	up_x = dir_y * side_z - dir_z * side_y;
	up_y = dir_z * side_x - dir_x * side_z;
	up_z = dir_x * side_y - dir_y * side_x;
	// ŒÆËã±ä»»ŸØÕó
	GLdouble  m[16] = { side_x, side_y, side_z, 0.0,
		up_x,   up_y,   up_z,   0.0,
		dir_x,  dir_y,  dir_z,  0.0,
		0.0,    0.0,    0.0,    1.0 };
	glMultMatrixd( m );
	// Ô²ÖùÌå²ÎÊý
	//GLdouble radius= 2.0;		// °ëŸ¶
	GLdouble slices = 30.0;		//	¶ÎÊý
	GLdouble stack = 6.0;		// µÝ¹éŽÎÊý
	gluCylinder( quad_obj, radius, radius, bone_length, slices, stack );
	glPopMatrix();
}

void orientMe(float ang) {
	lx_FrontBack = sin(ang);
	lz_FrontBack = -cos(ang);
	lx_LeftRight = -cos(ang);
	lz_LeftRight = -sin(ang);
	glLoadIdentity();
	gluLookAt(x - 1*lx_FrontBack*sin(0.01*lx), y - 1*sin(0.01*ly), z - 1*lz_FrontBack*sin(0.01*lz),
		      x,y,z,
			  0.0f,1.0f,0.0f);
}

void moveMeFlat_LeftRight(int i) {
	lx_LeftRight = -cos(angle);
	lz_LeftRight = -sin(angle);
	x = x + i*(lx_LeftRight)*0.03;
	z = z + i*(lz_LeftRight)*0.03;
	glLoadIdentity();
	gluLookAt(x - 1*lx_FrontBack*sin(0.01*lx), y - 1*sin(0.01*ly), z - 1*lz_FrontBack*sin(0.01*lz),
		      x,y,z,
			  0.0f,1.0f,0.0f);
}

void moveMeFlat_FrontBack(int i) {
	lx_FrontBack = sin(angle);
	lz_FrontBack = -cos(angle);
	x = x + i*(lx_FrontBack)*0.03;
	z = z + i*(lz_FrontBack)*0.03;
	glLoadIdentity();
	gluLookAt(x - 1*lx_FrontBack*sin(0.01*lx), y - 1*sin(0.01*ly), z - 1*lz_FrontBack*sin(0.01*lz),
		      x,y,z,
			  0.0f,1.0f,0.0f);
}

void moveMeFlat_UpDown(int i) {
	lx_FrontBack = sin(angle);
	lz_FrontBack = -cos(angle);
	lx_LeftRight = -cos(angle);
	lz_LeftRight = -sin(angle);
	ly_UpDown = 1.0f;
	if(i>0)// && y<3.0f)
		y = y + i*(ly_UpDown)*0.03;
	if(i<0)// && y>0.1f)
		y = y + i*(ly_UpDown)*0.03;
	if(y>0.2)
		vehicle_land=false;
	glLoadIdentity();
	gluLookAt(x - 1*lx_FrontBack*sin(0.01*lx), y - 1*sin(0.01*ly), z - 1*lz_FrontBack*sin(0.01*lz),
		      x,y,z,
			  0.0f,1.0f,0.0f);
}

void moveMeFlat_overlook(void) {
	glLoadIdentity();
	gluLookAt(0.0f, 28.0f, 0.0f,
		      0.0f ,28.0f -0.1f,0.0f ,
			  0.0f,0.0f,-1.0f);
}

void moveMeFlat_fuyang(void) {

if(lx>1)
	if(fu){
		glLoadIdentity();
		gluLookAt(x - 1*lx_FrontBack*sin(0.01*lx--), y - 1*sin(0.01*ly--), z - 1*lz_FrontBack*sin(0.01*lz--),
		  	     x,y,z,
			  	0.0f,cos(0.01*ly),0.0f);
	}
if(lx<290)
	if(yang){
		glLoadIdentity();
		gluLookAt(x - 1*lx_FrontBack*sin(0.01*lx++), y - 1*sin(0.01*ly++), z - 1*lz_FrontBack*sin(0.01*lz++),
		  	     x,y,z,
			  	0.0f,cos(0.01*ly),0.0f);
	}
}

void clear_track(){
	if(track_clear == true){
		for(int i=0;i<20000;i++){
			position_record_x[i] = 0;
			position_record_y[i] = 0;
			position_record_z[i] = 0;
		}
	}
}

Eigen::Vector3d vehicle2Global(float x,float y,float z)
{
	Eigen::Vector3d vector;
	vector(0)=convertMat[0][0]*x+convertMat[0][1]*y+convertMat[0][2]*z;
	vector(1)=convertMat[1][0]*x+convertMat[1][1]*y+convertMat[1][2]*z;
	vector(2)=convertMat[2][0]*x+convertMat[2][1]*y+convertMat[2][2]*z;
	return vector;
}

void renderScene(void) {
//Calculate time
	gettimeofday(&tvend,NULL);
	double avgtime = tvend.tv_sec - tvstart.tv_sec + 1e-6 * (tvend.tv_usec - tvstart.tv_usec);
	if(Robot_move==true)
		gametime+=avgtime;
	gettimeofday(&tvstart,NULL);

	if(fu || yang)
		moveMeFlat_fuyang();
	if(overlook)
		moveMeFlat_overlook();
	if (deltaMove_UpDown)
		moveMeFlat_UpDown(deltaMove_UpDown);
	if (deltaMove_FrontBack)
		moveMeFlat_FrontBack(deltaMove_FrontBack);
	if (deltaMove_LeftRight)
		moveMeFlat_LeftRight(deltaMove_LeftRight);
	if (deltaAngle) {
		angle += deltaAngle;
		orientMe(angle);
	}
	if(track_clear)
		clear_track();

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Draw ground
	glColor3f(0.2f, 0.4f, 0.6f);
	glLineWidth(1);
	glBegin(GL_LINES);
		for(int i = 26; i >-40 ; i--)
			for(int j=-30; j < 30; j++) {
				glVertex3f(j, 0.0f, i);
				glVertex3f(j+1.0f, 0.0f, i);
				glVertex3f(j, 0.0f, i);
				glVertex3f(j, 0.0f, i-1.0f);
			}
	glEnd();
//Line Red Line
	glColor3f(0.9f, 0.1f, 0.1f);
	glLineWidth(4);
	glBegin(GL_LINES);
		glVertex3f(-10.0f, 0.0f, 10.0f);
		glVertex3f(10.0f, 0.0f, 10.0f);
	glEnd();
//Line Green Line
	glColor3f(0.1f, 0.9f, 0.1f);
	glLineWidth(4);
	glBegin(GL_LINES);
		glVertex3f(-10.0f, 0.0f, -10.0f);
		glVertex3f(10.0f, 0.0f, -10.0f);
	glEnd();
//Line White Line
	glColor3f(0.9f, 0.9f, 0.9f);
	glLineWidth(4);
	glBegin(GL_LINES);
		glVertex3f(-10.0f, 0.0f, 10.0f);
		glVertex3f(10.0f, 0.0f, 10.0f);
		glVertex3f(-10.0f, 0.0f, -10.0f);
		glVertex3f(-10.0f, 0.0f, 10.0f);
		glVertex3f(10.0f, 0.0f, -10.0f);
		glVertex3f(10.0f, 0.0f, 10.0f);
		glVertex3f(10.0f, 0.0f, 0.0f);
		glVertex3f(-10.0f, 0.0f, 0.0f);
	glEnd();
//Start Circle
	RenderBone(0,0,0,0,0.05,0,1.5);
//iRobot
/*	glColor3f(0.9f, 0.9f, 0.0f);
	glLineWidth(2);
	for(int i=0; i<10; i++){
		RenderBone(Robot_position_x[i],0.0f,Robot_position_y[i] \
			,Robot_position_x[i],0.08f,Robot_position_y[i],R);
	}
	glColor3f(1.0f, 0.5f, 0.0f);
	glLineWidth(4);
	for(int i=0; i<10; i++){
		glBegin(GL_LINES);
			glVertex3f(Robot_position_x[i]+R*sin(Robot_direction[i]), 0.1f, Robot_position_y[i]+R*cos(Robot_direction[i]));
			glVertex3f(Robot_position_x[i], 0.1f, Robot_position_y[i]);
		glEnd();
	}
//Obstacle Robot
	glColor3f(0.8f, 0.2f, 0.2f);
	for(int i=0; i<4; i++){
		RenderBone(obstacle_position_x[i],0.0f,obstacle_position_y[i] \
			,obstacle_position_x[i],2.0f,obstacle_position_y[i],R);
	}
	glLineWidth(4);
	for(int i=0; i<4; i++){
		glBegin(GL_LINES);
			glVertex3f(obstacle_position_x[i]+R*sin(obstacle_direction[i]), 0.1f, obstacle_position_y[i]+R*cos(obstacle_direction[i]));
			glVertex3f(obstacle_position_x[i], 0.1f, obstacle_position_y[i]);
		glEnd();
	}*/
//Move Robot
	if(Robot_move==true){
		for(int i=0;i<10;i++){//³öÁË±ßœçŸÍ²»×ßÁË
			if(Robot_position_x[i]<=10.0f && Robot_position_x[i]>=-10.0f && Robot_position_y[i]<=10.0f && Robot_position_y[i]>=-10.0f){
				Robot_position_x[i]+=0.005f*sin(Robot_direction[i]);
				Robot_position_y[i]+=0.005f*cos(Robot_direction[i]);
			}
		}//Move obstacle
		for(int i=0;i<4;i++){
			obstacle_position_x[i]+=0.005f*sin(obstacle_direction[i]);
			obstacle_position_y[i]+=0.005f*cos(obstacle_direction[i]);
			obstacle_direction[i]+=0.001;
		}//Collision of iRobot
		for(int i=0;i<10;i++)
			for(int j=i+1;j<10;j++)
				if(sqrt(pow(Robot_position_x[i]-Robot_position_x[j],2)+pow(Robot_position_y[i]-Robot_position_y[j],2))<0.33){
						//jÔÚiµÄÇ°·œ£¬i×ª
						if(pow(Robot_position_x[i]+R*sin(Robot_direction[i])-Robot_position_x[j],2)+pow(Robot_position_y[i]+R*cos(Robot_direction[i])-Robot_position_y[j],2) < pow(Robot_position_x[i]-R*sin(Robot_direction[i])-Robot_position_x[j],2)+pow(Robot_position_y[i]-R*cos(Robot_direction[i])-Robot_position_y[j],2))
							Robot_direction[i]=Robot_direction[i]>0? (Robot_direction[i]-3.141):(Robot_direction[i]+3.141);
						//iÔÚjµÄÇ°·œ£¬j×ª
						if(pow(Robot_position_x[j]+R*sin(Robot_direction[j])-Robot_position_x[i],2)+pow(Robot_position_y[j]+R*cos(Robot_direction[j])-Robot_position_y[i],2) < pow(Robot_position_x[j]-R*sin(Robot_direction[j])-Robot_position_x[i],2)+pow(Robot_position_y[j]-R*cos(Robot_direction[j])-Robot_position_y[i],2))
							Robot_direction[j]=Robot_direction[j]>0? (Robot_direction[j]-3.141):(Robot_direction[j]+3.141);
				}
		for(int i=0;i<10;i++)
			for(int j=0;j<4;j++){
				if(sqrt(pow(Robot_position_x[i]-obstacle_position_x[j],2)+pow(Robot_position_y[i]-obstacle_position_y[j],2))<0.33){
					if(pow(Robot_position_x[i]+R*sin(Robot_direction[i])-obstacle_position_x[j],2)+pow(Robot_position_y[i]+R*cos(Robot_direction[i])-obstacle_position_y[j],2) < pow(Robot_position_x[i]-R*sin(Robot_direction[i])-obstacle_position_x[j],2)+pow(Robot_position_y[i]-R*cos(Robot_direction[i])-obstacle_position_y[j],2))
						Robot_direction[i]=Robot_direction[i]>0? (Robot_direction[i]-3.141):(Robot_direction[i]+3.141);
				}
		}
	}
//Vehicle Body
	glColor3f(0.8f, 0.95f, 0.8f);
	for(int i=0;i<4;i++)
	{
		Eigen::Vector3d v=vehicle2Global(0.4243*cos(0.7854+i*1.5708),0.4243*sin(0.7854+i*1.5708),-0.1);
		Eigen::Vector3d vv=vehicle2Global(0.4243*cos(0.7854+i*1.5708),0.4243*sin(0.7854+i*1.5708),-0.11);
		RenderBone(vehicle_position_x-v(0), vehicle_position_y-v(2), (vehicle_position_z+v(1)),  vehicle_position_x-vv(0), vehicle_position_y-vv(2), (vehicle_position_z+vv(1)), R);
	}
	glColor3f(0.1f, 0.95f, 0.1f);
	glLineWidth(2);
	glBegin(GL_LINE_LOOP);
		for(int i=0;i<4;i++)
		{
			Eigen::Vector3d v=vehicle2Global(0.8627*cos(0.7854+i*1.5708),0.8627*sin(0.7854+i*1.5708),-0.13);
			Eigen::Vector3d vv=vehicle2Global(0.8627*cos(0.7854+(i+1)*1.5708),0.8627*sin(0.7854+(i+1)*1.5708),-0.13);

			glVertex3f(vehicle_position_x-v(0), vehicle_position_y-v(2), (vehicle_position_z+v(1)));
			glVertex3f(vehicle_position_x-vv(0), vehicle_position_y-vv(2), (vehicle_position_z+vv(1)));
		}
	glEnd();
	//Wings
	glColor3f(0.8f, 0.95f, 0.6f);
	glBegin(GL_LINES);
		for(int i=0;i<4;i++)
		{
			Eigen::Vector3d v=vehicle2Global(0.4243*cos(0.7854+i*1.5708)+R*cos(6.2832/120*vehicle_wingangle++),0.4243*sin(0.7854+i*1.5708)+R*sin(6.2832/120*vehicle_wingangle++),-0.13);
			Eigen::Vector3d vv=vehicle2Global(0.4243*cos(0.7854+i*1.5708)-R*cos(6.2832/120*vehicle_wingangle++),0.4243*sin(0.7854+i*1.5708)-R*sin(6.2832/120*vehicle_wingangle++),-0.13);
			glVertex3f(vehicle_position_x-v(0), vehicle_position_y-v(2),(vehicle_position_z+v(1)));
                        glVertex3f(vehicle_position_x-vv(0), vehicle_position_y-vv(2),(vehicle_position_z+vv(1)));
		}
	glEnd();
	//Body
	glColor3f(0.8f, 0.6f, 0.8f);
		for(int i=0;i<4;i++)
		{
			Eigen::Vector3d v=vehicle2Global(0.2121*cos(0.7854+i*1.5708),0.2121*sin(0.7854+i*1.5708),0);
			Eigen::Vector3d vv=vehicle2Global(0.2121*cos(0.7854+i*1.5708),0.2121*sin(0.7854+i*1.5708),-0.25);
			RenderBone(vehicle_position_x-v(0), vehicle_position_y-v(2), (vehicle_position_z+v(1)), vehicle_position_x-vv(0), vehicle_position_y-vv(2), (vehicle_position_z+vv(1)),0.01);
		}
	glColor3f(0.5f, 0.0f, 0.2f);
	glBegin(GL_POLYGON);
	      for(int i=0;i<4;i++)
	      {
	      	Eigen::Vector3d v=vehicle2Global(0.2121*cos(0.7854+i*1.5708),0.2121*sin(0.7854+i*1.5708),-0.25);
	      	glVertex3f( vehicle_position_x-v(0), vehicle_position_y-v(2), (vehicle_position_z+v(1)));
	      }
	glEnd();
	//Leg
	glColor3f(0.8f, 0.95f, 0.6f);
	glBegin(GL_LINES);
		for(int i=0;i<4;i++)
		{
			Eigen::Vector3d v=vehicle2Global(0.2121*cos(0.7854+i*1.5708),0.2121*sin(0.7854+i*1.5708),0);
			Eigen::Vector3d vv=vehicle2Global(0.2621*cos(0.7854+i*1.5708),0.2621*sin(0.7854+i*1.5708),0);
			glVertex3f(vehicle_position_x-v(0), vehicle_position_y-v(2), (vehicle_position_z+v(1)));
			glVertex3f(vehicle_position_x-vv(0), vehicle_position_y-vv(2), (vehicle_position_z+vv(1)));
		}
	glEnd();
	//Leg
	glColor3f(0.8f, 0.95f, 0.6f);
	glBegin(GL_LINES);
		for(int i=0;i<4;i++)
		{
			Eigen::Vector3d v=vehicle2Global(0.4243*cos(0.7854+i*1.5708),0.4243*sin(0.7854+i*1.5708),-0.13);
			Eigen::Vector3d vv=vehicle2Global(0.8627*cos(0.7854+i*1.5708),0.8627*sin(0.7854+i*1.5708),-0.13);

			glVertex3f(vehicle_position_x-v(0), vehicle_position_y-v(2), (vehicle_position_z+v(1)));
			glVertex3f(vehicle_position_x-vv(0), vehicle_position_y-vv(2), (vehicle_position_z+vv(1)));

			vv=vehicle2Global(0.67978*cos(0.45707+i*1.5708),0.67978*sin(0.45707+i*1.5708),-0.13);

			glVertex3f(vehicle_position_x-v(0), vehicle_position_y-v(2), (vehicle_position_z+v(1)));
			glVertex3f(vehicle_position_x-vv(0), vehicle_position_y-vv(2), (vehicle_position_z+vv(1)));

			vv=vehicle2Global(0.67978*cos(1.11373+i*1.5708),0.67978*sin(1.11373+i*1.5708),-0.13);

			glVertex3f(vehicle_position_x-v(0), vehicle_position_y-v(2), (vehicle_position_z+v(1)));
			glVertex3f(vehicle_position_x-vv(0), vehicle_position_y-vv(2), (vehicle_position_z+vv(1)));
		}
	glEnd();
	//Shadow
	glColor3f(0.5f, 0.0f, 0.2f);
	glLineWidth(2);
	glBegin(GL_LINES);
		glVertex3f(vehicle_position_x,vehicle_position_y,vehicle_position_z);glVertex3f(vehicle_position_x,0.0f,vehicle_position_z);
	glEnd();

	//direction
	glColor3f(0.8f, 0.8f, 0.2f);
	glLineWidth(4);
	glBegin(GL_LINES);
		Eigen::Vector3d v=vehicle2Global(0,0,-0.26);
		Eigen::Vector3d vv=vehicle2Global(0.15,0,-0.26);

		glVertex3f(vehicle_position_x-v(0), vehicle_position_y-v(2), (vehicle_position_z+v(1)));
		glVertex3f(vehicle_position_x-vv(0), vehicle_position_y-vv(2), (vehicle_position_z+vv(1)));
	glEnd();

//Vehicle trace
	glColor3f(1.0f, 1.0f, 0.0f);
	glLineWidth(1);
	glBegin(GL_LINES);
		for(int i = 0; i < 19999 ; i++){
			if(position_record_x[i] && position_record_y[i] && position_record_z[i] && position_record_y[i+1] && position_record_z[i+1] && position_record_state[i] == NAV_USING_GROUND_TILE){
				glVertex3f(position_record_x[i], position_record_y[i], position_record_z[i]);
				glVertex3f(position_record_x[i+1], position_record_y[i+1], position_record_z[i+1]);
			}
		}
	glEnd();
	glColor3f(0.0f, 1.0f, 1.0f);
	glLineWidth(1);
	glBegin(GL_LINES);
		for(int i = 0; i < 19999 ; i++){
			if(position_record_x[i] && position_record_y[i] && position_record_z[i] && position_record_y[i+1] && position_record_z[i+1] && position_record_state[i] == NAV_USING_OPTICAL_FLOW){
				glVertex3f(position_record_x[i], position_record_y[i], position_record_z[i]);
				glVertex3f(position_record_x[i+1], position_record_y[i+1], position_record_z[i+1]);
			}
		}
	glEnd();
	glColor3f(1.0f, 0.0f, 1.0f);
	glLineWidth(1);
	glBegin(GL_LINES);
		for(int i = 0; i < 19999 ; i++){
			if(position_record_x[i] && position_record_y[i] && position_record_z[i] && position_record_y[i+1] && position_record_z[i+1] && position_record_state[i] == NAV_USING_SLAM){
				glVertex3f(position_record_x[i], position_record_y[i], position_record_z[i]);
				glVertex3f(position_record_x[i+1], position_record_y[i+1], position_record_z[i+1]);
			}
		}
	glEnd();

//Collapse Robot
	for(int i=0;i<10;i++){
		if((sqrt(pow(x-0.61-Robot_position_x[i],2)+pow(z+0.33-Robot_position_y[i],2))<R || \
		    sqrt(pow(x-0.61-Robot_position_x[i],2)+pow(z+0.16-Robot_position_y[i],2))<R || \
		    sqrt(pow(x-0.61-Robot_position_x[i],2)+pow(z-0.16-Robot_position_y[i],2))<R || \
		    sqrt(pow(x-0.61-Robot_position_x[i],2)+pow(z-0.33-Robot_position_y[i],2))<R || \
		    sqrt(pow(x+0.61-Robot_position_x[i],2)+pow(z+0.33-Robot_position_y[i],2))<R || \
		    sqrt(pow(x+0.61-Robot_position_x[i],2)+pow(z+0.16-Robot_position_y[i],2))<R || \
		    sqrt(pow(x+0.61-Robot_position_x[i],2)+pow(z-0.16-Robot_position_y[i],2))<R || \
		    sqrt(pow(x+0.61-Robot_position_x[i],2)+pow(z-0.33-Robot_position_y[i],2))<R || \
		    sqrt(pow(x+0.33-Robot_position_x[i],2)+pow(z-0.61-Robot_position_y[i],2))<R || \
		    sqrt(pow(x+0.16-Robot_position_x[i],2)+pow(z-0.61-Robot_position_y[i],2))<R || \
		    sqrt(pow(x-0.16-Robot_position_x[i],2)+pow(z-0.61-Robot_position_y[i],2))<R || \
		    sqrt(pow(x-0.33-Robot_position_x[i],2)+pow(z-0.61-Robot_position_y[i],2))<R || \
		    sqrt(pow(x+0.33-Robot_position_x[i],2)+pow(z+0.61-Robot_position_y[i],2))<R || \
		    sqrt(pow(x+0.16-Robot_position_x[i],2)+pow(z+0.61-Robot_position_y[i],2))<R || \
		    sqrt(pow(x-0.16-Robot_position_x[i],2)+pow(z+0.61-Robot_position_y[i],2))<R || \
		    sqrt(pow(x-0.33-Robot_position_x[i],2)+pow(z+0.61-Robot_position_y[i],2))<R || \
		    sqrt(pow(x-0.3-Robot_position_x[i],2)+pow(z-0.3-Robot_position_y[i],2))<3*R || \
		    sqrt(pow(x+0.3-Robot_position_x[i],2)+pow(z-0.3-Robot_position_y[i],2))<3*R || \
		    sqrt(pow(x+0.3-Robot_position_x[i],2)+pow(z+0.3-Robot_position_y[i],2))<3*R || \
		    sqrt(pow(x-0.3-Robot_position_x[i],2)+pow(z+0.3-Robot_position_y[i],2))<3*R) && \
			sqrt(pow(x-Robot_position_x[i],2)+pow(z-Robot_position_y[i],2))>2*R+0.3 && \
			y<=0.2 ){
				Robot_direction[i]=Robot_direction[i]>0? (Robot_direction[i]-3.141): (Robot_direction[i]+3.141);
				//vehicle_land=false;
		}
		if(sqrt(pow(x-Robot_position_x[i],2)+pow(z-Robot_position_y[i],2))<2*R+0.3 && y<=0.2 && vehicle_land==false){
			Robot_direction[i]=Robot_direction[i]>3.141*0.75 ? Robot_direction[i]-1.75*3.141 : Robot_direction[i]-0.25*3.141;
			vehicle_land=true;
		}
	}
//Collapse vehicle FIXME
	for(int i=0;i<4;i++){
		if((sqrt(pow(x-0.61-obstacle_position_x[i],2)+pow(z+0.33-obstacle_position_y[i],2))<R || \
		    sqrt(pow(x-0.61-obstacle_position_x[i],2)+pow(z+0.16-obstacle_position_y[i],2))<R || \
		    sqrt(pow(x-0.61-obstacle_position_x[i],2)+pow(z-0.16-obstacle_position_y[i],2))<R || \
		    sqrt(pow(x-0.61-obstacle_position_x[i],2)+pow(z-0.33-obstacle_position_y[i],2))<R || \
		    sqrt(pow(x+0.61-obstacle_position_x[i],2)+pow(z+0.33-obstacle_position_y[i],2))<R || \
		    sqrt(pow(x+0.61-obstacle_position_x[i],2)+pow(z+0.16-obstacle_position_y[i],2))<R || \
		    sqrt(pow(x+0.61-obstacle_position_x[i],2)+pow(z-0.16-obstacle_position_y[i],2))<R || \
		    sqrt(pow(x+0.61-obstacle_position_x[i],2)+pow(z-0.33-obstacle_position_y[i],2))<R || \
		    sqrt(pow(x+0.33-obstacle_position_x[i],2)+pow(z-0.61-obstacle_position_y[i],2))<R || \
		    sqrt(pow(x+0.16-obstacle_position_x[i],2)+pow(z-0.61-obstacle_position_y[i],2))<R || \
		    sqrt(pow(x-0.16-obstacle_position_x[i],2)+pow(z-0.61-obstacle_position_y[i],2))<R || \
		    sqrt(pow(x-0.33-obstacle_position_x[i],2)+pow(z-0.61-obstacle_position_y[i],2))<R || \
		    sqrt(pow(x+0.33-obstacle_position_x[i],2)+pow(z+0.61-obstacle_position_y[i],2))<R || \
		    sqrt(pow(x+0.16-obstacle_position_x[i],2)+pow(z+0.61-obstacle_position_y[i],2))<R || \
		    sqrt(pow(x-0.16-obstacle_position_x[i],2)+pow(z+0.61-obstacle_position_y[i],2))<R || \
		    sqrt(pow(x-0.33-obstacle_position_x[i],2)+pow(z+0.61-obstacle_position_y[i],2))<R || \
		    sqrt(pow(x-0.3-obstacle_position_x[i],2)+pow(z-0.3-obstacle_position_y[i],2))<3*R || \
		    sqrt(pow(x+0.3-obstacle_position_x[i],2)+pow(z-0.3-obstacle_position_y[i],2))<3*R || \
		    sqrt(pow(x+0.3-obstacle_position_x[i],2)+pow(z+0.3-obstacle_position_y[i],2))<3*R || \
		    sqrt(pow(x-0.3-obstacle_position_x[i],2)+pow(z+0.3-obstacle_position_y[i],2))<3*R) && \
			sqrt(pow(x-obstacle_position_x[i],2)+pow(z-obstacle_position_y[i],2))>2*R+0.3 && \
			y<=2.0 ){
				//vehicle_collision=true;
				Robot_move=false;
		}
		if(sqrt(pow(x-obstacle_position_x[i],2)+pow(z-obstacle_position_y[i],2))<2*R+0.3 && y<=2){
			//vehicle_collision=true;
			Robot_move=false;
		}
	}
//Text Score
	float score=0;
	for(int i=0;i<10;i++){
		if(Robot_position_y[i]<=-10.0f)
			score+=2000;
		else if(Robot_position_x[i]<=-10.0f || Robot_position_y[i]>=10.0f || Robot_position_x[i]>=10.0f)
			score+=-1000;
	}
	if(vehicle_collision==true)
		score=0;
	FTGLPixmapFont font("/home/darc/catkin_ws/src/glviewer/font/arial.ttf");
	FTPoint position_label_x(window_width-260.0f,window_height-40.0f,0.0f);
	font.FaceSize(13);
	char tempchar[10];
	sprintf(tempchar,"%f",score);
	const char *label_x=tempchar;
	font.Render(label_x,8,position_label_x);

	FTPoint position_text_x(window_width-330.0f,window_height-40.0f,0.0f);
	if(vehicle_collision==true){
		const char *positiontext_x="Failed:";
		font.Render(positiontext_x,7,position_text_x);
	}else{
		const char *positiontext_x="Score:";
		font.Render(positiontext_x,6,position_text_x);
	}
//Text time
	FTPoint position_label_time(window_width-260.0f,window_height-55.0f,0.0f);
	font.FaceSize(13);
	char timechar[10];
	sprintf(timechar,"%f",avgtime*1000);
	const char *label_time=timechar;
	font.Render(label_time,8,position_label_time);

	FTPoint position_text_time(window_width-330.0f,window_height-55.0f,0.0f);
	const char *timetext_x="Time:";
	font.Render(timetext_x,5,position_text_time);
//Text Position
	FTPoint position_label_position_x(window_width-260.0f,window_height-70.0f,0.0f);
	font.FaceSize(13);
	char positionchar[10];
	sprintf(positionchar,"%f",vehicle_position_x-10.0f);
	const char *label_position=positionchar;
	font.Render(label_position,8,position_label_position_x);

	FTPoint position_label_position_y(window_width-180.0f,window_height-70.0f,0.0f);
	font.FaceSize(13);
	sprintf(positionchar,"%f",vehicle_position_z);
	const char *label_position_y=positionchar;
	font.Render(label_position_y,8,position_label_position_y);

	FTPoint position_text_position(window_width-360.0f,window_height-70.0f,0.0f);
	const char *positiontext_x="Vehicle position:";
	font.Render(positiontext_x,17,position_text_position);
//Text Height
	FTPoint position_label_height(window_width-260.0f,window_height-85.0f,0.0f);
	font.FaceSize(13);
	char heightchar[10];
	sprintf(heightchar,"%f",vehicle_position_y);
	const char *label_height=heightchar;
	font.Render(label_height,8,position_label_height);

	FTPoint position_text_height(window_width-310.0f,window_height-85.0f,0.0f);
	const char *heighttext_x="Height:";
	font.Render(heighttext_x,7,position_text_height);

	glutSwapBuffers();

	ros::spinOnce();

}

void pressKey(unsigned char key, int x, int y) {

	switch (key) {
		case 'f' : fu=true;break;
		case 'r' : yang=true;break;
		case 'o' : overlook=true;break;
		case 'u' : deltaMove_UpDown = 1;break;
		case 'j' : deltaMove_UpDown = -1;break;
		case 'q' : deltaAngle = -0.01f;break;
		case 'e' : deltaAngle = 0.01f;break;
		case 'a' : deltaMove_LeftRight = 1;break;//deltaAngle = -0.01f;break;
		case 'd' : deltaMove_LeftRight = -1;break;//deltaAngle = 0.01f;break;
		case 'w' : deltaMove_FrontBack = 1;break;
		case 's' : deltaMove_FrontBack = -1;break;
		case 'c' : track_clear = true;break;
		case '1' : Robot_move = true;break;
		case '2' : Robot_move = false;break;
	}
}

void releaseKey(unsigned char key, int x, int y) {

	switch (key) {
		case 'f' : fu=false;break;
		case 'r' : yang=false;break;
		case 'o' : overlook=false;break;
		case 'u' :
		case 'j' : deltaMove_UpDown = 0;break;
		case 'q' :
		case 'e' : deltaAngle = 0.0f;break;
		case 'a' :
		case 'd' : deltaMove_LeftRight = 0;break;//deltaAngle = 0.0f;break;
		case 'w' :
		case 's' : deltaMove_FrontBack = 0;break;
		case 'c' : track_clear = false;break;
		case '1' : Robot_move = true;break;
		case '2' : Robot_move = false;break;
	}
}

void processNormalKeys(unsigned char key, int x, int y) {

	if (key == 27)
		exit(0);
}

void SetLight(void){
	const GLfloat AmbientLight[4]={1,1,1,-1};
	const GLfloat LightPosition[4]={10,10,10,0};
	glLightfv(GL_LIGHT0,GL_AMBIENT,AmbientLight);
	glLightfv(GL_LIGHT0,GL_POSITION,LightPosition);
	glEnable(GL_LIGHT0);
	glEnable(GL_LIGHTING);
}

int main(int argc, char **argv)
{
	gettimeofday(&tvstart,NULL);
//initial iRobot
	srand((unsigned)time(0));
	for(int i=0;i<10;i++){
		Robot_position_x[i]=rand()/(double)(RAND_MAX/2.0)-1.0;
		Robot_position_y[i]=rand()/(double)(RAND_MAX/2.0)-1.0;
		Robot_direction[i]=rand()/(double)(RAND_MAX/6.282)-3.141;
	}
	int j=0,mark=0,count=0;

	pitch = 0;
	yaw = 0;
	roll = 0;
	convertMat[0][0]=1;
	convertMat[1][0]=0;
	convertMat[2][0]=0;
	convertMat[0][1]=0;
	convertMat[1][1]=1;
	convertMat[2][1]=0;
	convertMat[0][2]=0;
	convertMat[1][2]=0;
	convertMat[2][2]=1;

	bool goon=true;
	while(goon){
		for(int i=0;i<10;i++){
			Robot_position_x[i]=rand()/(double)(RAND_MAX/2.0)-1.0;
			Robot_position_y[i]=rand()/(double)(RAND_MAX/2.0)-1.0;
		}
		for(int i=0;i<10;i++)
			for(j=i+1;j<10;j++){
				count+=1;
				if(sqrt(pow(Robot_position_x[i]-Robot_position_x[j],2)+pow(Robot_position_y[i]-Robot_position_y[j],2))>0.35)
					mark+=1;
			}
		if(mark==count) goon=false;
		mark=0;count=0;
	}

//initial ROS
        ros::init(argc, argv, "glviewer");

	ros::NodeHandle fusion_Opt_Imu;

	ros::NodeHandle optical_artificial;

	//ros::Subscriber sub_fusion_Opt_Imu = fusion_Opt_Imu.subscribe("fusion_Opt_Imu", 1000, opticalCallback);

	ros::Subscriber sub_optical_artificial = optical_artificial.subscribe("optical_artificial", 1000, opticalCallback);

//initial vehicle
	vehicle_position_x=10.0;vehicle_position_y=0.0;vehicle_direction=rand()/(double)(RAND_MAX/6.282)-3.141;
//initial obstacle
	obstacle_position_x[0]=5; obstacle_position_x[1]=0; obstacle_position_x[2]=-5; obstacle_position_x[3]=0;
	obstacle_position_y[0]=0; obstacle_position_y[1]=5; obstacle_position_y[2]=0; obstacle_position_y[3]=-5;
	obstacle_direction[0]=3.141;obstacle_direction[1]=3.141/2;obstacle_direction[2]=0;obstacle_direction[3]=3.141/2*3;
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGBA);
	glutInitWindowPosition(100,100);
	glutInitWindowSize(640,360);
	glutCreateWindow("IARC Mission 7");

	initScene();

	glutIgnoreKeyRepeat(1);
	glutKeyboardFunc(pressKey);
	glutKeyboardUpFunc(releaseKey);

	glutDisplayFunc(renderScene);
	glutIdleFunc(renderScene);

	glutReshapeFunc(changeSize);
	glutTimerFunc(20000,change_direction,0);
	glutTimerFunc(5000,error_direction,0);
	glutMainLoop();

	return(0);
}


#include "bodyParts.hpp"

#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

#include <set>
#include <tuple>

#include "creature.hpp"
#include "world.hpp"
#include "entity2DMap.hpp"
#include "pheromoneMap.hpp"
#include "obstacle.hpp"
#include "point.hpp"

#include "ant.hpp"

// BodyPart

bool BodyPart::isAccessible(Point p){
    if(!p.isInBounds(world_->getDimensions()))
        return false;

    int seeingRange=3;

    bool collision_detected = false;

    auto in_square = world_->getEntityMap().lock()->getEntitiesInSquare(
        Point(p.posX() - seeingRange, p.posY() - seeingRange),
        Point(p.posX() + seeingRange, p.posY() + seeingRange));

    for(const auto& a : in_square)
    {
        if(a.lock()->getPos() == p && a.lock()->hasCollision()){
            collision_detected = true;
            break;
        }
    }

    return !collision_detected;
}

// AntLegs

AntLegs::AntLegs(World* w, Creature* owner) : BodyPart(w,owner)
{
    targetPos_=owner->getPos();
    timeNotMoving_=0;
    timeGoingRandom_=0;
}

void AntLegs::goToPos(const Point& p){
    timeGoingRandom_=0;
    targetPos_=p;
}

void AntLegs::goRandom(){
    ++timeGoingRandom_;
    if(timeGoingRandom_>10 || timeGoingRandom_==1){
        targetPos_=Point(rand()-RAND_MAX/2, rand()-RAND_MAX/2);
        if(timeGoingRandom_>10)
            timeGoingRandom_=0;
    }
}

void AntLegs::step(int deltatime){
    while((deltatime--)>0){
        Point curPos=owner_->getPos();

        // check if this position is free (there is no other creature)
        // (detect collision)

        for(int i=0;i<=2;++i){
            // try to move by both axes, then single axes if not possible
            bool dx,dy;
            if(i==0){
                dx=1;
                dy=1;
            }else if(i==1){
                dx=1;
                dy=0;
            }else if(i==2){
                dx=0;
                dy=1;
            }

            int x=curPos.posX();
            int y=curPos.posY();

            if(dx && curPos.posX() != targetPos_.posX())
                x+= (curPos.posX() < targetPos_.posX()) ? 1 : -1;
            if(dy && curPos.posY() != targetPos_.posY())
                y+= (curPos.posY() < targetPos_.posY()) ? 1 : -1;

            if(BodyPart::isAccessible(Point(x,y))){
                owner_->energy_-=0.1;
                owner_->setPos(Point(x,y));
                timeNotMoving_=0;
                return;
            }
        }
        ++timeNotMoving_;
    }
}

// AntSensor

const float AntSensor::pheromoneRange=7.0;
const float AntSensor::seeingRange=7.0;

Point AntSensor::Observation::getPos()const{
    return ent_.lock()->getPos();
}

Entity::Smell AntSensor::Observation::getSmell()const{
    return ent_.lock()->getSmell();
}

std::vector<AntSensor::Observation> AntSensor::getObservations(){
    std::vector<Observation> ret;

    Point owner_pos = owner_-> getPos();

    auto in_square = world_->getEntityMap().lock()->getEntitiesInSquare(
        Point(owner_pos.posX() - seeingRange-1, owner_pos.posY() - seeingRange-1),
        Point(owner_pos.posX() + seeingRange+1, owner_pos.posY() + seeingRange+1));

    for(const auto& a : in_square)
    {
        if(a.lock()->getPos().getDistance(owner_pos) <= seeingRange)
            ret.push_back(Observation(a));
    }

    if(world_->isMultithreaded()){
        // thread safe sort
        std::vector<std::tuple<Point,unsigned,Observation> > vec;
        vec.reserve(ret.size());
        for(unsigned i=0;i<ret.size();++i){
            vec.push_back(std::make_tuple(ret[i].getPos(),i,ret[i]));
        }

        std::sort(vec.begin(),vec.end(),
            [owner_pos] (const auto& a,const auto& b) -> bool
            { return owner_pos.getDistance(std::get<0>(a))
                < owner_pos.getDistance(std::get<0>(b)); });

        for(auto v :vec){
            ret[std::get<1>(v)]=std::get<2>(v);
        }
    }else{
        std::sort(ret.begin(),ret.end(),
            [owner_pos] (const Observation& a,const Observation& b) -> bool
            { return owner_pos.getDistance(a.getPos())
                < owner_pos.getDistance(b.getPos()); });
    }

    return ret;
}

bool AntSensor::isAccessible(const Observation& o){
    return BodyPart::isAccessible(o.getPos());
}

Point AntSensor::getClosestPheromone(PheromoneMap::Type pType, float distance){
    float range=pheromoneRange;
    int r=int(range+1);
    Point ownPos=owner_->getPos();

    Point bestFit=Point(INT_MAX,INT_MAX);

    for(const auto pm : world_->getSimulationObjects<PheromoneMap>()){
        if(pm->getType() != pType)
            continue;

        for(int dx=-r; dx<=r; ++dx){
            for(int dy=-r; dy<=r; ++dy){

                int x=ownPos.posX()+dx;
                int y=ownPos.posY()+dy;
                Point pos=Point(x,y);

                if(pos.getDistance(ownPos)>range)
                    continue;

                if(!pos.isInBounds(world_->getDimensions()))
                    continue;

                if(pos.getDistance(ownPos)<distance)
                    continue;

                if(pm->getStrengthAtPosition(pos)<0.1)
                    continue;

                if(pos.getDistance(ownPos) < bestFit.getDistance(ownPos)){
                    bestFit=pos;
                }
            }
        }
    }

    return bestFit;
}

Point AntSensor::getFarthestPheromone(PheromoneMap::Type pType,float maxDistance){
    float range=pheromoneRange;
    int r=int(range+1);
    Point ownPos=owner_->getPos();
    Point bestFit=ownPos;

    for(const auto pm : world_->getSimulationObjects<PheromoneMap>()){
        if(pm->getType() != pType)
            continue;

        for(int dx=-r; dx<=r; ++dx){
            for(int dy=-r; dy<=r; ++dy){

                int x=ownPos.posX()+dx;
                int y=ownPos.posY()+dy;
                Point pos=Point(x,y);

                if(pos.getDistance(ownPos)>range)
                    continue;

                if(!pos.isInBounds(world_->getDimensions()))
                    continue;

                if(pos.getDistance(ownPos)>maxDistance)
                    continue;

                if(pm->getStrengthAtPosition(pos)<0.1)
                    continue;

                const float epsilon=1.5;
                if(pos.getDistance(ownPos)-epsilon > bestFit.getDistance(ownPos)){
                    // is much larger
                    bestFit=pos;
                    continue;
                }

                const float epsilon2=0.5;
                if(pos.getDistance(ownPos) + epsilon2 > bestFit.getDistance(ownPos)){
                    // is weakly(with epsilon) larger
                    // prefer pos that is closer to lastSensedPheromonePos
                    if(    pos.getDistance(lastSensedPheromonePos_) <
                        bestFit.getDistance(lastSensedPheromonePos_)){
                        bestFit=pos;
                    }
                }
            }
        }
        break;
    }
    lastSensedPheromonePos_=bestFit;
    return bestFit;
}

float AntSensor::getPheromoneStrength(PheromoneMap::Type type, Point pos){
    for(const auto pm : world_->getSimulationObjects<PheromoneMap>()){
        if(pm->getType() != type)
            continue;
        if(owner_->getPos().getDistance(pos)<=pheromoneRange)
            return pm->getStrengthAtPosition(pos);
        else
            return 0;
    }
    return 0;
}

Point AntSensor::findAdjecentPos(Point p){
    for(int dx=-1; dx<=1; ++dx){
        for(int dy=-1; dy<=1; ++dy){
            if((dx==0)!=(dy==0)){
                Point adjP=Point(p.posX()+dx,p.posY()+dy);
                if(BodyPart::isAccessible(adjP)){
                    return adjP;
                }
            }
        }
    }
    return Point(INT_MAX,INT_MAX);
}

// AntMandibles

boost::weak_ptr<Entity> AntMandibles::getHoldingObject() const
{
	return holdingObject_;
}

bool AntMandibles::grab(boost::weak_ptr<Entity> e){

    if(isHolding())
        return false;

    if (owner_->getPos()== e.lock()->getPos()){
        holdingObject_ = e;
    }

    return true;
}

bool AntMandibles::grab(AntSensor::Observation o){
    grab(o.ent_);
    return 1;
}

bool AntMandibles::bite(AntSensor::Observation o){
    if(!o.ent_.expired() && o.getPos().isAdjacent(owner_->getPos())){
        bittingTarget_=o.ent_;
        return 1;
    }
    return 0;
}

bool AntMandibles::drop(){
    if(holdingObject_.expired())
        return false;
    else{
        holdingObject_=boost::weak_ptr<Entity>();
        return true;
    }
}

void AntMandibles::step(int deltaTime){
    if(isHolding()){
        if(deltaTime>0)
            owner_->energy_-=0.1;
        holdingObject_.lock()->setPos(owner_->getPos());
    }
    if(deltaTime>0 && !bittingTarget_.expired()){
        float en=bittingTarget_.lock()->bite(4.01);
        owner_->energy_-=0.5;
        owner_->energy_+=en;
        bittingTarget_=boost::weak_ptr<Entity>();
    }
}

void AntMandibles::accept(Visitor &v) const
{
	v.visit(*this);
}

// AntWorkerAbdomen

void AntWorkerAbdomen::dropToFoodPheromones(){
    dropType=PheromoneMap::Type::ToFood;
}
void AntWorkerAbdomen::dropFromFoodPheromones(){
    dropType=PheromoneMap::Type::FromFood;
}

void AntWorkerAbdomen::step(int deltaTime){
    if(deltaTime<=0)
        return;
    if(dropType == PheromoneMap::Type::None)
        return;
    for(const auto& pm : world_->getSimulationObjects<PheromoneMap>()){
        if(pm->getType()==dropType){
            pm->createBlob(owner_->getPos(), 1.5, 100);
            owner_->energy_-=0.2;
            dropType = PheromoneMap::Type::None;
            return;
        }
    }

    throw std::runtime_error("AntWorkerAbdomen::step, no such pheromone map");
    dropType = PheromoneMap::Type::None;
}

// AntQueenAbdomen

void AntQueenAbdomen::dropAnthillPheromones(){
    dropType=PheromoneMap::Type::Anthill;
}
void AntQueenAbdomen::bornWorker(){
    bornType=BornType::Worker;
}
void AntQueenAbdomen::bornScout(){
    bornType=BornType::Scout;
}

void AntQueenAbdomen::step(int deltaTime){
    lastBornCounter-=deltaTime;
    if(deltaTime>0 && dropType != PheromoneMap::Type::None){
        for(auto pm : world_->getSimulationObjects<PheromoneMap>()){
            if(pm->getType()==dropType){
                pm->createBlob(owner_->getPos(), 10, 200);
                owner_->energy_-=1;
                dropType = PheromoneMap::Type::None;
                break;
            }
        }
    }

    if(bornType != BornType::None){
        if(lastBornCounter<=0){
            boost::shared_ptr<Creature> p;
            if(bornType==BornType::Worker)
                p=boost::make_shared<Ant>(world_, owner_->getPos(), Ant::Type::Worker);
            else
                p=boost::make_shared<Ant>(world_, owner_->getPos(), Ant::Type::Scout);
            world_->addSimulationObject<Creature>(p);
            owner_->energy_-=p->getMaxEnergy();
            bornType=BornType::None;
            lastBornCounter=40;
        }
    }
}

// AntLarvaBody

void AntLarvaBody::step(int deltaTime){
    // TODO
    assert(0);
}


// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is licensed under the Apache License, Version 2.0 (the
//  "License"); you may not use this file except in compliance with the
//  License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

#include <vw/Math/Matrix.h>
#include <vw/Math/Quaternion.h>
#include <vw/Math/Vector.h>
#include <vw/Math/LevenbergMarquardt.h>
#include <vw/Camera/CameraModel.h>
#include <asp/IsisIO/IsisInterfaceMapLineScan.h>

#include <algorithm>
#include <limits>
#include <string>
#include <vector>

#include <Camera.h>
#include <CameraDistortionMap.h>
#include <CameraFocalPlaneMap.h>
#include <CameraGroundMap.h>
#include <Latitude.h>
#include <Longitude.h>
#include <Projection.h>
#include <ProjectionFactory.h>
#include <Sensor.h>
#include <SpiceRotation.h>
#include <iTime.h>

#include <boost/smart_ptr/scoped_ptr.hpp>

using namespace vw;
using namespace asp;
using namespace asp::isis;

// Constructor
IsisInterfaceMapLineScan::IsisInterfaceMapLineScan( std::string const& filename ) :
  IsisInterface( filename ){//, m_projection( Isis::ProjectionFactory::CreateFromCube(*m_label) ) {

  Isis::TProjection* tempProj = (Isis::TProjection*)Isis::ProjectionFactory::CreateFromCube(*m_label);
  m_projection.reset(tempProj);

  // Gutting Isis::Camera
  m_distortmap = m_camera->DistortionMap();
  m_groundmap = m_camera->GroundMap();
  m_focalmap = m_camera->FocalPlaneMap();
  m_cache_px[0] = m_cache_px[1] = std::numeric_limits<double>::quiet_NaN();
}

// Custom Functions
class EphemerisLMA : public vw::math::LeastSquaresModelBase<EphemerisLMA> {
  vw::Vector3 m_point;
  Isis::Camera* m_camera;
  Isis::CameraDistortionMap *m_distortmap;
  Isis::CameraFocalPlaneMap *m_focalmap;
public:
  typedef vw::Vector<double> result_type; // Back project result
      typedef vw::Vector<double> domain_type; // Ephemeris time
  typedef vw::Matrix<double> jacobian_type;

  inline EphemerisLMA( vw::Vector3 const& point,
                       Isis::Camera* camera,
                       Isis::CameraDistortionMap* distortmap,
                       Isis::CameraFocalPlaneMap* focalmap ) : m_point(point), m_camera(camera), m_distortmap(distortmap), m_focalmap(focalmap) {}

  inline result_type operator()( domain_type const& x ) const;
};

// LMA for projecting point to linescan camera
EphemerisLMA::result_type
EphemerisLMA::operator()( EphemerisLMA::domain_type const& x ) const {

  // Setting Ephemeris Time
  m_camera->setTime( Isis::iTime( x[0] ));

  // Calculating the look direction in camera frame
  Vector3 instru;
  m_camera->instrumentPosition(&instru[0]);
  instru *= 1000;  // Spice gives in km
  Vector3 lookB = normalize( m_point - instru );
  std::vector<double> lookB_copy(3);
  std::copy(lookB.begin(),lookB.end(),lookB_copy.begin());
  std::vector<double> lookJ = m_camera->bodyRotation()->J2000Vector(lookB_copy);
  std::vector<double> lookC = m_camera->instrumentRotation()->ReferenceVector(lookJ);
  Vector3 look;
  std::copy(lookC.begin(),lookC.end(),look.begin());

  // Projecting to mm focal plane
  look = m_camera->FocalLength() * (look / look[2]);
  m_distortmap->SetUndistortedFocalPlane(look[0], look[1]);
  m_focalmap->SetFocalPlane( m_distortmap->FocalPlaneX(),
                             m_distortmap->FocalPlaneY() );
  result_type result(1);
  // Not exactly sure about lineoffset .. but ISIS does it
  result[0] = m_focalmap->DetectorLineOffset() - m_focalmap->DetectorLine();

  return result;
}

Vector2
IsisInterfaceMapLineScan::point_to_pixel( Vector3 const& point ) const {

  // First seed LMA with an ephemeris time in the middle of the image
  double middle_et =
    m_camera->cacheStartTime().Et() + (m_camera->cacheEndTime().Et()-m_camera->cacheStartTime().Et())/2.0;

  // Build LMA
  EphemerisLMA model( point, m_camera.get(), m_distortmap, m_focalmap );
  int status;
  Vector<double> objective(1), start(1);
  start[0] = middle_et;
  Vector<double> solution_e = math::levenberg_marquardt( model,
                                                         start,
                                                         objective,
                                                         status );

  // Make sure we found ideal time
  VW_ASSERT( status > 0,
             camera::PointToPixelErr() << " Unable to project point into ISIS map linescan camera " );

  // Setting to camera time to solution
  m_camera->setTime( Isis::iTime( solution_e[0] ) );

  // Working out pointing
  Vector3 center;
  m_camera->instrumentPosition(&center[0]);
  Vector3 look = normalize(point-1000*center);

  // Calculating rotation to camera frame
  std::vector<double> rot_inst = m_camera->instrumentRotation()->Matrix();
  std::vector<double> rot_body = m_camera->bodyRotation()->Matrix();
  MatrixProxy<double,3,3> R_inst(&(rot_inst[0]));
  MatrixProxy<double,3,3> R_body(&(rot_body[0]));
  look = transpose(R_body*transpose(R_inst))*look;
  look = m_camera->FocalLength() * (look / look[2]);

  // Projecting back on to ground to find out time
  m_groundmap->SetFocalPlane( look[0],
                              look[1],
                              look[2] );

  m_projection->SetGround( m_camera->UniversalLatitude(),
                           m_camera->UniversalLongitude() );
  m_cache_px = Vector2( m_projection->WorldX()-1,
                        m_projection->WorldY()-1 );
  return m_cache_px;
}

Vector3
IsisInterfaceMapLineScan::camera_center( Vector2 const& px ) const {
  if ( px != m_cache_px ) {
    m_cache_px = px;
    if (!m_projection->SetWorld( px[0]+1,
                                 px[1]+1 ))
      vw_throw( camera::PixelToRayErr() << "Failed to SetWorld." );
    if (!m_groundmap->SetGround( Isis::Latitude(m_projection->UniversalLatitude(),Isis::Angle::Degrees),
                                 Isis::Longitude(m_projection->UniversalLongitude(),Isis::Angle::Degrees) ) )
      vw_throw( camera::PixelToRayErr() << "Failed to SetGround." );
  }
  Vector3 position;
  m_camera->instrumentPosition( &position[0] );
  return position * 1e3;
}

Vector3
IsisInterfaceMapLineScan::pixel_to_vector( Vector2 const& px ) const {
  Vector3 sB = camera_center( px );
  Vector3 p_pB;
  m_camera->Sensor::Coordinate( &p_pB[0] );
  return normalize(p_pB*1000 - sB);
}

Quat
IsisInterfaceMapLineScan::camera_pose( Vector2 const& px ) const {
  camera_center( px );
  std::vector<double> rot_inst = m_camera->instrumentRotation()->Matrix();
  std::vector<double> rot_body = m_camera->bodyRotation()->Matrix();
  MatrixProxy<double,3,3> R_inst(&(rot_inst[0]));
  MatrixProxy<double,3,3> R_body(&(rot_body[0]));
  return Quat(R_body*transpose(R_inst));
}


#include <iostream>
#include <fstream>
#include <algorithm>
#include <stdio.h>
#include <math.h>
#include <vector>
#include <numeric>
#include <strings.h>
#include <assert.h>
#include <sys/types.h>
#include <stdexcept>

#include <dirent.h>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)

typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > Polygon;

using namespace std;

/*=======================================================================
STATIC EVALUATION PARAMETERS
=======================================================================*/

// easy and hard evaluation level
enum DIFFICULTY{EASY=0, HARD=1};

const int32_t N_TESTIMAGES = 27661;

const int32_t MIN_3D_N_POINTS = 10;
const double MAX_3D_DIST[2] = {15, 25};
const double MIN_2D_AREA[2] = {1600, 500};
const double MAX_2D_OCC = 2;
// evaluation metrics: image, ground or 3D
enum METRIC{IMAGE=0, GROUND=1, BOX3D=2};

// evaluated object classes
enum CLASSES{CAR=0, PEDESTRIAN=1, CYCLIST=2};
const int NUM_CLASS = 3;

// parameters varying per class
vector<string> CLASS_NAMES;
vector<string> CLASS_NAMES_CAP;
// the minimum overlap required for 2D evaluation on the image/ground plane and 3D evaluation
const double MIN_OVERLAP[3][3] = {{0.7, 0.5, 0.5}, {0.7, 0.5, 0.5}, {0.5, 0.3, 0.3}};

// no. of recall steps that should be evaluated (discretized)
const double N_SAMPLE_PTS = 41;

// initialize class names
void initGlobals () {
  CLASS_NAMES.push_back("car");
  CLASS_NAMES.push_back("pedestrian");
  CLASS_NAMES.push_back("cyclist");
  CLASS_NAMES_CAP.push_back("Car");
  CLASS_NAMES_CAP.push_back("Pedestrian");
  CLASS_NAMES_CAP.push_back("Cyclist");
}

/*=======================================================================
DATA TYPES FOR EVALUATION
=======================================================================*/

// holding data needed for precision-recall and precision-aos
struct tPrData {
  vector<double> v;           // detection score for computing score thresholds
  double         similarity;  // orientation similarity
  int32_t        tp;          // true positives
  int32_t        fp;          // false positives
  int32_t        fn;          // false negatives
  tPrData () :
    similarity(0), tp(0), fp(0), fn(0) {}
};

// holding bounding boxes for ground truth and detections
struct tBox {
  string  type;     // object type as car, pedestrian or cyclist,...
  double   x1;      // left corner
  double   y1;      // top corner
  double   x2;      // right corner
  double   y2;      // bottom corner
  double   alpha;   // image orientation
  tBox (string type, double x1,double y1,double x2,double y2,double alpha) :
    type(type),x1(x1),y1(y1),x2(x2),y2(y2),alpha(alpha) {}
};

// holding ground truth data
struct tGroundtruth {
  tBox    box;        // object type, box, orientation
  int32_t  truncation; // truncation 0..1
  int32_t occlusion;  // occlusion 0,1,2,3 (non, partly, partly, fully)
  int32_t num_points_3d;
  double ry;
  double  t1, t2, t3;
  double h, w, l;
  tGroundtruth () :
    box(tBox("invalild",-1,-1,-1,-1,-10)),truncation(-1),occlusion(-1) {}
  tGroundtruth (tBox box,int32_t truncation,int32_t occlusion) :
    box(box),truncation(truncation),occlusion(occlusion) {}
  tGroundtruth (string type,double x1,double y1,double x2,double y2,double alpha,int32_t truncation,int32_t occlusion) :
    box(tBox(type,x1,y1,x2,y2,alpha)),truncation(truncation),occlusion(occlusion) {}
};

// holding detection data
struct tDetection {
  tBox    box;    // object type, box, orientation
  double  thresh; // detection score
  double  ry;
  double  t1, t2, t3;
  double  h, w, l;
  tDetection ():
    box(tBox("invalid",-1,-1,-1,-1,-10)),thresh(-1000) {}
  tDetection (tBox box,double thresh) :
    box(box),thresh(thresh) {}
  tDetection (string type,double x1,double y1,double x2,double y2,double alpha,double thresh) :
    box(tBox(type,x1,y1,x2,y2,alpha)),thresh(thresh) {}
};

/*=======================================================================
FUNCTIONS TO LOAD DETECTION AND GROUND TRUTH DATA ONCE, SAVE RESULTS
=======================================================================*/
vector<tDetection> loadDetection(string file_name) {

  vector<tDetection> detections;
  FILE *fp = fopen(file_name.c_str(),"r");
  if (!fp)
    throw invalid_argument("cannot read detection file " + file_name);
  while (!feof(fp)) {
    tDetection d;
    int trash;
    char str[255];
    if (fscanf(fp, "%s %d %d %d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
                   str, &trash, &trash, &trash, &d.box.alpha, &d.box.x1, &d.box.y1,
                   &d.box.x2, &d.box.y2, &d.h, &d.w, &d.l, &d.t1, &d.t2, &d.t3,
                   &d.ry, &d.thresh)==17) {
      d.box.type = str;
      detections.push_back(d);
    }
  }

  fclose(fp);
  return detections;
}

vector<tGroundtruth> loadGroundtruth(string file_name) {
  vector<tGroundtruth> groundtruth;
  FILE *fp = fopen(file_name.c_str(),"r");
  if (!fp)
    throw invalid_argument("cannot read ground truth file " + file_name);
  while (!feof(fp)) {
    tGroundtruth g;
    int trash;
    char str[255];
    if (fscanf(fp, "%s %d %d %d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %d %d",
                   str, &g.truncation, &g.occlusion, &g.num_points_3d,
                   &g.box.alpha, &g.box.x1, &g.box.y1, &g.box.x2, &g.box.y2,
                   &g.h, &g.w, &g.l, &g.t1, &g.t2, &g.t3, &g.ry, &trash)==17) {
      g.box.type = str;
      groundtruth.push_back(g);
    }
  }
  fclose(fp);
  return groundtruth;
}

vector<string> list_dir(const string path) {
  struct dirent *entry;
  DIR *dir = opendir(path.c_str());
  vector<string> entries;

  if (dir == NULL) {
    return entries;
  }
  while ((entry = readdir(dir)) != NULL) {
    if (strcmp(entry->d_name, ".") != 0 && strcmp(entry->d_name, "..") != 0)
      entries.push_back(entry->d_name);
  }
  closedir(dir);
  return entries;
}

/*=======================================================================
EVALUATION HELPER FUNCTIONS
=======================================================================*/

// criterion defines whether the overlap is computed with respect to both areas (ground truth and detection)
// or with respect to box a or b (detection and "dontcare" areas)
inline double imageBoxOverlap(tBox a, tBox b, int32_t criterion=-1){

  // overlap is invalid in the beginning
  double o = -1;

  // get overlapping area
  double x1 = max(a.x1, b.x1);
  double y1 = max(a.y1, b.y1);
  double x2 = min(a.x2, b.x2);
  double y2 = min(a.y2, b.y2);

  // compute width and height of overlapping area
  double w = x2-x1;
  double h = y2-y1;

  // set invalid entries to 0 overlap
  if(w<=0 || h<=0)
    return 0;

  // get overlapping areas
  double inter = w*h;
  double a_area = (a.x2-a.x1) * (a.y2-a.y1);
  double b_area = (b.x2-b.x1) * (b.y2-b.y1);

  // intersection over union overlap depending on users choice
  if(criterion==-1)     // union
    o = inter / (a_area+b_area-inter);
  else if(criterion==0) // bbox_a
    o = inter / a_area;
  else if(criterion==1) // bbox_b
    o = inter / b_area;

  // overlap
  return o;
}

inline double imageBoxOverlap(tDetection a, tGroundtruth b, int32_t criterion=-1){
  return imageBoxOverlap(a.box, b.box, criterion);
}

// compute polygon of an oriented bounding box
template <typename T>
Polygon toPolygon(const T& g) {
    using namespace boost::numeric::ublas;
    using namespace boost::geometry;
    matrix<double> mref(2, 2);
    mref(0, 0) = cos(g.ry); mref(0, 1) = sin(g.ry);
    mref(1, 0) = -sin(g.ry); mref(1, 1) = cos(g.ry);

    static int count = 0;
    matrix<double> corners(2, 4);
    double data[] = {g.l / 2, g.l / 2, -g.l / 2, -g.l / 2,
                     g.w / 2, -g.w / 2, -g.w / 2, g.w / 2};
    std::copy(data, data + 8, corners.data().begin());
    matrix<double> gc = prod(mref, corners);
    for (int i = 0; i < 4; ++i) {
        gc(0, i) += g.t1;
        gc(1, i) += g.t3;
    }

    double points[][2] = {{gc(0, 0), gc(1, 0)},{gc(0, 1), gc(1, 1)},{gc(0, 2), gc(1, 2)},{gc(0, 3), gc(1, 3)},{gc(0, 0), gc(1, 0)}};
    Polygon poly;
    append(poly, points);
    return poly;
}

// measure overlap between bird's eye view bounding boxes, parametrized by (ry, l, w, tx, tz)
inline double groundBoxOverlap(tDetection d, tGroundtruth g, int32_t criterion = -1) {
    using namespace boost::geometry;
    Polygon gp = toPolygon(g);
    Polygon dp = toPolygon(d);

    std::vector<Polygon> in, un;
    intersection(gp, dp, in);
    union_(gp, dp, un);

    double inter_area = in.empty() ? 0 : area(in.front());
    double union_area = area(un.front());
    double o;
    if(criterion==-1)     // union
        o = inter_area / union_area;
    else if(criterion==0) // bbox_a
        o = inter_area / area(dp);
    else if(criterion==1) // bbox_b
        o = inter_area / area(gp);

    return o;
}

// measure overlap between 3D bounding boxes, parametrized by (ry, h, w, l, tx, ty, tz)
inline double box3DOverlap(tDetection d, tGroundtruth g, int32_t criterion = -1) {
    using namespace boost::geometry;
    Polygon gp = toPolygon(g);
    Polygon dp = toPolygon(d);

    std::vector<Polygon> in, un;
    intersection(gp, dp, in);
    union_(gp, dp, un);

    double ymax = min(d.t2, g.t2);
    double ymin = max(d.t2 - d.h, g.t2 - g.h);

    double inter_area = in.empty() ? 0 : area(in.front());
    double inter_vol = inter_area * max(0.0, ymax - ymin);

    double det_vol = d.h * d.l * d.w;
    double gt_vol = g.h * g.l * g.w;

    double o;
    if(criterion==-1)     // union
        o = inter_vol / (det_vol + gt_vol - inter_vol);
    else if(criterion==0) // bbox_a
        o = inter_vol / det_vol;
    else if(criterion==1) // bbox_b
        o = inter_vol / gt_vol;

    return o;
}

vector<double> getThresholds(vector<double> &v, double n_groundtruth){

  // holds scores needed to compute N_SAMPLE_PTS recall values
  vector<double> t;

  // sort scores in descending order
  // (highest score is assumed to give best/most confident detections)
  sort(v.begin(), v.end(), greater<double>());

  // get scores for linearly spaced recall
  double current_recall = 0;
  for(int32_t i=0; i<v.size(); i++){

    // check if right-hand-side recall with respect to current recall is close than left-hand-side one
    // in this case, skip the current detection score
    double l_recall, r_recall, recall;
    l_recall = (double)(i+1)/n_groundtruth;
    if(i<(v.size()-1))
      r_recall = (double)(i+2)/n_groundtruth;
    else
      r_recall = l_recall;

    if( (r_recall-current_recall) < (current_recall-l_recall) && i<(v.size()-1))
      continue;

    // left recall is the best approximation, so use this and goto next recall step for approximation
    recall = l_recall;

    // the next recall step was reached
    t.push_back(v[i]);
    current_recall += 1.0/(N_SAMPLE_PTS-1.0);
  }
  return t;
}

void cleanData(CLASSES current_class, const vector<tGroundtruth> &gt, const vector<tDetection> &det, vector<int32_t> &ignored_gt, vector<tGroundtruth> &dc, vector<int32_t> &ignored_det, int32_t &n_gt, DIFFICULTY difficulty, bool depth) {

  // extract ground truth bounding boxes for current evaluation class
  for(int32_t i=0;i<gt.size(); i++){

    // neighboring classes are ignored ("van" for "car" and "person_sitting" for "pedestrian")
    // (lower/upper cases are ignored)
    int32_t valid_class;

    // all classes without a neighboring class
    if(!strcasecmp(gt[i].box.type.c_str(), CLASS_NAMES[current_class].c_str()))
      valid_class = 1;

    // classes with a neighboring class
    else if(!strcasecmp(CLASS_NAMES[current_class].c_str(), "Pedestrian") && !strcasecmp("Person_sitting", gt[i].box.type.c_str()))
      valid_class = 0;
    else if(!strcasecmp(CLASS_NAMES[current_class].c_str(), "Car") && !strcasecmp("Van", gt[i].box.type.c_str()))
      valid_class = 0;

    // classes not used for evaluation
    else
      valid_class = -1;

    bool ignore = false;
    bool invalid = false;
    // 3D groundtruth filter criteria
    if (depth) {
      if (gt[i].num_points_3d < 0)
        invalid = true;
      if (gt[i].num_points_3d < MIN_3D_N_POINTS)
        ignore = true;
      if (gt[i].t1 * gt[i].t1 + gt[i].t3 * gt[i].t3 > MAX_3D_DIST[difficulty] * MAX_3D_DIST[difficulty])
        ignore = true;
    } else {
      double height = gt[i].box.y2 - gt[i].box.y1;
      double width = gt[i].box.x2 - gt[i].box.x1;
      double area = width * height;
      if (gt[i].box.x1 < 0)
        invalid = true;
      if (area < MIN_2D_AREA[difficulty])
        ignore = true;
      if (gt[i].occlusion > MAX_2D_OCC)
        ignore = true;
    }
    // set ignored vector for ground truth
    // current class and not ignored (total no. of ground truth is detected for recall denominator)
    if(invalid)
      ignored_gt.push_back(-1);
    else if(valid_class==1 && !ignore){
      ignored_gt.push_back(0);
      n_gt++;
    }
    else
      ignored_gt.push_back(1);
  }

  // extract dontcare areas
  for(int32_t i=0;i<gt.size(); i++)
    if(!strcasecmp("DontCare", gt[i].box.type.c_str()))
      dc.push_back(gt[i]);

  // extract detections bounding boxes of the current class
  for(int32_t i=0;i<det.size(); i++){

    // neighboring classes are not evaluated
    int32_t valid_class;
    if(!strcasecmp(det[i].box.type.c_str(), CLASS_NAMES[current_class].c_str()))
      valid_class = 1;
    else
      valid_class = -1;

    bool ignore = false;
    if (depth) {
      if (det[i].t1 * det[i].t1 + det[i].t3 * det[i].t3 > MAX_3D_DIST[difficulty] * MAX_3D_DIST[difficulty])
        ignore = true;
    } else {
      double height = det[i].box.y2 - det[i].box.y1;
      double width = det[i].box.x2 - det[i].box.x1;
      double area = width * height;
      if (area < MIN_2D_AREA[difficulty])
        ignore = true;
    }
    // set ignored vector for detections
    if(ignore)
      ignored_det.push_back(1);
    else if(valid_class==1)
      ignored_det.push_back(0);
    else
      ignored_det.push_back(-1);
  }
}

tPrData computeStatistics(CLASSES current_class, const vector<tGroundtruth> &gt,
        const vector<tDetection> &det, const vector<tGroundtruth> &dc,
        const vector<int32_t> &ignored_gt, const vector<int32_t>  &ignored_det,
        bool compute_fp, double (*boxoverlap)(tDetection, tGroundtruth, int32_t),
        METRIC metric, bool compute_aos=false, double thresh=0, bool debug=false){

  tPrData stat = tPrData();
  const double NO_DETECTION = -10000000;
  vector<double> delta;            // holds angular difference for TPs (needed for AOS evaluation)
  vector<bool> assigned_detection; // holds wether a detection was assigned to a valid or ignored ground truth
  assigned_detection.assign(det.size(), false);
  vector<bool> ignored_threshold;
  ignored_threshold.assign(det.size(), false); // holds detections with a threshold lower than thresh if FP are computed

  // detections with a low score are ignored for computing precision (needs FP)
  if(compute_fp)
    for(int32_t i=0; i<det.size(); i++)
      if(det[i].thresh<thresh)
        ignored_threshold[i] = true;

  // evaluate all ground truth boxes
  for(int32_t i=0; i<gt.size(); i++){

    // this ground truth is not of the current or a neighboring class and therefore ignored
    if(ignored_gt[i]==-1)
      continue;

    /*=======================================================================
    find candidates (overlap with ground truth > 0.5) (logical len(det))
    =======================================================================*/
    int32_t det_idx          = -1;
    double valid_detection = NO_DETECTION;
    double max_overlap     = 0;

    // search for a possible detection
    bool assigned_ignored_det = false;
    for(int32_t j=0; j<det.size(); j++){

      // detections not of the current class, already assigned or with a low threshold are ignored
      if(ignored_det[j]==-1)
        continue;
      if(assigned_detection[j])
        continue;
      if(ignored_threshold[j])
        continue;

      // find the maximum score for the candidates and get idx of respective detection
      double overlap = boxoverlap(det[j], gt[i], -1);

      // for computing recall thresholds, the candidate with highest score is considered
      if(!compute_fp && overlap>MIN_OVERLAP[metric][current_class] && det[j].thresh>valid_detection){
        det_idx         = j;
        valid_detection = det[j].thresh;
      }

      // for computing pr curve values, the candidate with the greatest overlap is considered
      // if the greatest overlap is an ignored detection, the overlapping detection is used
      else if(compute_fp && overlap>MIN_OVERLAP[metric][current_class] && (overlap>max_overlap || assigned_ignored_det) && ignored_det[j]==0){
        max_overlap     = overlap;
        det_idx         = j;
        valid_detection = 1;
        assigned_ignored_det = false;
      }
      else if(compute_fp && overlap>MIN_OVERLAP[metric][current_class] && valid_detection==NO_DETECTION && ignored_det[j]==1){
        det_idx              = j;
        valid_detection      = 1;
        assigned_ignored_det = true;
      }
    }

    /*=======================================================================
    compute TP, FP and FN
    =======================================================================*/

    // nothing was assigned to this valid ground truth
    if(valid_detection==NO_DETECTION && ignored_gt[i]==0) {
      stat.fn++;
    }

    // only evaluate valid ground truth <=> detection assignments
    else if(valid_detection!=NO_DETECTION && (ignored_gt[i]==1 || ignored_det[det_idx]==1))
      assigned_detection[det_idx] = true;

    // found a valid true positive
    else if(valid_detection!=NO_DETECTION){

      // write highest score to threshold vector
      stat.tp++;
      stat.v.push_back(det[det_idx].thresh);

      // compute angular difference of detection and ground truth if valid detection orientation was provided
      if(compute_aos)
        delta.push_back(gt[i].box.alpha - det[det_idx].box.alpha);

      // clean up
      assigned_detection[det_idx] = true;
    }
  }

  // if FP are requested, consider stuff area
  if(compute_fp){

    // count fp
    for(int32_t i=0; i<det.size(); i++){

      // count false positives if required (height smaller than required is ignored (ignored_det==1)
      if(!(assigned_detection[i] || ignored_det[i]==-1 || ignored_det[i]==1 || ignored_threshold[i]))
        stat.fp++;
    }

    // do not consider detections overlapping with stuff area
    int32_t nstuff = 0;
    for(int32_t i=0; i<dc.size(); i++){
      for(int32_t j=0; j<det.size(); j++){

        // detections not of the current class, already assigned, with a low threshold or a low minimum height are ignored
        if(assigned_detection[j])
          continue;
        if(ignored_det[j]==-1 || ignored_det[j]==1)
          continue;
        if(ignored_threshold[j])
          continue;

        // compute overlap and assign to stuff area, if overlap exceeds class specific value
        double overlap = boxoverlap(det[j], dc[i], 0);
        if(overlap>MIN_OVERLAP[metric][current_class]){
          assigned_detection[j] = true;
          nstuff++;
        }
      }
    }

    // FP = no. of all not to ground truth assigned detections - detections assigned to stuff areas
    stat.fp -= nstuff;

    // if all orientation values are valid, the AOS is computed
    if(compute_aos){
      vector<double> tmp;

      // FP have a similarity of 0, for all TP compute AOS
      tmp.assign(stat.fp, 0);
      for(int32_t i=0; i<delta.size(); i++)
        tmp.push_back((1.0+cos(delta[i]))/2.0);

      // be sure, that all orientation deltas are computed
      assert(tmp.size()==stat.fp+stat.tp);
      assert(delta.size()==stat.tp);

      // get the mean orientation similarity for this image
      if(stat.tp>0 || stat.fp>0)
        stat.similarity = accumulate(tmp.begin(), tmp.end(), 0.0);

      // there was neither a FP nor a TP, so the similarity is ignored in the evaluation
      else
        stat.similarity = -1;
    }
  }
  return stat;
}

/*=======================================================================
EVALUATE CLASS-WISE
=======================================================================*/

bool eval_class(CLASSES current_class,
        const vector< vector<tGroundtruth> > &groundtruth,
        const vector< vector<tDetection> > &detections, bool compute_aos,
        double (*boxoverlap)(tDetection, tGroundtruth, int32_t),
        vector<double> &precision,
        METRIC metric, DIFFICULTY difficulty, bool depth) {
  assert(groundtruth.size() == detections.size());

  // init
  int32_t n_gt=0;                                     // total no. of gt (denominator of recall)
  vector<double> v, thresholds;                       // detection scores, evaluated for recall discretization
  vector< vector<int32_t> > ignored_gt, ignored_det;  // index of ignored gt detection for current class
  vector< vector<tGroundtruth> > dontcare;            // index of dontcare areas, included in ground truth

  // for all test images do
  for (int32_t i=0; i<groundtruth.size(); i++){

    // holds ignored ground truth, ignored detections and dontcare areas for current frame
    vector<int32_t> i_gt, i_det;
    vector<tGroundtruth> dc;

    // only evaluate objects of current class and ignore occluded, truncated objects
    cleanData(current_class, groundtruth[i], detections[i], i_gt, dc, i_det, n_gt, difficulty, depth);
    ignored_gt.push_back(i_gt);
    ignored_det.push_back(i_det);
    dontcare.push_back(dc);

    // compute statistics to get recall values
    tPrData pr_tmp = tPrData();
    pr_tmp = computeStatistics(current_class, groundtruth[i], detections[i], dc, i_gt, i_det, false, boxoverlap, metric);

    // add detection scores to vector over all images
    for(int32_t j=0; j<pr_tmp.v.size(); j++)
      v.push_back(pr_tmp.v[j]);
  }

  // get scores that must be evaluated for recall discretization
  thresholds = getThresholds(v, n_gt);

  // compute TP,FP,FN for relevant scores
  vector<tPrData> pr;
  pr.assign(thresholds.size(),tPrData());
  for (int32_t i=0; i<groundtruth.size(); i++){

    // for all scores/recall thresholds do:
    for(int32_t t=0; t<thresholds.size(); t++){
      tPrData tmp = tPrData();
      tmp = computeStatistics(current_class, groundtruth[i], detections[i], dontcare[i],
                              ignored_gt[i], ignored_det[i], true, boxoverlap, metric,
                              compute_aos, thresholds[t], t==38);

      // add no. of TP, FP, FN, AOS for current frame to total evaluation for current threshold
      pr[t].tp += tmp.tp;
      pr[t].fp += tmp.fp;
      pr[t].fn += tmp.fn;
      if(tmp.similarity!=-1)
        pr[t].similarity += tmp.similarity;
    }
  }

  // compute recall, precision and AOS
  precision.assign(N_SAMPLE_PTS, 0);
  double r=0;
  for (int32_t i=0; i<thresholds.size(); i++){
    r = pr[i].tp/(double)(pr[i].tp + pr[i].fn);
    precision[i] = pr[i].tp/(double)(pr[i].tp + pr[i].fp);
  }

  // filter precision and AOS using max_{i..end}(precision)
  for (int32_t i=0; i<thresholds.size(); i++){
    precision[i] = *max_element(precision.begin()+i, precision.end());
  }

  return true;
}

void write_result(ofstream& outfile, string exp_name, vector<double> &precisions) {
  double ap = accumulate(precisions.begin() + 1, precisions.end(), 0.0) / (N_SAMPLE_PTS - 1);
  outfile << exp_name << "," << ap ;
  for (const double& prec : precisions) {
    outfile << ',' << prec;
  }
  outfile << endl;
}

void eval(string gt_dir, string result_dir, int c, bool depth, ofstream& outfile) {

  vector<vector<tGroundtruth>> groundtruths;
  vector<vector<tDetection>> detections;
  map<string, vector<vector<tGroundtruth>>> groundtruths_perseq;
  map<string, vector<vector<tDetection>>> detections_perseq;

  cout << "Loading data" << endl;
  string path;
  vector<string> sequences = list_dir(gt_dir);
  for (const auto& sequence : sequences) {
    vector<string> frames = list_dir(gt_dir + '/' + sequence);
    vector<vector<tGroundtruth>> groundtruths_seq;
    vector<vector<tDetection>> detections_seq;
    for (const auto& frame : frames) {
      string gt_path = gt_dir + '/' + sequence + '/' + frame;
      string result_path = "";
      if (depth) {
        result_path = result_dir + '/' + sequence + '/' + frame;
      } else {
        result_path = result_dir + '/' + sequence + "/image_stitched/" + frame;
      }
      vector<tGroundtruth> gt = loadGroundtruth(gt_path);
      vector<tDetection> det = loadDetection(result_path);
      groundtruths.push_back(gt);
      detections.push_back(det);
      groundtruths_seq.push_back(gt);
      detections_seq.push_back(det);
    }
    groundtruths_perseq[sequence] = groundtruths_seq;
    detections_perseq[sequence] = detections_seq;
  }
  // if (groundtruths.size() != N_TESTIMAGES)
  //   throw invalid_argument("Mismatch in number of ground truth files.");

  cout << "Loaded data" << endl;

  CLASSES cls = (CLASSES)c;

  // eval image 2D bounding boxes
  if (!depth) {
    cout << "Starting 2D evaluation (" << CLASS_NAMES[c].c_str() << ") ..." << endl;
    vector<double> precision_2d_hard;
    if (!eval_class(cls, groundtruths, detections, false, imageBoxOverlap, precision_2d_hard, IMAGE, HARD, depth)) {
      cout << CLASS_NAMES[c].c_str() << " evaluation failed." << endl;
    } else {
      write_result(outfile, "overall", precision_2d_hard);
    }
    for (auto const& groundtruths_seq : groundtruths_perseq) {
      cout << "Starting per-sequence 2D evaluation (" << groundtruths_seq.first << ", " << CLASS_NAMES[c].c_str() << ") ..." << endl;
      vector<double> precision_2d_seq;
      if (!eval_class(cls, groundtruths_seq.second, detections_perseq[groundtruths_seq.first], false, imageBoxOverlap, precision_2d_seq, IMAGE, HARD, depth)) {
        cout << CLASS_NAMES[c].c_str() << " evaluation failed." << endl;
      } else {
        write_result(outfile, groundtruths_seq.first, precision_2d_seq);
      }
    }
  } else {
    cout << "Starting 3D evaluation (" << CLASS_NAMES[c].c_str() << ") ..." << endl;
    vector<double> precision_3d_hard;
    if (!eval_class(cls, groundtruths, detections, false, box3DOverlap, precision_3d_hard, BOX3D, HARD, depth)) {
      cout << CLASS_NAMES[c].c_str() << " evaluation failed." << endl;
    } else {
      write_result(outfile, "overall", precision_3d_hard);
    }
    for (auto const& groundtruths_seq : groundtruths_perseq) {
      cout << "Starting per-sequence 3D evaluation (" << groundtruths_seq.first << ", " << CLASS_NAMES[c].c_str() << ") ..." << endl;
      vector<double> precision_3d_seq;
      if (!eval_class(cls, groundtruths_seq.second, detections_perseq[groundtruths_seq.first], false, box3DOverlap, precision_3d_seq, BOX3D, HARD, depth)) {
        cout << CLASS_NAMES[c].c_str() << " evaluation failed." << endl;
      } else {
        write_result(outfile, groundtruths_seq.first, precision_3d_seq);
      }
    }
  }
}

int32_t main (int32_t argc, char *argv[]) {
  if (argc != 5) {
    cout << "Usage: ./eval_detection gt_dir result_dir eval_type save_path" << endl;
    return 1;
  }
  initGlobals();

  vector<vector<tGroundtruth>> groundtruths;
  vector<vector<tDetection>> detections;

  bool depth = strcmp(argv[3], "0") != 0;

  // run evaluation
  ofstream outfile;
  outfile.open(argv[4]);

  eval(argv[1], argv[2], 1, depth, outfile);
  cout << "Finished evaluating" << endl;

  outfile.close();
  cout << "Saved metrics to " << argv[4] << endl;

  return 0;
}


// __BEGIN_LICENSE__
// Copyright (C) 2006-2010 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

#include <iostream>
#include <fstream>
#include <sys/stat.h>
#include <vw/Image.h>
#include <vw/Image/PixelMath.h>
#include <vw/Image/PixelMask.h>
#include <vw/Image/MaskViews.h>
#include <vw/FileIO.h>
#include <math.h>
#include <time.h>

//using namespace std;
//using namespace vw;
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/distributions/normal.hpp> // for normal_distribution
#include <boost/math/distributions/gamma.hpp>

#include <vw/Core.h>
#include <vw/Image.h>
#include <vw/FileIO.h>
#include <vw/Cartography.h>
#include <vw/Math.h>
using namespace vw;
using namespace vw::math;
using namespace vw::cartography;
#include <vw/Photometry/Reconstruct.h>
#include <vw/Photometry/Index.h>

template <class ViewT>
std::vector<float> build_histogram(ImageViewBase<ViewT> const& view) {
        typedef typename ViewT::pixel_accessor pixel_accessor;
        std::vector<float> result(DYNAMIC_RANGE);

        int num_valid = 0;
        pixel_accessor row_acc = view.impl().origin();
        for( int32 row=0; row < view.impl().rows(); ++row ) {
                pixel_accessor col_acc = row_acc;
                for( int32 col=0; col < view.impl().cols(); ++col ) {
                        if ( is_valid(*col_acc) ) {
                                result[ (*col_acc)[0] ] += 1.0;
                                ++num_valid;
                        }
                        col_acc.next_col();
                }
                row_acc.next_row();
        }

        // Renormalize the histogram values
        for (size_t i=0; i< result.size(); ++i)
                result[i] /= num_valid;

        return result;
}


#include "VoDTopologyOracle.hpp"
#include "Scheduler.hpp"
#include "boost/random/normal_distribution.hpp"
#include "UGCPopularity.hpp"
#include <boost/lexical_cast.hpp>
#include <initializer_list>
#include <boost/random/mersenne_twister.hpp>

// random generator
extern boost::mt19937 gen;

VoDTopologyOracle::VoDTopologyOracle(Topology* topo, po::variables_map vm, uint roundDuration)
    : TopologyOracle(topo, vm, roundDuration) {
  this->contentNum = vm["contents"].as<uint>();
  this->popularity = new UGCPopularity(vm["rounds"].as<uint>(), vm["perturbations"].as<bool>());
}

VoDTopologyOracle::~VoDTopologyOracle() {
  beforePeak.clear();
  atPeak.clear();
  afterPeak.clear();
}

void VoDTopologyOracle::populateCatalog() {
  // normal distribution to generate content length in minutes
  boost::random::normal_distribution<> normDist(this->avgContentLength,
          this->devContentLength);
  Capacity size = 0;
  // generate content
  for (uint i = 0; i < contentNum; i++) {
    size = std::ceil(normDist(gen) * 60 * this->bitrate);
    ContentElement* content = new ContentElement(boost::lexical_cast<string > (i),
            0, size, size);
    content->setPeakingRound(popularity->generatePeakRound());
    if (content->getPeakingRound() == 0)
      this->atPeak.push_back(content);
    else
      this->beforePeak.push_back(content);
    this->addContent(content, 0);
  }
}

void VoDTopologyOracle::generateUserViewMap(Scheduler* scheduler) {
  uint totalViews = 0;
  if (!beforePeak.empty()) {
    totalViews += popularity->generateViews(beforePeak, BEFORE_PEAK);
  }
  if (!atPeak.empty()) {
    totalViews += popularity->generateViews(atPeak, AT_PEAK);
  }
  if (!afterPeak.empty()) {
    totalViews += popularity->generateViews(afterPeak, AFTER_PEAK);
  }
  uint totalPeers = topo->getNumCustomers();
  double avgViewsPerPeer = (double) totalViews / (double) totalPeers;
  double avgHoursPerUserGen = avgViewsPerPeer * (this->avgContentLength / 60);
  if (avgHoursPerUserGen != this->avgHoursPerUser) {
    // scale content views to available customers
    std::cout << "Scaling views to ensure the average hours of view "
            "per peer per round is approximately " << this->avgHoursPerUser
            << " (current average: " << avgHoursPerUserGen << ")"
            << std::endl;
    double scale =  this->avgHoursPerUser / avgHoursPerUserGen;
    totalViews = 0;

    BOOST_FOREACH(ContentElement* content, beforePeak) {
      if (content->getViewsThisRound() > 0) {
        content->setViewsThisRound(std::max(1,
                (int) std::floor(content->getViewsThisRound() * scale)));
        totalViews += content->getViewsThisRound();
      }
    }

    BOOST_FOREACH(ContentElement* content, atPeak) {
      if (content->getViewsThisRound() > 0) {
        content->setViewsThisRound(std::max(1,
                (int) std::floor(content->getViewsThisRound() * scale)));
        totalViews += content->getViewsThisRound();
      }
    }

    BOOST_FOREACH(ContentElement* content, afterPeak) {
      if (content->getViewsThisRound() > 0) {
        content->setViewsThisRound(std::max(1,
                (int) std::floor(content->getViewsThisRound() * scale)));
        totalViews += content->getViewsThisRound();
      }
    }
    std::cout << "Total views after scaling: " << totalViews
              << ", avgViewsPerPeer: " << (double) totalViews / (double) totalPeers
              << std::endl;
  }
  // Views have been generated, now we need to map them to random users
  this->scheduleRequests(beforePeak, scheduler);
  this->scheduleRequests(atPeak, scheduler);
  this->scheduleRequests(afterPeak, scheduler);
}

// This method is not used in VoD simulations, remove it
void VoDTopologyOracle::generateNewRequest(PonUser user, SimTime time,
        Scheduler* scheduler) {
  return;
  }

void VoDTopologyOracle::updateCatalog(uint currentRound) {
  // move content that peaked last round to afterPeak
  afterPeak.splice(afterPeak.begin(), atPeak);
  // move content that peaks in the next round from beforePeak to atPeak
  std::list<ContentElement*>::iterator temp;
  std::list<ContentElement*>::iterator it = beforePeak.begin();
  while (it != beforePeak.end()) {
    if ((*it)->getPeakingRound() == currentRound + 1) {
      temp = it;
      temp++;
      atPeak.splice(atPeak.end(), beforePeak, it);
      it = temp;
    } else
      it++;
  }
}

void VoDTopologyOracle::scheduleRequests(std::list<ContentElement*> list,
                                        Scheduler* scheduler) {
  boost::random::discrete_distribution<> hourDist(usrPctgByHour);
  boost::random::discrete_distribution<> dayDist(dayWeights);
  boost::random::uniform_int_distribution<> minSecDist(0, 3599);
  std::vector<Vertex> ponNodes = this->getTopology()->getPonNodes();
  uint totUsers = topo->getNumCustomers();
  std::set<uint> assignedUsers;
  uint randUser;
  PonUser randPonUser;
  BOOST_FOREACH (ContentElement* content, list) {
    // Newest implementation: ensures that a random value is computed exactly
    // viewsThisWeek times, no more. However this doesn't check that the
    // selected user does not have this content cached. After all, cached
    // content CAN be requested for a new view and that's part of the model.

    // Check that we are going to have enough users to be mapped to requests
    uint views = content->getViewsThisRound();
    if (totUsers < views) {
      std::cout << "VoDTopologyOracle::serveRequests() - Not enough users to map all "
              << views <<
              " requests for content " << content->getName() <<
              ", truncating to " << totUsers << std::endl;
      content->setViewsThisRound(totUsers);
    }
    // Select random users to map requests
    for (uint j = totUsers - views; j < totUsers; j++) {
      boost::random::uniform_int_distribution<> userDist(0, j-1);
      randUser = userDist(gen);
      if (assignedUsers.insert(randUser).second == false) {
        randUser = j;
        auto rValue = assignedUsers.insert(randUser);
        assert(rValue.second == true);
      }
      /* this worked very well when all PONs had the same # of customers, now
       * we are forced to go through the following loop to map a planar integer
       * to the right PonUser, unless I can find a better way to deal with this.
       */
      uint t(0), oldt(0), i(0);
      while (randUser >= t) {
        oldt = t;
        t += topo->getPonCustomers(ponNodes.at(i));
        i++;
      }
      assert(topo->getPonCustomers(ponNodes.at(i - 1)) > randUser - oldt);
      randPonUser = std::make_pair(ponNodes.at(i - 1), randUser - oldt);
      // generate a random request time
      SimTime reqTime = dayDist(gen) * 86400 + // day
              hourDist(gen) * 3600 + // hour
              minSecDist(gen); // minutes and seconds
      // generate request
      // TODO: Add support for zapping in VoD too.
      Flow* req = new Flow(content, randPonUser, reqTime);
      scheduler->schedule(req);
    }
    assignedUsers.clear();

    /* New implementation: roll over the amount of users and add those already
     * chosen to a set to avoid repetitions. Hopefully faster.

    // Check that we are going to have enough users to be mapped to requests
    // TODO: might want to do this even if views do not reach this value but
    // are numerous enough that rolling for a random unassigned user becomes a
    // significant time sink
    uint unavailableUsers = oracle->getSources(content).size();
    if (totUsers -  unavailableUsers <= content->getViewsThisRound()) {
      std::cout << "TestRun::serveRequests() - Not enough users to map all "
              << content->getViewsThisRound() <<
              " requests for content " << content->getName() <<
              ", truncating to " << totUsers -  unavailableUsers << std::endl;
      content->setViewsThisRound(totUsers -  unavailableUsers);
      // assign the views sequentially rather than randomly, as we know all
      // users are going to be involved anyway
      BOOST_FOREACH(Vertex v, ponNodes) {
        for (uint i = 0; i < ponCardinality; i++) {
          // check that this user has not the content in its cache
          if (oracle->checkIfCached(std::make_pair(v,i), content) == false) {
            // generate a random request time
            SimTime reqTime = dayDist(gen) * 86400 + // day
                    hourDist(gen) * 3600 + // hour
                    minSecDist(gen); // minutes and seconds
            // generate request
            Flow* req = new Flow(content, std::make_pair(v, i), reqTime);
            scheduler->schedule(req);
          }
        }
      }
    } else { // we've got more users than requests, roll randomly
      PonUser randPonUser;
      uint ponNodeIndex, userIndex;
      for (unsigned int i = 0; i < content->getViewsThisRound(); i++) {
        // choose a random new Pon node as the requester of the content;
        // checks that the user hasn't already requested that content this round
        // and that it is not caching it
        do {
          randUser = userDist(gen);
          ponNodeIndex = std::floor(randUser / ponCardinality);
          userIndex = randUser % ponCardinality;
          randPonUser = std::make_pair(ponNodes[ponNodeIndex],userIndex);
        } while (assignedUsers.insert(randUser).second != false
                && oracle->checkIfCached(randPonUser, content));
        // generate a random request time
          SimTime reqTime = dayDist(gen) * 86400 + // day
                  hourDist(gen) * 3600 + // hour
                  minSecDist(gen); // minutes and seconds
          // generate request
          Flow* req = new Flow(content, randPonUser, reqTime);
          scheduler->schedule(req);
      }
    }
    assignedUsers.clear();
    */

    /* Old implementation - building a vector of all available PonUsers and
     removing them from the list as we select them. Terribly slow.

    // build a vector of all the available PonUsers
    std::vector<PonUser> availableUsers;
    BOOST_FOREACH(Vertex v, ponNodes) {
      for (unsigned int i = 0; i < ponCardinality; i++) {
        availableUsers.push_back(std::make_pair<unsigned int, unsigned int>(v,i));
      }
    }
    for (unsigned int i = 0; i < content->getViewsThisRound(); i++) {
      // choose a random new Pon node as the requester of the content
      boost::uniform_int<unsigned int> uniDist(0, availableUsers.size());
      unsigned int randUser = uniDist(gen);
      // generate a random request time
      SimTime reqTime = dayDist(gen) * 86400 + // day
              hourDist(gen) * 3600 + // hour
              minSecDist(gen); // minutes and seconds
      std::vector<PonUser>::iterator vIt = availableUsers.begin()+randUser;
      Request* req = new Request(reqTime, *vIt, content);
      scheduler->schedule(req);
      // erase the pon nodes from the available requesters (1 request
      // per node per content)
      availableUsers.erase(vIt);
      if (availableUsers.size() == 0) {
        // we ran out of requesters so we cannot allocate any more requests
        std::cout << "TestRun::execute() - not enough PON nodes to serve "
                "requests for content " << content->getName() << std::endl;
        break;
      }
    }
   */
  }
}


#include <boost/numeric/odeint/stepper/rosenbrock4_controller.hpp>


#include <memory>
#include <vector>

#include <Eigen/Core>
#include <glog/logging.h>
#include <gtest/gtest.h>

#include <aslam/cameras/camera-pinhole.h>
#include <aslam/cameras/distortion-fisheye.h>
#include <aslam/common/memory.h>
#include <aslam/frames/visual-frame.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>
#include <vi-map/landmark.h>
#include <vi-map/mission.h>
#include <vi-map/pose-graph.h>
#include <vi-map/vertex.h>
#include <vi-map/vi_map.pb.h>
#include <vi-map/viwls-edge.h>

namespace map_optimization_legacy {

class ViwlsGraph : public ::testing::Test {
 protected:
  typedef aslam::FisheyeDistortion DistortionType;
  typedef aslam::PinholeCamera CameraType;

  virtual void SetUp() {
    cameras_ = aslam::NCamera::createTestNCamera(kNumCameras);
  }

  aslam::NCamera::Ptr cameras_;
  static constexpr int kNumCameras = 1;
  static constexpr int kVisualFrameIndex = kNumCameras - 1;
};

TEST_F(ViwlsGraph, SimpleTest) {
  vi_map::PoseGraph posegraph;
  Aligned<std::vector, vi_map::VIMission> missions;
  vi_map::LandmarkIdList landmark_ids;

  vi_map::VIMission mission;
  vi_map::MissionId mission_id;
  mission_id.fromHexString("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa0");
  missions.push_back(mission);

  aslam::VisualFrame::DescriptorsT descriptors;
  descriptors.resize(48, 1);
  descriptors.setRandom();

  vi_map::Landmark landmark;
  vi_map::LandmarkId landmark_id;
  landmark_id.fromHexString("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa1");
  landmark.setId(landmark_id);
  landmark.set_p_B(pose::Position3D(1, 0, 0));
  landmark.set_p_B_Covariance(Eigen::Matrix<double, 3, 3>::Identity());
  landmark_ids.push_back(landmark_id);

  Eigen::Matrix<double, 6, 1> imu_bias;
  imu_bias << 1, 2, 3, 4, 5, 6;

  Eigen::Matrix<double, 2, 1> points0, points1;
  points0 << 1, 2;
  points1 << 3, 4;

  Eigen::VectorXd uncertainties0, uncertainties1;
  uncertainties0.resize(1);
  uncertainties0 << 1;
  uncertainties1.resize(1);
  uncertainties1 << 0.5;

  pose_graph::VertexId vertex0;
  pose_graph::VertexId vertex1;
  vertex0.fromHexString("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa2");
  vertex1.fromHexString("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa3");

  aslam::FrameId frame_id0;
  aslam::FrameId frame_id1;
  frame_id0.fromHexString("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa4");
  frame_id1.fromHexString("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa5");

  posegraph.addVIVertex(
      vertex0, imu_bias, points0, uncertainties0, descriptors, landmark_ids,
      mission.id(), frame_id0, 0, cameras_);
  posegraph.addVIVertex(
      vertex1, imu_bias, points1, uncertainties1, descriptors, landmark_ids,
      mission.id(), frame_id1, 1, cameras_);
  EXPECT_TRUE(posegraph.vertexExists(vertex0));
  EXPECT_TRUE(posegraph.vertexExists(vertex1));

  Eigen::Matrix<double, 6, 1> imu_data;
  imu_data.setOnes();

  // Assume starting vertex is at zero time and then each IMU measurement
  // should have a corresponding timestamp along the edge.
  Eigen::Matrix<int64_t, 1, 1> imu_timestamps;
  imu_timestamps << 1;

  pose_graph::EdgeId edge0;
  posegraph.addVIEdge(edge0, vertex0, vertex1, imu_timestamps, imu_data);
  EXPECT_TRUE(posegraph.edgeExists(edge0));

  pose_graph::VertexIdList all_vertex_ids;
  pose_graph::EdgeIdList all_edge_ids;
  posegraph.getAllVertexIds(&all_vertex_ids);
  posegraph.getAllEdgeIds(&all_edge_ids);
  EXPECT_EQ(1u, all_edge_ids.size());
  EXPECT_EQ(2u, all_vertex_ids.size());

  const vi_map::Vertex* ba_vertex =
      dynamic_cast<const vi_map::Vertex*>(posegraph.getVertexPtr(vertex0));
  CHECK(ba_vertex);  // Continuing without a valid ptr makes no sense.

  const Eigen::Matrix2Xd& image_points_distorted =
      ba_vertex->getVisualFrame(kVisualFrameIndex).getKeypointMeasurements();
  EXPECT_EQ(1, image_points_distorted.cols());
  EXPECT_NEAR_EIGEN(image_points_distorted, points0, 1e-15);

  const Eigen::VectorXd& image_points_uncertainties =
      ba_vertex->getVisualFrame(kVisualFrameIndex)
          .getKeypointMeasurementUncertainties();
  EXPECT_EQ(1, image_points_uncertainties.cols());
  EXPECT_NEAR_EIGEN(image_points_uncertainties, uncertainties0, 1e-15);
}

}  // namespace map_optimization_legacy

MAPLAB_UNITTEST_ENTRYPOINT