# include "coms.h"

extern Globals globals;

WTm4 csys_xform;
int pos1_count = 0,  pos2_count = 0;
int poscount = 0;

WTnode *origin[4], *inv_origin[4];
WTmtable *geom_mat;
	

void make_robot(void)
{
    WTgeometry *y_axis;
    WTgeometry *inv_origin_block, *origin_block;
    WTnode *yaxis_node;
    WTnode *t_node[4];
    int j = 0, axis_tess;
    WTp3 translation;
    float axis_radius;
    
/*    float csys_axis_length = 12.5;
    axis_radius = 3.5;
    axis_tess = 8;
    sphere_radius = 3.5;	*/
    
    float csys_axis_length = 10.5;
    axis_radius = 1.5;
    axis_tess = 8;
    
    //float b_lx = 8.5, b_ly = 4.0, b_lz = 9.5;
//    float length = 120.0;
    float b_lx = 4.5, b_ly = 2.0, b_lz = 4.5;
    globals.length = 150.0;
    
    pos1_count = 0; 
    pos2_count = 0;
   
    
    WTmessage("1: Starting at csys ... \n");
    
    WTp3_init(translation);
    translation[X] = -globals.length/2.0 + b_lx;
    translation[Z] = globals.length/2.0 - b_lz;
    globals.nodes.origin_movsep[poscount] = WTmovsepnode_new(globals.rootnode);
    WTnode_settranslation(globals.nodes.origin_movsep[poscount], translation);
    WTnode_setname(globals.nodes.origin_movsep[poscount], "origin_movsep_node");
    
    WTp3_init(translation);
    t_node[poscount] = WTxformnode_new(globals.nodes.origin_movsep[poscount]);
    translation[Y] = -b_ly/2.0;
    WTnode_settranslation(t_node[poscount], translation);
	       
    origin_block = WTgeometry_newblock(b_lx, b_ly, b_lz , TRUE);
    WTgeometry_setrgb(origin_block, 0, 55, 55);
    origin[poscount] = WTmovgeometrynode_new(globals.nodes.origin_movsep[poscount], origin_block);
    WTnode_setname(origin[poscount], "origin_node");
    
    inv_origin_block = WTgeometry_newblock(b_lx/2.0, b_ly/2.0, b_lz/2.0 , TRUE);
    WTgeometry_setrgb(inv_origin_block, 0, 55, 55);
    WTgeometry_prebuild(inv_origin_block);
    inv_origin[poscount] = WTmovgeometrynode_new(NULL, inv_origin_block);
    WTnode_setname(inv_origin[poscount], "invisible_origin");
    WTmovnode_attach(origin[poscount], inv_origin[poscount], j);
    j++;
    
    WTp3_init(translation);
    y_axis = WTgeometry_newcylinder(csys_axis_length, axis_radius, axis_tess, FALSE, TRUE);
    WTgeometry_setrgb(y_axis, 0, 55, 55);
    WTgeometry_prebuild(y_axis);
    yaxis_node = WTmovgeometrynode_new(NULL, y_axis);
    WTnode_setname(yaxis_node, "yaxis_cylinder");
    WTmovnode_attach(origin[poscount], yaxis_node, j);
    translation[Y] = -csys_axis_length/2;
    WTnode_settranslation(yaxis_node, translation);
    j++;
    
    WTmessage("3: Ending at csys ... \n");
    
    poscount++;
}

