#include "coms.h"

    extern Globals globals;
    
    WTgeometry *origin_sphere;	    WTnode *origin_node;
    WTp3 translation;
    float xpoint = 0, zpoint = 0, point_xz[2][5];
    float x_init, z_init, x_goal, z_goal;
    float nearx, nearz, min_dist, theta, step, tol;
    int id = 0, num = 3, theta_num, iter_num, test; 
   
void generate_goals(void)
{
/* This routine generates the Initial and the Goal points for
 * the Robot. The position are tested to make sure they are in 
 * free space.
 */
    srand(time(NULL));
    
    
    test = 1;
    while(test == 1)
     {
	x_init = (float)(rand() % 120);
	z_init = (float)(rand() % 120);
	
	test = check_function(x_init, z_init);
    }
	cout << "x_init = " << x_init << "	" << "z_init = " << z_init << endl;
	
    test = 1;
    while(test == 1)
     {
	z_goal = (float)(rand() % 120);
	x_goal = (float)(rand() % 120);
	test = check_function(x_goal, z_goal);
    }
	cout << "x_goal = " << x_goal << "	" << "z_goal = " << z_goal << endl;

/*    x_init = -40.0;
    z_init = 30.0;
    
    x_goal = 65.0;
    z_goal = -35.0;		


    x_init = -60.0;
    z_init = -55.0;
    
    x_goal = 45.0;	
    z_goal = 65.0;  */	
    
	cout << "x_init = " << x_init << "	" << "z_init = " << z_init << endl;
	cout << "x_goal = " << x_goal << "	" << "z_goal = " << z_goal << endl;
	WTp3_init(translation);
	origin_sphere = WTgeometry_newsphere(1.2, 4, 8, FALSE, TRUE);
	WTgeometry_setrgb(origin_sphere, 55, 255, 25);
	origin_node = WTmovgeometrynode_new(globals.rootnode, origin_sphere);
	translation[X] = x_init;
	translation[Z] = z_init;
	WTnode_settranslation(origin_node, translation);

	WTp3_init(translation);
	origin_sphere = WTgeometry_newsphere(1.2, 4, 8, FALSE, TRUE);
	WTgeometry_setrgb(origin_sphere, 25, 25, 255);
	origin_node = WTmovgeometrynode_new(globals.rootnode, origin_sphere);
	translation[X] = x_goal;
	translation[Z] = z_goal;
	WTnode_settranslation(origin_node, translation);
 
    return;   
}


void make_path()
{
/* This is the routine that calls the function to generate the Initial and Goal 
 * points and then generates the path that the robot will follow. The generated
 * points are tested to make sure they are not in the obstacle region
 */
    FLAG skip_flag = FALSE;
    int i, j =0, counter = 0, change = 0; 
    float num1, num2, new_xpoint, new_zpoint, mult;
    float cos_1, sin_1, x1, z1;
    globals.animate_flag = FALSE;
    srand(time(NULL));
    
    min_dist = 50.0;	iter_num = 360;
    step = 3.5;		tol = 0.2 * step;
    globals.i = 0;	globals.max_iter = 500;
    globals.i_count = 0;
    
/* Randomly pick the initial and goal points
 * for the robot.
 */      
    generate_goals();

    new_xpoint = x_init;
    new_zpoint = z_init;

    globals.path.x[0] = x_init;
    globals.path.z[0] = z_init;

/* Generate the path from the initial point to the Goal point, that the
 *Robot will follow. The random points for the path are marked by small dots 
 *on the ground.
 */
    
    for (i = 1; i< globals.max_iter; i++){
      if (min_dist >= step){
	globals.i_count++;
      for(j=0;j<5;j++){
	 test = 1;
	
	 while(test == 1 && counter < 100)
	 {
	    if(i % 50 != 0){
		num1 = (float)(rand() % iter_num);
		num2 = (float)(rand() % iter_num);
		theta_num = rand() % iter_num;
		theta = ((float)theta_num * 2.0*PI)/360.0;
		cos_1 = cos(theta);
		sin_1 = sin(theta);
	    }
	    else
	    {
		num1 = x_goal;
		num2 = z_goal;
    		x1 = (float)(num1 - new_xpoint);
		z1 = (float)(num2 - new_zpoint);
		cos_1 = x1/(sqrt((x1 * x1) + (z1 * z1)));
		sin_1 = z1/(sqrt((x1 * x1) + (z1 * z1)));
	    }	

	    xpoint = new_xpoint + cos_1*step;
	    zpoint = new_zpoint + sin_1*step;	    
	    test = check_function(xpoint, zpoint);
	    counter++;  
	 }
	point_xz[0][j] = xpoint;
	point_xz[1][j] = zpoint;
	counter = 0;	
	}
	nearest_point(point_xz);
	
	globals.path.x[i] = nearx;
	globals.path.z[i] = nearz;
	new_xpoint = nearx;
	new_zpoint = nearz;
	
	if(i > 5 && abs(globals.path.z[i-1] - globals.path.z[i]) < 2.0*tol )
	{
	   
	   for(j = 0; j < 8; j++){
	     change =0;
	     mult = 1.0;
	     while(test == 1 && change < 20){
		theta_num = rand() % iter_num;
		theta = ((float)theta_num * 2.0*PI)/360.0;
		xpoint = globals.path.x[i] + 8.0*mult*step;	
		zpoint = globals.path.z[i]; // + 12*sin(theta)*abs(globals.path.z[i-1] - globals.path.z[i]);
		test = check_function(xpoint, zpoint);
		mult = mult*0.5;
		change++;
	     }
	     if (change < 20){
		globals.path.x[i] = nearx;
		globals.path.z[i] = nearz;
		new_xpoint = nearx;
		new_zpoint = nearz;
	     }
	   
	  }  	    
	    cout << "1: if statement " << endl;
	     
	}
	
	if(i > 5 && abs(globals.path.x[i-1] - globals.path.x[i]) < 2*tol &&
	    globals.path.x[i-1] - globals.path.x[i] < tol)
	{
	    for(j = 0; j < 8; j++){
	     change =0;
	     mult = 1.0;
	     while(test == 1 && change < 20){
		theta_num = rand() % iter_num;
		theta = ((float)theta_num * 2.0*PI)/360.0;
		zpoint = globals.path.z[i] + 8.0*step;
		xpoint = globals.path.x[i]; // + 12*sin(theta)*abs(globals.path.x[i-1] - globals.path.x[i]);
		test = check_function(xpoint, zpoint);
		change++;
		mult = mult*0.5;
	    }
	    if(change < 20){
		globals.path.x[i] = nearx;
		globals.path.z[i] = nearz;
		new_xpoint = nearx;
		new_zpoint = nearz;
		cout << "2: if statement " << endl; 	
	    }
	  }	
	}	    
	
		    
/* Draw the geenrated points for the path */
	
	WTp3_init(translation);
	origin_sphere = WTgeometry_newsphere(1.2, 4, 8, FALSE, TRUE);
	WTgeometry_setrgb(origin_sphere, 255, 0, 0);
	origin_node = WTmovgeometrynode_new(globals.rootnode, origin_sphere);
	translation[X] = globals.path.x[i];
	translation[Z] = globals.path.z[i];
	WTnode_settranslation(origin_node, translation);
      }
    }

/* Delete the trasparent robot used to test if there the points are in the obstacle
 * region. Draw the robot at the initial point. 
 */
    WTp3_init(translation);
    WTnode_delete(globals.nodes.origin_movsep[0]);
    make_robot();
    translation[X] = globals.path.x[0];
    translation[Z] = globals.path.z[0];
    WTnode_settranslation(globals.nodes.origin_movsep[0], translation);
}


