///////////////////////////////////////////////////////////////////////////////
//                                                                           //
// Copyright (C) 2006 by Intel Corporation and Carnegie Mellon University    //
// Contacts: casey.j.helfrich @ intel.com                                    //
//           bdr @ cs.cmu.edu                                                //
//                                                                           //
///////////////////////////////////////////////////////////////////////////////

#include <iostream>
#include <fstream>
#include <ext/hash_set>

#include "CatomWorld.hxx"
#include "CatomSim.hxx"
#include "StateFile/StateFile.hxx"
#include "graphics/graphics.h"
#include "graphics/internal.h"

pthread_mutex_t CatomWorld::worldLock = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t CatomWorld::logLock = PTHREAD_MUTEX_INITIALIZER;
extern bool NC_Oracles;
extern bool NC_AllThreads;

unsigned int CatomWorld::lattice_type;
unsigned int CatomWorld::num_neighbors;
unsigned int CatomWorld::num_features;

#define myASSERT( c ) \
   if (!(c)) { cerr << "ASSERT FAILED: " << #c << "\n"; exit(1); }



///////////////////////////////////////////////////////////////////////////////
// CatomWorld::CatomWorld()
//

CatomWorld::CatomWorld() : 
  worldFile("not_initialized"),
  experimentFile(""),
  seed(314159),
  pov_visualize(false),
  res_x("640"),
  res_y("480"),
  camera("0,180,-200"),
  camera_look("80,100,0"),
  save_pov(false),
  save(0),
  timesteps(0),
  use_physics(false),
  use_cylinder(false),
  use_faceted(false),
  gravity(0),
  bounce(0.85),
  friction(dInfinity),
  ground_friction(0),
  use_ground_friction(false),
  show_features(false),
  phys_time_step(0.05),
  sim_time_step(0.01),
  num_catoms(0),
  current_time(0) {

  // Initialize lattice variables
  get_lattice_info( "cube", &lattice_type, &num_features, &num_neighbors );
  
  // Initialize persistent file and (_sim) module
  myModule = StateFile::Module::getInstance( "_sim" );
  myModule->setVersion( 2 );
  myModule->setVersionRange( 1 );
  myModule->setContentProfile( "CCCCCCCC" );
  myModule->setContentNames( "x:y:z:r:g:b:a:bat" );
  myModule->setContent( new StateFile::ByteData32() );
  myFile.registerModule( myModule );
  
  // Create physics world and spaces
  dyn_world = dWorldCreate();
  dWorldSetAutoDisableFlag( dyn_world, 0 );
  coll_space = dHashSpaceCreate(0);
  neigh_space = dHashSpaceCreate(0);
  mag_space = dHashSpaceCreate(0);

  threadStart = NULL;
  getView = NULL;
}

///////////////////////////////////////////////////////////////////////////////
// CatomWorld::~CatomWorld()
//
// The destructor clean up used memory.

CatomWorld::~CatomWorld() {

  hash_map<const unsigned long, CatomSim *, 
    hash<const unsigned long>, equl>::iterator pos;  
  for( pos = catomHash.begin(); pos != catomHash.end(); pos++ ) 
    delete (*pos).second;
  
  dSpaceDestroy( mag_space );
  dSpaceDestroy( coll_space );
  dWorldDestroy( dyn_world );
  dCloseODE();

  dprdebug_close();
}


///////////////////////////////////////////////////////////////////////////////
// CatomWorld::loadExperiment()
//
// Load the file associated with the value stored in 
// CatomWorld->experimentFile and fill in key value pairs for experiment

bool CatomWorld::loadExperiment( vector<string> &options ) {

  unsigned int pos;
  string line, key, value;
  char *endptr;
  
  vector<string>::iterator it;
  for ( it = options.begin(); it != options.end(); it++ ) {
    line = *it;
    
    // Ignore blank line and comment line
    if( line == "" || line[0] == '#' || line == " " ) {
      continue;
    }
    
    if( (pos = line.find('=', 0)) != string::npos ) {
      key = line.substr( 0, pos );
      value = line.substr( pos+1 );
      
      if( key == "TIMESTEPS" ) {
	timesteps = strtoul( value.c_str(), &endptr, 0 );
      }
      else if( key == "SAVE" ) {
	save = strtoul( value.c_str() , &endptr, 0 );
      }
      else if( key == "SEED" ) {
	seed = strtoul( value.c_str() , &endptr, 0 );
	srand(seed);
      }
      else if( key == "VISUALIZE" ) {
	if( value == "true" )
	  pov_visualize = true;
	else
	  pov_visualize = false;
      }
      else if ( key=="THREAD_MODEL" ) {
        if( value=="concurrent" || value=="normal" ) {
	  NC_Oracles = false; NC_AllThreads=false;
	} else if ( value=="nc_oracles" ) {
	  NC_Oracles = true; NC_AllThreads=false;
	} else if ( value=="nc_all" ) {
	  NC_Oracles = true; NC_AllThreads=true;
	} else if ( value=="deterministic" ) {
	  cerr << "Warning: deterministic threading not yet supported\n"; 
	  NC_Oracles = true; NC_AllThreads=true;
	} else {
	  NC_Oracles = false; NC_AllThreads=false;
	}
      }
      else if( key == "RESOLUTIONX" ) {
	res_x = value;
      }
      else if( key == "RESOLUTIONY" ) {
	res_y = value;
      }
      else if( key == "LATTICE" ) {
	featureMapType = value;
	get_lattice_info( value, &lattice_type, &num_features, &num_neighbors );
      }
      else if( key == "SAVEPOV" ) {
	if( value == "true" )
	  save_pov = true;
	else
	  save_pov = false;
      }
      else if( key == "USE_PHYSICS" ) {
	if( value == "true" )
	  use_physics = true;
	else
	  use_physics = false;
      }
      else if( key == "SHOW_FEATURES" ) {
	if( value == "true" )
	  show_features = true;
	else
	  show_features = false;
      }
      else if( key == "CYLINDRICAL" ) {
	if( value == "true" ) {
	  use_cylinder = true;
	  use_faceted = false;
	} else
	  use_cylinder = false;
      }
      else if ( key == "FACETED" ) {
        if( value == "true" ) {
	  use_faceted = true;
	  use_cylinder = false;
	} else 
	  use_faceted = false;
      }
      else if( key == "GRAVITY" ) {
	gravity = strtod( value.c_str(), &endptr );
      }
      else if( key == "FRICTION" ) {
	if ( value == "infinity" )
	  friction = dInfinity;
	else
	  friction = strtod( value.c_str(), &endptr );
      }
      else if( key == "GROUND_FRICTION" ) {
	use_ground_friction = true;
	if ( value == "infinity" )
	  ground_friction = dInfinity;
	else
	  ground_friction = strtod( value.c_str(), &endptr );
      }
      else if( key == "BOUNCE" ) {
	bounce = strtod( value.c_str(), &endptr );
      }
      else if( key == "PHYSICS_TIME_STEP" ) {
	phys_time_step = strtod( value.c_str(), &endptr );
      }
      else if( key == "SIM_TIME_STEP" ) {
	sim_time_step = strtod( value.c_str(), &endptr );
      }
      else if( key == "CATOM_MASS" ) {
	CatomSim::catom_mass = strtod( value.c_str(), &endptr );
      }
      else if( key == "CATOM_RADIUS" ) {
	CatomSim::catom_radius = strtod( value.c_str(), &endptr );
      }
      else if( key == "MAGNET_DEPTH" ) {
	CatomSim::magnet_depth = strtod( value.c_str(), &endptr );
      }
      else if( key == "MAGNET_MAX_FORCE" ) {
	CatomSim::magnet_max_force = strtod( value.c_str(), &endptr );
      }
      else if( key == "MAGNET_MAX_RADIUS" ) {
	CatomSim::magnet_radius = strtod( value.c_str(), &endptr );
      }
      else if( key == "MODULES" ) {
	modules_list = value;
      }
      else if( key == "CAMERA" ) {
	camera = value;
      }
      else if( key == "CAMERALOOK" ) {
	camera_look = value;
      }
      else if( key == "WORLD" ) {
	if ( (value.length()>0 && value[0]=='/') ||
	     (value.length()>1 && value[0]=='.' && value[1]=='/') )
	  worldFile = value;
	else
	  worldFile = experimentFile.substr( 0, experimentFile.find_last_of('/') + 1 ) + value;
      }
      
      // Support for static, non-Catom objects
      else if( key == "OBJECT" ) {	
	unsigned int pos, lastpos;
	const string delimiter = ",";
	const string format = "%=0^+!#7.0n";
	vector<string> objectTokens;
	char *endptr;
	string objectType;
	double x, y, z;
	double qx,qy,qz;
	uint8 red, green, blue, alpha;
	
	// Tokenize the line of Object data
	lastpos = value.find_first_not_of( delimiter, 0 );
	pos = value.find_first_of( delimiter, lastpos );
	
	while(( pos != string::npos || lastpos != string::npos ) && (pos != lastpos)) {
	  objectTokens.push_back( value.substr( lastpos, pos-lastpos ) );
	  lastpos = value.find_first_not_of( delimiter, pos );
	  pos = value.find_first_of( delimiter, lastpos );
	}
	
	if( objectTokens.size() < 10 ) {
	  cerr << "Experiment File appears invalid..." << endl;
	  exit(1);
	}
	
	objectType = objectTokens[0].c_str();
	x = strtod( (objectTokens[1].c_str()), &endptr );
	y = strtod( (objectTokens[2].c_str()), &endptr );
	z = strtod( (objectTokens[3].c_str()), &endptr );
	qx = strtod( (objectTokens[4].c_str()), &endptr );
	qy = strtod( (objectTokens[5].c_str()), &endptr );
	qz = strtod( (objectTokens[6].c_str()), &endptr );
	red = (uint8)strtod( (objectTokens[7].c_str()), &endptr );
	green = (uint8)strtod( (objectTokens[8].c_str()), &endptr );
	blue = (uint8)strtod( (objectTokens[9].c_str()), &endptr );
	alpha = (uint8)strtod( (objectTokens[10].c_str()), &endptr );
	
	myASSERT( objectTokens.size() > 11 );
	bool need_body=false;
	dBodyID newbody;
	dGeomID newshape=0;
	dQuaternion Q;
	dMass mass_struct;
	double mass = strtod( (objectTokens[11].c_str()), &endptr );
	dQFromAxisAndAngle( Q, qx, qy, qz, dSqrt( qx*qx + qy*qy + qz*qz ) );
	
	// Create shapes
	if ( objectType == "SPHERE" ) {
	  myASSERT( objectTokens.size() == 13 );
	  double radius = strtod( (objectTokens[12].c_str()), &endptr );
	  newshape = dCreateSphere( coll_space, radius );
	  dMassSetSphereTotal( &mass_struct, mass, radius );
	}
	else if ( objectType == "CYL" || objectType == "CYLINDER" ) {
	  myASSERT( objectTokens.size() == 14 );
	  double radius = strtod( (objectTokens[12].c_str()), &endptr );
	  double length = strtod( (objectTokens[13].c_str()), &endptr );
	  newshape = dCreateCylinder( coll_space, radius, length );
	  dMassSetCylinderTotal( &mass_struct, mass, 3, radius, length );
	}
	else if ( objectType == "CCYL" || objectType == "CCYLINDER" ) {
	  myASSERT( objectTokens.size() == 14 );
	  double radius = strtod( (objectTokens[12].c_str()), &endptr );
	  double length = strtod( (objectTokens[13].c_str()), &endptr );
	  newshape = dCreateCCylinder( coll_space, radius, length );
	  dMassSetCappedCylinderTotal( &mass_struct, mass, 3, radius, length );
	}
	else if ( objectType == "BOX" ) {
	  myASSERT( objectTokens.size() == 15 );
	  double lx = strtod( (objectTokens[12].c_str()), &endptr );
	  double ly = strtod( (objectTokens[13].c_str()), &endptr );
	  double lz = strtod( (objectTokens[14].c_str()), &endptr );
	  newshape = dCreateBox( coll_space, lx, ly, lz );
	  dMassSetBoxTotal( &mass_struct, mass, lx, ly, lz );
	}
	else if ( objectType == "PLANE" ) {
	  myASSERT( objectTokens.size() == 16 );
	  double a = strtod( (objectTokens[12].c_str()), &endptr );
	  double b = strtod( (objectTokens[13].c_str()), &endptr );
	  double c = strtod( (objectTokens[14].c_str()), &endptr );
	  double d = strtod( (objectTokens[15].c_str()), &endptr );
	  newshape = dCreatePlane( coll_space, a, b, c, d );
	  other_objects[newshape] = 0;
	  newshape = 0;
	}
	// create body as needed and set parameters
	if (newshape) {
	  if ( need_body ) {
	    newbody = dBodyCreate( dyn_world );
	    dBodySetPosition( newbody, x, y, z );
	    dBodySetQuaternion( newbody, Q );
	    dBodySetMass( newbody, &mass_struct );
	    dGeomSetBody( newshape, newbody );
	  } else {
	    dGeomSetPosition( newshape, x, y, z );
	    dGeomSetQuaternion( newshape, Q );
	  }
	  object_color* obj_col = new object_color;
	  obj_col->red = red;
	  obj_col->green = green;
	  obj_col->blue = blue;
	  obj_col->alpha = alpha;
	  other_objects[newshape] = obj_col;
	}
	objectTokens.clear();
      }

      else {
	cout << "DPRSim: Adding (" << key << " = " << value 
	     << ") to CatomWorld::key_value_list." << endl;
	pair<string,string> entry;
	entry.first = key;
	entry.second = value;
	key_value_list.push_back( entry );
      }
    }
  }
  
  // Quick error checking
  if( worldFile == "not_initialized" ) {
    cerr << "ERROR: a world file has not been specified in the "
	 << "experiment file nor the commandline\n" << endl;
    exit(1);
  }
  
  return true;
}


///////////////////////////////////////////////////////////////////////////////
// CatomWorld::loadWorld()
//
// Load the file associated with the value stored in CatomWorld->worldFile and
// create all of the CatomSim objects, threads, and Code Modules.

bool CatomWorld::loadWorld() {

  fstream fs_world;
  string line;
  unsigned int pos, lastpos;
  const string delimiter = ",";
  const string format = "%=0^+!#7.0n";
  vector<string> catomTokens;
  char *endptr;

  // Local temp variable for the formation of Catom Objects
  unsigned long GUID;
  double x, y, z;
  double qx,qy,qz;
  uint8 red, green, blue, alpha;

  // Open the world file
  fs_world.open( worldFile.c_str(), ios::in );
  if( !fs_world.good() ) {
    cerr << "ERROR: File not found: " << worldFile << endl;
    exit(1);
  }

  /////////////////////////////////////////////////////////////////////////////
  // READ THE MAGIC NUMBER HERE
  // If it is a CLAY file, do the binary load, else continue below
  // with the text based .dpr file load

  uint32 tempMagic = 0;
  fs_world.get( (char*)&tempMagic, 5 );
  if ( ntohl( tempMagic ) == StateFile::MagicNumber ) {
    fs_world.close();
    myFile.fileOpen( (char*)(worldFile.c_str()) );

    cout << "DPRSim: Registering Code Module: _sim" << endl;

    // Create a temporary CatomSim in order to initialize
    // the module_list creation
    CatomSim *t = new CatomSim();
    // Allocate the code modules on the CatomSim
    t->loadCodeModules( modules_list );

    // Based on the modules_list from the experiment File,
    // setup myFile::outModules_
    vector<CodeModule*>::iterator i;
    StateFile::Module* handle;

    for ( i = t->codeModules.begin(); i != t->codeModules.end(); i++ ) {
      handle = (*i)->StateFileConstructor();
      if( handle != NULL ) {
	cout << "DPRSim: Registering Code Module: " 
	     << (handle->getName()) << endl;
	myFile.registerModule(handle);
      }
      startCodeModuleOracle( *i );
    }

    // Continue loading the StateFile
    myFile.fileRead_ModuleHeader();

    GUID = myFile.fileRead_Content();
    StateFile::ByteData32 *bd32 = myModule->getContent();
    int version = myModule->getVersion();
    Point3D p(0,0,0);
    float xPos, yPos, zPos, xVel, yVel, zVel; 
    float xQuat, yQuat, zQuat, aQuat, xAVel, yAVel, zAVel;

    while ( bd32 != NULL and GUID > 0 ) {
      switch(version) {
      case 1: {
	uint8* data = bd32->getData();	
	p.setX(data[0]);
	p.setY(data[1]);
	p.setZ(data[2]);
	qx = qy = qz = 0;
	red = data[3];
	green = data[4];
	blue = data[5];
	alpha = data[6];
	break;
      }
      case 2: {
	uint8* data = bd32->getData();
	
	unsigned char* bp;
	bp = (unsigned char*)&xPos;
	bp[0] = data[0];
	bp[1] = data[1];
	bp[2] = data[2];
	bp[3] = data[3];
	bp = (unsigned char*)&yPos;
	bp[0] = data[4];
	bp[1] = data[5];
	bp[2] = data[6];
	bp[3] = data[7];
	bp = (unsigned char*)&zPos;
	bp[0] = data[8];
	bp[1] = data[9];
	bp[2] = data[10];
	bp[3] = data[11];

	// POSITION	
	p.setX(xPos);
	p.setY(yPos);
	p.setZ(zPos);
	qx = qy = qz = 0;
	red = data[12];
	green = data[13];
	blue = data[14];
	alpha = data[15];

	// VELOCITY
	bp = (unsigned char*)&xVel;
	bp[0] = data[17];
	bp[1] = data[18];
	bp[2] = data[19];
	bp[3] = data[20];
	bp = (unsigned char*)&yVel;
	bp[0] = data[21];
	bp[1] = data[22];
	bp[2] = data[23];
	bp[3] = data[24];
	bp = (unsigned char*)&zVel;
	bp[0] = data[25];
	bp[1] = data[26];
	bp[2] = data[27];
	bp[3] = data[28];

	// ANGULAR VELOCITY
	bp = (unsigned char*)&xAVel;
	bp[0] = data[29];
	bp[1] = data[30];
	bp[2] = data[31];
	bp[3] = data[32];
	bp = (unsigned char*)&yAVel;
	bp[0] = data[33];
	bp[1] = data[34];
	bp[2] = data[35];
	bp[3] = data[36];
	bp = (unsigned char*)&zAVel;
	bp[0] = data[37];
	bp[1] = data[38];
	bp[2] = data[39];
	bp[3] = data[40];

	// ROTATION
	bp = (unsigned char*)&xQuat;
	bp[0] = data[41];
	bp[1] = data[42];
	bp[2] = data[43];
	bp[3] = data[44];
	bp = (unsigned char*)&yQuat;
	bp[0] = data[45];
	bp[1] = data[46];
	bp[2] = data[47];
	bp[3] = data[48];
	bp = (unsigned char*)&zQuat;
	bp[0] = data[49];
	bp[1] = data[50];
	bp[2] = data[51];
	bp[3] = data[52];
	bp = (unsigned char*)&aQuat;
	bp[0] = data[53];
	bp[1] = data[54];
	bp[2] = data[55];
	bp[3] = data[56];

	break;
      }
      }

      // Declare the CatomSim object      
      CatomSim *c = new CatomSim(GUID,p,qx,qy,qz,red,green,blue,alpha);

      if (version == 2) {
	// Set the velocity
	dBodySetLinearVel(c->body, xVel, yVel, zVel);
	// Set the angular velocity
	dBodySetAngularVel(c->body, xAVel, yAVel, zAVel);
	// Set the rotation
	dQuaternion Q;
	Q[0] = xQuat;
	Q[1] = yQuat;
	Q[2] = zQuat;
	Q[3] = aQuat;
	dBodySetQuaternion(c->body, Q);
      }
	
      // Allocate the code modules on the CatomSim
      c->loadCodeModules( modules_list );

      // Load the CodeModule's StateFile Modules
      for ( i = c->codeModules.begin(); i != c->codeModules.end(); i++ ) {
	(*i)->StateFileConstructor();
	(*i)->loadModule();
      }

      // Store pointer to Catom object in world.catomHash
      catomHash[GUID] = c;

      // Increment the number of Catoms in the world
      num_catoms++;

      // Add this Catom to the thread scheduler
      thread_schedule.push_back(GUID);

      GUID = myFile.fileRead_Content();
      bd32 = myModule->getContent();
    }
    myFile.fileClose();
  }


  /////////////////////////////////////////////////////////////////////////////
  // This code will execute if the magic number did not match.
  // We assume that the file is a .dpr ascii text world file

  else {
    fs_world.seekg( 0, ios::beg );

    // Create a temporary CatomSim in order to initialize
    // the module_list creation
    CatomSim *t = new CatomSim();
    // Allocate the code modules on the CatomSim
    t->loadCodeModules( modules_list );

    // Based on the modules_list from the experiment File,
    // setup myFile::outModules_
    vector<CodeModule*>::iterator i;

    for ( i = t->codeModules.begin(); i != t->codeModules.end(); i++ ) {
      startCodeModuleOracle( *i );
    }

    while( fs_world.good() ) {
      getline( fs_world, line );
      if ( ! fs_world ) break;

      // Ignore blank line and comment line
      if( line == "" || line[0] == '#' || line == " " ) {
	continue;
      }

      // Tokenize the line of Catom data
      lastpos = line.find_first_not_of( delimiter, 0 );
      pos = line.find_first_of( delimiter, lastpos );

      while(( pos != string::npos || lastpos != string::npos ) && (pos != lastpos)) {
	catomTokens.push_back( line.substr( lastpos, pos-lastpos ) );
	lastpos = line.find_first_not_of( delimiter, pos );
	pos = line.find_first_of( delimiter, lastpos );
      }

      if( catomTokens.size() < 10 ) {
	cerr << "World File appears invalid..." << endl;
	exit(1);
      }

      // Parse the tokens and create the Catom object
      GUID = strtoul( (catomTokens[0].c_str()), &endptr, 0 );
      x = strtod( (catomTokens[1].c_str()), &endptr );
      y = strtod( (catomTokens[2].c_str()), &endptr );
      z = strtod( (catomTokens[3].c_str()), &endptr );
      Point3D p(x,y,z);    
      qx = strtod( (catomTokens[4].c_str()), &endptr );
      qy = strtod( (catomTokens[5].c_str()), &endptr );
      qz = strtod( (catomTokens[6].c_str()), &endptr );
      red = (uint8)strtod( (catomTokens[7].c_str()), &endptr ) * 255;
      green = (uint8)strtod( (catomTokens[8].c_str()), &endptr ) * 255;
      blue = (uint8)strtod( (catomTokens[9].c_str()), &endptr ) * 255;
      alpha = (uint8)strtod( (catomTokens[10].c_str()), &endptr ) * 255;

      // TEMPORARY HACK TO REMAIN COMPATIBLE WITH OLD .DPR FILES !!!
      if (qx==1 && qy==1 && qz==3) qx=qy=qz=0;

      // Non-catom objects
      if ( GUID==0 ) {
	myASSERT( catomTokens.size() > 11 );
	bool need_body=true;
	if ( catomTokens[0].c_str()[0] == 's' ) need_body=false;
	dBodyID newbody;
	dGeomID newshape=0;
	dQuaternion Q;
	dMass mass_struct;
	double mass = strtod( (catomTokens[11].c_str()), &endptr );
	dQFromAxisAndAngle( Q, qx, qy, qz, dSqrt( qx*qx + qy*qy + qz*qz ) );
	// create shape
	if ( catomTokens[0] == "SPHERE" || catomTokens[0]=="sSPHERE" ) {
	  myASSERT( catomTokens.size() == 13 );
	  double radius = strtod( (catomTokens[12].c_str()), &endptr );
	  newshape = dCreateSphere( coll_space, radius );
	  dMassSetSphereTotal( &mass_struct, mass, radius );
	}
	else if ( catomTokens[0] == "CYL" || catomTokens[0]=="sCYL" ) {
	  myASSERT( catomTokens.size() == 14 );
	  double radius = strtod( (catomTokens[12].c_str()), &endptr );
	  double length = strtod( (catomTokens[13].c_str()), &endptr );
	  newshape = dCreateCylinder( coll_space, radius, length );
	  dMassSetCylinderTotal( &mass_struct, mass, 3, radius, length );
	}
	else if ( catomTokens[0] == "CCYL" || catomTokens[0]=="sCCYL" ) {
	  myASSERT( catomTokens.size() == 14 );
	  double radius = strtod( (catomTokens[12].c_str()), &endptr );
	  double length = strtod( (catomTokens[13].c_str()), &endptr );
	  newshape = dCreateCCylinder( coll_space, radius, length );
	  dMassSetCappedCylinderTotal( &mass_struct, mass, 3, radius, length );
	}
	else if ( catomTokens[0] == "BOX" || catomTokens[0]=="sBOX" ) {
	  myASSERT( catomTokens.size() == 15 );
	  double lx = strtod( (catomTokens[12].c_str()), &endptr );
	  double ly = strtod( (catomTokens[13].c_str()), &endptr );
	  double lz = strtod( (catomTokens[14].c_str()), &endptr );
	  newshape = dCreateBox( coll_space, lx, ly, lz );
	  dMassSetBoxTotal( &mass_struct, mass, lx, ly, lz );
	}
	else if ( catomTokens[0] == "PLANE" ) {
	  myASSERT( catomTokens.size() == 16 );
	  double a = strtod( (catomTokens[12].c_str()), &endptr );
	  double b = strtod( (catomTokens[13].c_str()), &endptr );
	  double c = strtod( (catomTokens[14].c_str()), &endptr );
	  double d = strtod( (catomTokens[15].c_str()), &endptr );
	  newshape = dCreatePlane( coll_space, a, b, c, d );
	  other_objects[newshape] = 0;
	  newshape = 0;
	}
	// create body as needed and set parameters
	if (newshape) {
	  if ( need_body ) {
	    newbody = dBodyCreate( dyn_world );
	    dBodySetPosition( newbody, x, y, z );
	    dBodySetQuaternion( newbody, Q );
	    dBodySetMass( newbody, &mass_struct );
	    dGeomSetBody( newshape, newbody );
	  } else {
	    dGeomSetPosition( newshape, x, y, z );
	    dGeomSetQuaternion( newshape, Q );
	  }
	  object_color* obj_col = new object_color;
	  obj_col->red = red;
	  obj_col->green = green;
	  obj_col->blue = blue;
	  obj_col->alpha = alpha;
	  other_objects[newshape] = obj_col;
	}
	catomTokens.clear();
	continue;
      }

      catomTokens.clear();

      // Declare the CatomSim object
      CatomSim *c = new CatomSim(GUID,p,qx,qy,qz,red,green,blue,alpha);

      // Allocate the code modules on the Catom
      c->loadCodeModules( modules_list );

      // Store pointer to Catom object in world.catomHash
      catomHash[GUID] = c;

      // Add this Catom to the thread scheduler
      thread_schedule.push_back(GUID);

      // Increment the number of Catoms in the world
      num_catoms++;
    }
    fs_world.close();
  }

  return true;
}


///////////////////////////////////////////////////////////////////////////////
// CatomWorld::saveWorld()
//
// Creates a new binary .world file, whose name is based on the loaded world
// and the timestep that the simulator is curently executing.

bool CatomWorld::saveWorld() {

  string outFile, cmd;
  unsigned int pos = worldFile.find_last_of('.');

  char buf[MAX_NUM_DIGITS];
  const string format = "%=0^+!#7.0n";

  /////////////////////////////
  // Create the proper filename

  // Special case to handle the _end file
  if( current_time > timesteps ) {
    outFile = worldFile.substr(0,pos) + "_end";
  }
  else {
    // Create new filename
    if ((snprintf(buf, sizeof(buf), " %7.7ld",
		current_time + 1)) < 0) {
      cerr << "Error: unable to create file name :: " 
	   << strerror(errno) << endl;
      exit(0);
    }
    outFile = worldFile.substr(0, pos) + "_" + (buf+1);
  }

  ofstream povfile((outFile + ".pov").c_str());

  // Open the file for writing
  myFile.fileOpen( (char*)(outFile + ".world").c_str(), true );

  // If appropriate, write out the povRay header to the temp pov file    
  if( pov_visualize ) { 
    if ( povfile.is_open() ) {      
      povfile << "#version 3.6;\n"
	      << "#include \"colors.inc\"\n\n"
	      << "global_settings { assumed_gamma 1.0 "
	      << "max_trace_level 5 }\n\n"
	      << "sky_sphere { pigment { gradient y color_map { "
	      << "[0.0 rgb <0.6,0.7,1.0>] [0.7 rgb <0.0,0.1,0.8>] } } }\n\n"
	      << "light_source {<0, 0, 0> color rgb <1, 1, 1> "
	      << "translate <-100, 300, -300> }\n\n"
	      << "plane { y, 0 texture{ pigment { checker color rgb 0.5 "
	      << "color rgb 0.2 scale 16.0 }finish{ "
	      << "diffuse 0.8 ambient 0.1 } } }\n\n"
	      << "camera { right x*image_width/image_height "
	      << "location <" << camera
	      << "> look_at <" << camera_look
	      << "> }\n\n";
    }
  }

  // iterate through the catom objects and save them to the file
  hash_map<const unsigned long, CatomSim *, 
    hash<const unsigned long>, equl>::iterator catom_i;

  vector<CodeModule*>::iterator mod_i;

  myFile.fileWrite_ModuleHeader();
  myFile.fileWrite_BodyStart();

  for(catom_i = catomHash.begin(); catom_i != catomHash.end(); catom_i++) {

    // Get the GUID
    uint32 id = catom_i->first;

    // _sim version 1 ( Simple )
    assert( catom_i->second->updateFileModule( myModule->getContent(), 
					       myModule->getVersion() ) );

    // TODO: tell the catom codemodules to all update their StateFile::Modules
    for ( mod_i = catom_i->second->codeModules.begin(); 
	  mod_i != catom_i->second->codeModules.end(); 
	  mod_i++ ) {
      (*mod_i)->saveModule();
    }

    // Write the new content to the file
    myFile.fileWrite_Content( id );

    // If appropriate, write out the povRay data to the temp pov file
    if( pov_visualize ) {
      povfile << catom_i->second->toPovObject() << "\n";
    }
  }

  myFile.fileClose();    
  povfile.close();

  if( pov_visualize ) {
    // Run povray on the result
    cmd = "povray +I" + outFile + ".pov +O" + outFile 
      + ".png +FN +W" + res_x + " +H" + res_y 
      + " -D -V -A0.3 2>/dev/null";
    system( cmd.c_str() ); 
  }

  // Remove the temporary file
  if ( !save_pov || !pov_visualize) {
    cmd = "rm -f " + outFile + ".pov";
    system( cmd.c_str() );
  }

  // If we make it here, then the file is saved, and we are done
  return true;
}


///////////////////////////////////////////////////////////////////////////////
// CatomWorld::search_key_value_list(string key)
//
// if a key exists in key_value_list return the value
//

string CatomWorld::search_key_value_list(string key) {
  for( unsigned int i = 0; i < key_value_list.size(); i++ ) {
    if( key_value_list[i].first == key )
      return key_value_list[i].second;
  }
  return "";
}


///////////////////////////////////////////////////////////////////////////////
// CatomWorld::createThreadSchedule()
//
// Create the next ordering for Catom execution in worker thread model
//

void CatomWorld::createThreadSchedule() {

  string rr = search_key_value_list("ROUNDROBIN");
  if(rr == "true") {
    return;
  }

  for(unsigned int i = 0; i < num_catoms; i++) {
    int r = rand() % num_catoms;
    unsigned int temp = thread_schedule[i]; 
    thread_schedule[i] = thread_schedule[r];
    thread_schedule[r] = temp;
  }
}


///////////////////////////////////////////////////////////////////////////////
// CatomWorld::manageLock( pthread_mutex_t, const char*, bool)
//
// lock or unlock screen output mutex
//

void CatomWorld::manageLock( pthread_mutex_t* mutex, 
			     const char* name, bool lock ) const {
  int ret;

  if (lock)  ret = pthread_mutex_lock(mutex);
  else       ret = pthread_mutex_unlock(mutex);

  if (ret == 0) return;

  std::cerr << "Unable to " << ((lock)? "" : "un")
	    << "lock " << name << std::endl;
  exit(0);
}


///////////////////////////////////////////////////////////////////////////////
// These functions below are only meant to be used by the worldbuilder       //
///////////////////////////////////////////////////////////////////////////////

void CatomWorld::clearCatoms() {
  hash_map<const unsigned long, CatomSim *, hash<const unsigned long>, equl>::iterator pos;
  
  for( pos = catomHash.begin(); pos != catomHash.end(); pos++ ) {
    delete (*pos).second;
  }
  
  catomHash.clear();
  num_catoms = 0;
}

void CatomWorld::deleteCatom(unsigned long int GUID) {
  if(catomHash.count(GUID)){
    delete catomHash.find(GUID)->second;
    catomHash.erase(GUID);
    num_catoms--;
  }
}

CatomSim* CatomWorld::getCatom(unsigned long int GUID) {
  if(catomHash.count(GUID))
    return catomHash.find(GUID)->second;
  return NULL;
}

bool CatomWorld::addCatom(CatomSim* c, unsigned long GUID) { 
  // Store pointer to Catom object in world.catomHash
  CatomSim *newCatom = new CatomSim(c, GUID);
  
  catomHash[GUID] = newCatom;
  
  // Increment the number of Catoms in the world
  num_catoms++;
  return true;
}

unsigned long int CatomWorld::catomAt(Point3D pos) {
  const dReal *dpos;
  if(num_catoms == 0 ) return 0;
  
  hash_map<const unsigned long, CatomSim *, hash<const unsigned long>, equl>::iterator c_i;
  for (c_i = catomHash.begin(); c_i != catomHash.end(); c_i++ ) {
    dpos = dGeomGetPosition(c_i->second->shape);
    Point3D temp(dpos[0], dpos[1], dpos[2]);
    if(temp == pos) {
      return c_i->first;
    }
  }
  return 0;
}

void CatomWorld::swap(unsigned long int GUID1, unsigned long int GUID2) {
  CatomSim* c1 = new CatomSim(catomHash.find(GUID1)->second, GUID2);
  CatomSim* c2 = new CatomSim(catomHash.find(GUID2)->second, GUID1);
  
  deleteCatom(GUID1);
  deleteCatom(GUID2);
  
  addCatom(c1, GUID2);
  addCatom(c2, GUID1);
  
  delete c1;
  delete c2;
}

bool loadPointHash(string path, hash_set<Point3D, HashPoint3D, EqualPoint3D>& targetMap) {

  targetMap.clear();

  long unsigned int GUID; //these 3 are necessary for StateFile to do its job
  StateFile::File myFile;
  StateFile::Module *myModule;

  // Initialize persistent file and (_sim) module
  myModule = StateFile::Module::getInstance( "_sim" );
  myModule->setVersion( 1 );
  myModule->setVersionRange( 1 );
  myModule->setContentProfile( "cccCCCCC" );
  myModule->setContentNames( "x:y:z:r:g:b:a:bat" );
  myModule->setContent( new StateFile::ByteData32() );
  myFile.registerModule( myModule );

  fstream fs_world;
  string line;
  unsigned int pos, lastpos;
  const string delimiter = ",";
  const string format = "%=0^+!#7.0n";
  vector<string> catomTokens;
  char *endptr;
  
  // Open the world file
  fs_world.open(path.c_str(), ios::in );
  if( !fs_world.good() ) {
    return false;
  }

  /////////////////////////////////////////////////////////////////////////////
  // READ THE MAGIC NUMBER HERE
  // If it is a CLAY file, do the binary load, else continue below
  // with the text based .dpr file load

  uint32 tempMagic = 0;
  fs_world.get( (char*)&tempMagic, 5 );
  if ( ntohl( tempMagic ) == StateFile::MagicNumber ) {
    fs_world.close();
 
    myFile.fileOpen( (char*)(path.c_str()) ); //reopen file through StateFile

    myFile.fileRead_ModuleHeader();

    GUID = myFile.fileRead_Content();
    StateFile::ByteData32 *bd32 = myModule->getContent(); //now we can begin reading catom points

    while ( bd32 != NULL and GUID > 0 ) {
      uint8* data = bd32->getData();

      Point3D p( (int)data[0], (int)data[1], (int)data[2] );
      targetMap.insert(p);

      GUID = myFile.fileRead_Content();
      bd32 = myModule->getContent();
    }
    myFile.fileClose();
  }


  /////////////////////////////////////////////////////////////////////////////
  // This code will execute if the magic number did not match.
  // We assume that the file is a .dpr ascii text world file

  else {
    fs_world.seekg( 0, ios::beg );

    while( fs_world.good() ) {
      getline( fs_world, line );
      if ( ! fs_world ) break;

      // Ignore blank line and comment line
      if( line == "" || line[0] == '#' || line == " " ) {
	continue;
      }

      // Tokenize the line of Catom data
      lastpos = line.find_first_not_of( delimiter, 0 );
      pos = line.find_first_of( delimiter, lastpos );

      while(( pos != string::npos || lastpos != string::npos ) && (pos != lastpos)) {
	catomTokens.push_back( line.substr( lastpos, pos-lastpos ) );
	lastpos = line.find_first_not_of( delimiter, pos );
	pos = line.find_first_of( delimiter, lastpos );
      }

      if( catomTokens.size() < 10 ) {
	cerr << "World File appears invalid..." << endl;
	return false;
      }

      // Parse the tokens and create the Catom object
      float x = strtod( (catomTokens[1].c_str()), &endptr );
      float y = strtod( (catomTokens[2].c_str()), &endptr );
      float z = strtod( (catomTokens[3].c_str()), &endptr );
      Point3D p(x,y,z);    

      catomTokens.clear();
      targetMap.insert(p);
    }
    fs_world.close();
  }

  return true;
}

///////////////////////////////////////////////////////////////////////
// loadPointHash loads a world or dpr file and places Point3ds
// representing the positions of catoms into a haset.
// This code is very similar to the world-loading code above.
//

hash_set<Point3D, HashPoint3D, EqualPoint3D> loadPointHash(string path) {
  
  hash_set<Point3D, HashPoint3D, EqualPoint3D> targetMap;
  targetMap.clear();
  
  long unsigned int GUID; //these 3 are necessary for StateFile to do its job
  StateFile::File myFile;
  StateFile::Module *myModule;
  
  // Initialize persistent file and (_sim) module
  myModule = StateFile::Module::getInstance( "_sim" );
  myModule->setVersion( 1 );
  myModule->setVersionRange( 1 );
  myModule->setContentProfile( "cccCCCCC" );
  myModule->setContentNames( "x:y:z:r:g:b:a:bat" );
  myModule->setContent( new StateFile::ByteData32() );
  myFile.registerModule( myModule );
  
  fstream fs_world;
  string line;
  unsigned int pos, lastpos;
  const string delimiter = ",";
  const string format = "%=0^+!#7.0n";
  vector<string> catomTokens;
  char *endptr;
  
  // Open the world file
  fs_world.open(path.c_str(), ios::in );
  if( !fs_world.good() ) {
    cerr << "ERROR: File not found: " << path << endl;
    exit(1);
  }
  
  /////////////////////////////////////////////////////////////////////////////
  // READ THE MAGIC NUMBER HERE
  // If it is a CLAY file, do the binary load, else continue below
  // with the text based .dpr file load
  
  uint32 tempMagic = 0;
  fs_world.get( (char*)&tempMagic, 5 );
  if ( ntohl( tempMagic ) == StateFile::MagicNumber ) {
    fs_world.close();
    
    myFile.fileOpen( (char*)(path.c_str()) ); //reopen file through StateFile
    
    myFile.fileRead_ModuleHeader();
    
    GUID = myFile.fileRead_Content();
    StateFile::ByteData32 *bd32 = myModule->getContent(); //now we can begin reading catom points
    
    while ( bd32 != NULL and GUID > 0 ) {
      uint8* data = bd32->getData();
      
      Point3D p( (int)data[0], (int)data[1], (int)data[2] );
      targetMap.insert(p);
      
      GUID = myFile.fileRead_Content();
      bd32 = myModule->getContent();
    }
    myFile.fileClose();
  }
  
  
  /////////////////////////////////////////////////////////////////////////////
  // This code will execute if the magic number did not match.
  // We assume that the file is a .dpr ascii text world file
  
  else {
    fs_world.seekg( 0, ios::beg );

    while( fs_world.good() ) {
      getline( fs_world, line );
      if ( ! fs_world ) break;
      
      // Ignore blank line and comment line
      if( line == "" || line[0] == '#' || line == " " ) {
	continue;
      }

      // Tokenize the line of Catom data
      lastpos = line.find_first_not_of( delimiter, 0 );
      pos = line.find_first_of( delimiter, lastpos );

      while(( pos != string::npos || lastpos != string::npos ) && (pos != lastpos)) {
	catomTokens.push_back( line.substr( lastpos, pos-lastpos ) );
	lastpos = line.find_first_not_of( delimiter, pos );
	pos = line.find_first_of( delimiter, lastpos );
      }

      if( catomTokens.size() < 10 ) {
	cerr << "World File appears invalid..." << endl;
	exit(1);
      }

      // Parse the tokens and create the Catom object
      float x = strtod( (catomTokens[1].c_str()), &endptr );
      float y = strtod( (catomTokens[2].c_str()), &endptr );
      float z = strtod( (catomTokens[3].c_str()), &endptr );
      Point3D p(x,y,z);    

      catomTokens.clear();
      targetMap.insert(p);
    }
    fs_world.close();
  }

  return targetMap;
}



// LINES STUFF

void CatomWorld::addLine(catomID src, catomID dest) {
  DPRLine *temp = new DPRLine(src, dest);
  pthread_mutex_lock(&worldLock);
  lines.push_back(*temp);
  pthread_mutex_unlock(&worldLock);
}

void CatomWorld::addLine(catomID src, Point3D dest) 
{
  DPRLine *temp = new DPRLine(src, dest);
  pthread_mutex_lock(&worldLock);
  lines.push_back(*temp);
  pthread_mutex_unlock(&worldLock);
}

void CatomWorld::addLine(catomID src, Point3D dest, 
                         unsigned int r, unsigned int g, unsigned int b) 
{
  DPRLine *temp = new DPRLine(src, dest, r, g, b);
  pthread_mutex_lock(&worldLock);
  lines.push_back(*temp);
  pthread_mutex_unlock(&worldLock);
}

void CatomWorld::addLine(catomID src, catomID dest,unsigned int r, unsigned int g, unsigned int b)
{
  DPRLine *temp = new DPRLine(src, dest, r, g, b);
  pthread_mutex_lock(&worldLock);
  lines.push_back(*temp);
  pthread_mutex_unlock(&worldLock);
}

void CatomWorld::clearLine(catomID src, catomID dest) {
  pthread_mutex_lock(&worldLock);
  vector<DPRLine>::iterator iter;
  for( iter = lines.begin(); iter != lines.end(); iter++ ) {
    if( iter->equals(src, dest) ) {
      lines.erase(iter);
    }
  }
  pthread_mutex_unlock(&worldLock);
}

void CatomWorld::clearLine(catomID src, Point3D dest)
{
  pthread_mutex_lock(&worldLock);
  vector<DPRLine>::iterator iter;
  for( iter = lines.begin(); iter != lines.end(); iter++ ) {
    if( iter->equals(src, dest) ) {
      lines.erase(iter);

    }
  }
  pthread_mutex_unlock(&worldLock);
}


void CatomWorld::clearLines(catomID src)
{
  pthread_mutex_lock(&worldLock);
  vector<DPRLine>::iterator iter;
  for( iter = lines.begin(); iter != lines.end(); iter++ ) {
    if( iter->getSrc()==src ) {
      lines.erase(iter);
    }
  }
  pthread_mutex_unlock(&worldLock);
}


void CatomWorld::listLines() {
  pthread_mutex_lock(&worldLock);
  vector<DPRLine>::iterator iter;
  for( iter = lines.begin(); iter != lines.end(); iter++ ) {
    if(iter->getType()==0)
      cout << iter->getSrc() << " " << iter->getDest() << endl;
    else
    {
      Point3D b = iter->getDestPt();
      cout << iter->getSrc() << " (" <<b.getX()<<", "<<b.getY()<<", "<<b.getZ()<<")"<< endl;
    }
      
  }
  pthread_mutex_unlock(&worldLock);
}


void CatomWorld::clearLines() {
  pthread_mutex_lock(&worldLock);
  lines.clear();
  pthread_mutex_unlock(&worldLock);
}