void make_obstacles(void)
{
    WTgeometry *wall_block;
    WTnode *wall[4];
    WTgeometry *obs_block[20];
    WTnode *obs_node[20];
    int wallcount = 0;
    int obs_count = 0;
    WTp3 translation;
 
    float b_lx = 5.5, b_ly = 8.0, b_lz = 4.5;
    float w_x, w_y, w_z;
    globals.length = 150.0;	// the length of the space surrouding 
					// the obstacles/robot
    float theta_45 = 45.0*PI/180.0;
    poscount = 1;
    pos1_count = 0; 
    pos2_count = 0;
   
    
    WTmessage("1: Starting at objects ... \n");

    
/* create the surrouding (bounding) wall boundary */  
    WTp3_init(translation);    
    w_x = 1.0;
    w_y = 8.8;
    w_z = globals.length;  
    wall_block = WTgeometry_newblock(w_x, w_y, w_z , TRUE);
    WTgeometry_setrgb(wall_block, 255, 155, 55);
//    wall[wallcount] = WTmovgeometrynode_new(globals.rootnode, wall_block);
    wall[wallcount] = WTmovgeometrynode_new(globals.rootnode, wall_block);
    translation[Y] = -w_y/2.0;
    translation[X] = -w_z/2.0;
    WTnode_settranslation(wall[wallcount], translation);
    wallcount++;

    WTp3_init(translation);    
    w_x = globals.length;
    w_y = 8.8;
    w_z = 1.0;  
    wall_block = WTgeometry_newblock(w_x, w_y, w_z , TRUE);
    WTgeometry_setrgb(wall_block, 255, 155, 55);
    wall[wallcount] = WTmovgeometrynode_new(globals.rootnode, wall_block);
    translation[Y] = -w_y/2.0;
    translation[Z] =  w_x/2.0;
    WTnode_settranslation(wall[wallcount], translation);
    wallcount++;

    WTp3_init(translation);    
    w_x = 1.0;
    w_y = 8.8;
    w_z = globals.length;  
    wall_block = WTgeometry_newblock(w_x, w_y, w_z , TRUE);
    WTgeometry_setrgb(wall_block, 255, 155, 55);
    wall[wallcount] = WTmovgeometrynode_new(globals.rootnode, wall_block);
    translation[Y] = -w_y/2.0;
    translation[X] = w_z/2.0;
    WTnode_settranslation(wall[wallcount], translation);
    wallcount++;
  
    WTp3_init(translation);    
    w_x = globals.length;
    w_y = 8.8;
    w_z = 1.0;  
    wall_block = WTgeometry_newblock(w_x, w_y, w_z , TRUE);
    WTgeometry_setrgb(wall_block, 255, 155, 55);
    wall[wallcount] = WTmovgeometrynode_new(globals.rootnode, wall_block);
    translation[Y] = -w_y/2.0;
    translation[Z] = -w_x/2.0;
    WTnode_settranslation(wall[wallcount], translation);
    wallcount++;
    
/* Obstacles in the scene */
    WTp3_init(translation);       
    obs_block[obs_count] = WTgeometry_newblock(100/2.0, b_ly, b_lz*2 , TRUE);
    WTgeometry_setrgb(obs_block[obs_count], 255, 55, 55);
    //obs_node[obs_count] = WTmovgeometrynode_new(globals.rootnode, obs_block[obs_count]);
    obs_node[obs_count] = WTmovgeometrynode_new(globals.rootnode, obs_block[obs_count]);
    translation[X] = globals.length/4.0;
    translation[Y] = -b_ly/2.0;
    translation[Z] = globals.length/4.0;
    WTnode_settranslation(obs_node[obs_count], translation);
    globals.nodepaths.obstacles[obs_count] = 
		WTnodepath_new(obs_node[obs_count], globals.rootnode, 0);
    
    obs_count++;

    WTp3_init(translation);       
    obs_block[obs_count] = WTgeometry_newblock(120/2.0, b_ly, b_lz*2 , TRUE);
    WTgeometry_setrgb(obs_block[obs_count], 255, 55, 55);
    obs_node[obs_count] = WTmovgeometrynode_new(globals.rootnode, obs_block[obs_count]);
    translation[X] = -globals.length/4.0;
    translation[Y] = -b_ly/2.0;
    translation[Z] = -globals.length/4.0;
    WTnode_settranslation(obs_node[obs_count], translation);
    globals.nodepaths.obstacles[obs_count] = 
		WTnodepath_new(obs_node[obs_count], globals.rootnode, 0);
    obs_count++;

    
    WTp3_init(translation);       
    obs_block[obs_count] = WTgeometry_newblock(b_lx*2, b_ly, 110/4.0 , TRUE);
    WTgeometry_setrgb(obs_block[obs_count], 255, 55, 55);
    obs_node[obs_count] = WTmovgeometrynode_new(globals.rootnode, obs_block[obs_count]);
    translation[X] = 3*b_lx;
    translation[Y] = -b_ly/2.0;
    translation[Z] = b_lz;
    WTnode_settranslation(obs_node[obs_count], translation);
    globals.nodepaths.obstacles[obs_count] = 
		WTnodepath_new(obs_node[obs_count], globals.rootnode, 0);
    obs_count++;    
    
    WTp3_init(translation);
    obs_block[obs_count] = WTgeometry_newblock(b_lx*2.0, b_ly, 120/3.0 , TRUE);
    WTgeometry_setrgb(obs_block[obs_count], 255, 55, 55);
    WTgeometry_prebuild(obs_block[obs_count]);
    obs_node[obs_count] = WTmovgeometrynode_new(globals.rootnode, obs_block[obs_count]);
    translation[X] = -b_lx*4.0;
    translation[Y] = -b_ly/2.0;
    translation[Z] = -b_lz*2.0;
    WTnode_settranslation(obs_node[obs_count], translation);
    globals.nodepaths.obstacles[obs_count] = 
		WTnodepath_new(obs_node[obs_count], globals.rootnode, 0);
    obs_count++;

    WTp3_init(translation);
    obs_block[obs_count] = WTgeometry_newblock(b_lx, b_ly, 110/3.0 , TRUE);
    WTgeometry_setrgb(obs_block[obs_count], 255, 55, 55);
    WTgeometry_prebuild(obs_block[obs_count]);
    obs_node[obs_count] = WTmovgeometrynode_new(globals.rootnode, obs_block[obs_count]);
    translation[X] = globals.length/4.0;
    translation[Y] = -b_ly/2.0;
    translation[Z] = -globals.length/4.0;
    WTnode_rotation(obs_node[obs_count], theta_45, 0, 0, WTFRAME_LOCAL);
    WTnode_settranslation(obs_node[obs_count], translation);
    globals.nodepaths.obstacles[obs_count] = 
		WTnodepath_new(obs_node[obs_count], globals.rootnode, 0);
    obs_count++;
   
    WTmessage("3: Ending at objects ... \n");
    
    poscount++;
}