int check_function(float x_value, float z_value)
{
/* This routine whether there is collision between the obstacles and each of the 
 * path points generated.
 */ 
 
   WTp3_init(translation);
   translation[X] = x_value;
   translation[Z] = z_value;
   
   WTnode_settranslation(globals.nodes.origin_movsep[0], translation);
  
   if(x_value > -globals.length/2.0 &&  x_value < globals.length/2.0 
	    && z_value > -globals.length/2.0 && z_value < globals.length/2.0)
   {
	    if((WTnodepath_intersectbbox(globals.nodepaths.obstacles[0], 
		    globals.nodepaths.transparent_robot) == TRUE) || 
		(WTnodepath_intersectbbox(globals.nodepaths.obstacles[1], 
		    globals.nodepaths.transparent_robot) == TRUE) ||
		(WTnodepath_intersectbbox(globals.nodepaths.obstacles[2], 
		    globals.nodepaths.transparent_robot) == TRUE) ||
		(WTnodepath_intersectbbox(globals.nodepaths.obstacles[3], 
		    globals.nodepaths.transparent_robot) == TRUE) ||
		(WTnodepath_intersectbbox(globals.nodepaths.obstacles[4], 
			globals.nodepaths.transparent_robot) == TRUE))	
			    return 1;
	    else
		return 0;	
	
   }
   else
   {    
       return 1;
   }  
    
}



void nearest_point(float points[2][5])
{   
/* This routine determines the point closest to the goal using the Euclidean
 * metric
 */
    int i, count,  steps;
    float dist[5], distx[5], distz[5];
    float start_dist, start_distxz;

    for(i = 0; i < 5; i++)
    {
      dist[i] = sqrt((x_goal - points[0][i]) *(x_goal - points[0][i]) 
		+ (z_goal - points[1][i]) * (z_goal - points[1][i]));
      distx[i] = sqrt((x_goal - points[0][i]) * (x_goal - points[0][i]));
      distz[i] = sqrt((z_goal - points[1][i]) * (z_goal - points[1][i]));

    }
    start_dist = dist[0];  start_distxz = distx[0] + distz[0];
    count = 0;
 
    for(i = 0; i < 5; i++)
      {
	if (start_dist >= dist[i])
	{
	    start_dist = dist[i];
	    count = i;
	}
     }
    
    min_dist = dist[count];
    cout << "1: min_dist = " << min_dist << endl;
    nearx =  points[0][count];
    nearz =  points[1][count]; 
  
}	



void animate_robot(void)
{
/* This is the routine that is called after the path generation is completed. The
 * animation is started by the user by putting the mouse on the graphics window
 * and press "a" on the keyboard. The robot is translated along the path to the 
 * final position generated on the path. The frames for each animation are also
 * generated and saved in rgb format.
 */
    int i;
    char str[100];
	
    translation[X] = globals.path.x[globals.i];
    translation[Z] = globals.path.z[globals.i];
    WTnode_settranslation(globals.nodes.origin_movsep[0], translation);


	sprintf(str, "scrsave /home/users/mkulima/cs576/finalp/temp/%02d_%03d.rgb 28 490 685 955",
	    num, id);
	system(str);   
	fprintf(stderr, "saving %02d_%03d.rgb\n", num, id++);
    
}