#include "Environment.h"

// data to draw robot simulator scene

Environment::Environment(void)
{

  show_objects = true;

  // slide , i.e working area of lasor scanner

  slide_base[0] = 1000;
  slide_base[1] = 1000;
  slide_base[2] = 50;

  slide_base_pos[0] = 0;
  slide_base_pos[1] = 1000;
  slide_base_pos[2] = 25;

  slide_side[0] = 1000;
  slide_side[1] = 50;
  slide_side[2] = 200;

  slide_side_pos[0] = 0;
  slide_side_pos[1] = 0;
  slide_side_pos[2] = 100;

  slide_back[0] = 2000;
  slide_back[1] = 1000;
  slide_back[2] = 50;

  slide_back_pos[0] = -1000;
  slide_back_pos[1] = 1000;
  slide_back_pos[2] = 25;

  slide_back_side[0] = 2000;
  slide_back_side[1] = 50;
  slide_back_side[2] = 200;

  slide_back_side_pos[0] = -1000;
  slide_back_side_pos[1] = 550;
  slide_back_side_pos[2] = 100;

  // support, place of available grippers 
  
  sup_base[0] = 500;
  sup_base[1] = 900;
  sup_base[2] = 50;

  sup_base_pos[0] = 1000;
  sup_base_pos[1] = 0;
  sup_base_pos[2] = 25;

  sup_1_R = 30;
  sup_1_H = 500;
  sup_1_N = 8;

  sup_1_pos[0] = 1000;
  sup_1_pos[1] = 100;
  sup_1_pos[2] = 250;

  sup_1_1[0] = 150;
  sup_1_1[1] = 80;
  sup_1_1[2] = 10;

  sup_1_1_pos[0] = 1000;
  sup_1_1_pos[1] = 150;
  sup_1_1_pos[2] = 500;

  sup_2_R = 30;
  sup_2_H = 500;
  sup_2_N = 8;

  sup_2_pos[0] = 1000;
  sup_2_pos[1] = 300;
  sup_2_pos[2] = 250;

  sup_2_1[0] = 150;
  sup_2_1[1] = 80;
  sup_2_1[2] = 10;

  sup_2_1_pos[0] = 1000;
  sup_2_1_pos[1] = 350;
  sup_2_1_pos[2] = 500;

  sup_3_R = 30;
  sup_3_H = 550;
  sup_3_N = 8;

  sup_3_pos[0] = 1000;
  sup_3_pos[1] = -100;
  sup_3_pos[2] = 275;


  // for fun 
  GLfloat color[] = { 0.8, 0.8, 0.0};
  GLfloat pose[] = { 0, 0, 0};
  // OpenGL matrix type!!!
  GLfloat frame[] = { 1,0,0,0,0,1,0,0,0,0,1,0,0,1000,150,1 }; 
  GLfloat size[] = { 200, 300, 200 };

  add_object(size,pose,frame,color);

}


// adding object into a scene
bool Environment::add_object( GLfloat *Size, 
			      GLfloat *Pose,
			      GLfloat *Frame,
			      GLfloat *Color ){
  if( object_list.size() < MAX_NUM_OBJECTS ){
    Object obj( Size, Pose, Frame, Color);
    object_list.push_back(obj);
    return true;
  }
  return false;
}

// removing object from a scene 
bool Environment::remove_object(int i){

  if( ( unsigned int ) i < object_list.size() ) {
    list<Object>::iterator it = object_list.begin();
    for( ; i != 0; it++, i--);
    object_list.erase( it);
    return true;
  }
  return false;
}