void make_transparent_robot(void)
{
    WTgeometry *y_axis;
    WTgeometry *origin_block, *inv_origin_block;
    WTnode *yaxis_node;
    WTnode *t_node[4];
    int j = 0, axis_tess;
    WTp3 translation;
    float axis_radius;
    
    float csys_axis_length = 12.5;
    axis_radius = 3.5;
    axis_tess = 8;

    
    float b_lx = 8.5, b_ly = 4.0, b_lz = 9.5;
//    float length = 120.0;
    globals.length = 150.0;    
    pos1_count = 0; 
    pos2_count = 0;
   
    
    WTmessage("1: transparent robot ... \n");
    geom_mat = WTmtable_load("geom.mat");
    WTp3_init(translation);
    translation[X] = -globals.length/2.0 + b_lx;
    translation[Z] = globals.length/2.0 - b_lz;
    globals.nodes.origin_movsep[poscount] = WTmovsepnode_new(globals.rootnode);
    WTnode_settranslation(globals.nodes.origin_movsep[poscount], translation);
    
    WTp3_init(translation);
    t_node[poscount] = WTxformnode_new(globals.nodes.origin_movsep[poscount]);
    translation[Y] = -b_ly/2.0;
    WTnode_settranslation(t_node[poscount], translation);
	       
    origin_block = WTgeometry_newblock(b_lx, b_ly, b_lz , TRUE);
    //WTgeometry_setrgb(origin_block, 0, 55, 55);
    WTgeometry_setmtable(origin_block, geom_mat);
    WTgeometry_setmatid(origin_block, 0);
    origin[poscount] = WTmovgeometrynode_new(globals.nodes.origin_movsep[poscount], origin_block);
    WTnode_setname(origin[poscount], "origin_node");
    
    inv_origin_block = WTgeometry_newblock(b_lx/2.0, b_ly/2.0, b_lz/2.0 , TRUE);
    //WTgeometry_setrgb(inv_origin_block, 0, 55, 55);
    WTgeometry_setmtable(inv_origin_block, geom_mat);
    WTgeometry_setmatid(inv_origin_block, 0);
    WTgeometry_prebuild(inv_origin_block);
    inv_origin[poscount] = WTmovgeometrynode_new(NULL, inv_origin_block);
    WTnode_setname(inv_origin[poscount], "invisible_origin");
    WTmovnode_attach(globals.nodes.origin_movsep[poscount], inv_origin[poscount], j);
    j++;
    
    WTp3_init(translation);
    y_axis = WTgeometry_newcylinder(csys_axis_length, axis_radius, axis_tess, FALSE, TRUE);
    //WTgeometry_setrgb(y_axis, 0, 55, 55);
    WTgeometry_setmtable(y_axis, geom_mat);
    WTgeometry_setmatid(y_axis, 0);
    WTgeometry_prebuild(y_axis);
    yaxis_node = WTmovgeometrynode_new(NULL, y_axis);
    WTnode_setname(yaxis_node, "yaxis_cylinder");
    WTmovnode_attach(globals.nodes.origin_movsep[poscount], yaxis_node, j);
    translation[Y] = -csys_axis_length/2;
    WTnode_settranslation(yaxis_node, translation);
    j++;
    
    globals.nodepaths.transparent_robot = 
		WTnodepath_new(globals.nodes.origin_movsep[poscount], globals.rootnode, 0);
    
    WTmessage("2: Ending transparent robot ... \n");
    
    poscount++;
}

void make_transparent_obstacles(void)
{
    WTgeometry *wall_block;
    WTnode *wall[4];
    WTgeometry *obs_block[20];
    WTnode *obs_node[20];
    int wallcount = 0;
    int obs_count = 0;
    WTp3 translation;
 
    float b_lx = 5.5, b_ly = 8.0, b_lz = 4.5;
    float w_x, w_y, w_z;
//    float length = 120.0;		// the length of the space surrouding 
    globals.length = 150.0;		// the obstacles/robot
    float theta_45 = 45.0*PI/180.0;
    poscount = 1;
    pos1_count = 0; 
    pos2_count = 0;
   
    
    WTmessage("1: Starting at objects ... \n");
    geom_mat = WTmtable_load("geom.mat");
    
/* create the surrouding (bounding) wall boundary */  
    WTp3_init(translation);    
    w_x = 1.0;
    w_y = 8.8;
    w_z = globals.length;  
    wall_block = WTgeometry_newblock(w_x, w_y, w_z , TRUE);
    //WTgeometry_setrgb(wall_block, 255, 155, 55);
    WTgeometry_setmtable(wall_block, geom_mat);
    WTgeometry_setmatid(wall_block, 1);
//    wall[wallcount] = WTmovgeometrynode_new(globals.rootnode, wall_block);
    wall[wallcount] = WTmovgeometrynode_new(globals.rootnode, wall_block);
    translation[Y] = -w_y/2.0;
    translation[X] = -w_z/2.0;
    WTnode_settranslation(wall[wallcount], translation);
    wallcount++;

    WTp3_init(translation);    
    w_x = globals.length;
    w_y = 8.8;
    w_z = 1.0;  
    wall_block = WTgeometry_newblock(w_x, w_y, w_z , TRUE);
    //WTgeometry_setrgb(wall_block, 255, 155, 55);
    WTgeometry_setmtable(wall_block, geom_mat);
    WTgeometry_setmatid(wall_block, 1);
    wall[wallcount] = WTmovgeometrynode_new(globals.rootnode, wall_block);
    translation[Y] = -w_y/2.0;
    translation[Z] =  w_x/2.0;
    WTnode_settranslation(wall[wallcount], translation);
    wallcount++;

    WTp3_init(translation);    
    w_x = 1.0;
    w_y = 8.8;
    w_z = globals.length;  
    wall_block = WTgeometry_newblock(w_x, w_y, w_z , TRUE);
    //WTgeometry_setrgb(wall_block, 255, 155, 55);
    WTgeometry_setmtable(wall_block, geom_mat);
    WTgeometry_setmatid(wall_block, 1);
    wall[wallcount] = WTmovgeometrynode_new(globals.rootnode, wall_block);
    translation[Y] = -w_y/2.0;
    translation[X] = w_z/2.0;
    WTnode_settranslation(wall[wallcount], translation);
    wallcount++;
  
    WTp3_init(translation);    
    w_x = globals.length;
    w_y = 8.8;
    w_z = 1.0;  
    wall_block = WTgeometry_newblock(w_x, w_y, w_z , TRUE);
    //WTgeometry_setrgb(wall_block, 255, 155, 55);
    WTgeometry_setmtable(wall_block, geom_mat);
    WTgeometry_setmatid(wall_block, 1);
    wall[wallcount] = WTmovgeometrynode_new(globals.rootnode, wall_block);
    translation[Y] = -w_y/2.0;
    translation[Z] = -w_x/2.0;
    WTnode_settranslation(wall[wallcount], translation);
    wallcount++;
    
/* Obstacles in the scene */
    WTp3_init(translation);       
    obs_block[obs_count] = WTgeometry_newblock(100/2.0, b_ly, b_lz*2 , TRUE);
    //WTgeometry_setrgb(obs_block[obs_count], 255, 55, 55);
    WTgeometry_setmtable(obs_block[obs_count], geom_mat);
    WTgeometry_setmatid(obs_block[obs_count], 1);
    //obs_node[obs_count] = WTmovgeometrynode_new(globals.rootnode, obs_block[obs_count]);
    obs_node[obs_count] = WTmovgeometrynode_new(globals.rootnode, obs_block[obs_count]);
    translation[X] = globals.length/4.0;
    translation[Y] = -b_ly/2.0;
    translation[Z] = globals.length/4.0;
    WTnode_settranslation(obs_node[obs_count], translation);
    globals.nodepaths.obstacles[obs_count] = 
		WTnodepath_new(obs_node[obs_count], globals.rootnode, 0);
    
    obs_count++;

    WTp3_init(translation);       
    obs_block[obs_count] = WTgeometry_newblock(120/2.0, b_ly, b_lz*2 , TRUE);
    //WTgeometry_setrgb(obs_block[obs_count], 255, 55, 55);
    WTgeometry_setmtable(obs_block[obs_count], geom_mat);
    WTgeometry_setmatid(obs_block[obs_count], 1);
    obs_node[obs_count] = WTmovgeometrynode_new(globals.rootnode, obs_block[obs_count]);
    translation[X] = -globals.length/4.0;
    translation[Y] = -b_ly/2.0;
    translation[Z] = -globals.length/4.0;
    WTnode_settranslation(obs_node[obs_count], translation);
    globals.nodepaths.obstacles[obs_count] = 
		WTnodepath_new(obs_node[obs_count], globals.rootnode, 0);
    obs_count++;

    
    WTp3_init(translation);       
    obs_block[obs_count] = WTgeometry_newblock(b_lx*2, b_ly, 110/4.0 , TRUE);
    //WTgeometry_setrgb(obs_block[obs_count], 255, 55, 55);
    WTgeometry_setmtable(obs_block[obs_count], geom_mat);
    WTgeometry_setmatid(obs_block[obs_count], 1);
    obs_node[obs_count] = WTmovgeometrynode_new(globals.rootnode, obs_block[obs_count]);
    translation[X] = 3*b_lx;
    translation[Y] = -b_ly/2.0;
    translation[Z] = b_lz;
    WTnode_settranslation(obs_node[obs_count], translation);
    globals.nodepaths.obstacles[obs_count] = 
		WTnodepath_new(obs_node[obs_count], globals.rootnode, 0);
    obs_count++;    
    
    WTp3_init(translation);
    obs_block[obs_count] = WTgeometry_newblock(b_lx*2.0, b_ly, 120/3.0 , TRUE);
    //WTgeometry_setrgb(obs_block[obs_count], 255, 55, 55);
    WTgeometry_setmtable(obs_block[obs_count], geom_mat);
    WTgeometry_setmatid(obs_block[obs_count], 1);
    WTgeometry_prebuild(obs_block[obs_count]);
    obs_node[obs_count] = WTmovgeometrynode_new(globals.rootnode, obs_block[obs_count]);
    translation[X] = -b_lx*4.0;
    translation[Y] = -b_ly/2.0;
    translation[Z] = -b_lz*2.0;
    WTnode_settranslation(obs_node[obs_count], translation);
    globals.nodepaths.obstacles[obs_count] = 
		WTnodepath_new(obs_node[obs_count], globals.rootnode, 0);
    obs_count++;

    WTp3_init(translation);
    obs_block[obs_count] = WTgeometry_newblock(b_lx, b_ly, 110/3.0 , TRUE);
    //WTgeometry_setrgb(obs_block[obs_count], 255, 55, 55);
    WTgeometry_setmtable(obs_block[obs_count], geom_mat);
    WTgeometry_setmatid(obs_block[obs_count], 1);
    WTgeometry_prebuild(obs_block[obs_count]);
    obs_node[obs_count] = WTmovgeometrynode_new(globals.rootnode, obs_block[obs_count]);
    translation[X] = globals.length/4.0;
    translation[Y] = -b_ly/2.0;
    translation[Z] = -globals.length/4.0;
    WTnode_rotation(obs_node[obs_count], theta_45, 0, 0, WTFRAME_LOCAL);
    WTnode_settranslation(obs_node[obs_count], translation);
    globals.nodepaths.obstacles[obs_count] = 
		WTnodepath_new(obs_node[obs_count], globals.rootnode, 0);
    obs_count++;
    cout << "obs_count = " << obs_count << endl;
   
    WTmessage("3: Ending at objects ... \n");
    
    poscount++;
}