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

//
// How DPRSim works
//
// 1) Parse the Commandline
//    a) get the Experiment file
//    b) get the World file
//
// 2) Load the info from the Experiment file
//    a) get the number of timesteps to run
//    b) get how often to save
//    c) get whether to visualize or not
//    d) get the Code Module List for this experiment
//    e) get the current world variables (gravity etc...)
//
// 3) Load the info from the World file
//    a) get the current geometry information
//    b) get the current state of running modules if applicable
//
// 4) Create the Catom objects and associated threads
//    a) load the proper geometrical info into the catoms
//    b) load the proper code modules
//    c) initialize the code modules
//
// 5) Run the simulation
//    a) run on timestep with appropriate code modules
//    b) save new state file if appropriate
//    c) visualize if appropriate
//    d) repeat (5)
//

#include <stdlib.h>
#include <pthread.h>
#include <errno.h>
#include <string.h>
#include <utility>
#include <vector>
#include <unistd.h>
#include <iostream>
#include <fstream>

#include "Catom.hxx"
#include "CatomSim.hxx"
#include "Debugging.hxx"
#include "SmartYield.hxx"
#include "CatomWorld.hxx"
#include "GlobalConstants.hxx"
#include "DPRSim.hxx"
#include "WPManager.hxx"
#include "Util.hxx"

using namespace std;

// Thread Specific data ptrs
pthread_key_t _tcatom;
pthread_key_t _tcatomsim;

// Global pointer to the simulated world
CatomWorld* worldPtr;
SimCallback simcb = {NULL, NULL, NULL, NULL, NULL, NULL};

// Shortcuts for Physics calculation spaces
#define sim_world (worldPtr->dyn_world)
#define sim_space (worldPtr->coll_space)
#define sim_feature_space (worldPtr->mag_space)

double current_time = 0;     // simulation time
double next_step_time = 0;   // next time threads are awakened (step)

#ifdef __THREADED__
// Global array of thread IDs
vector<pthread_t> catomThreadList;
#endif

// Flag signifying whether the main loop has finished
bool isComplete = false;
bool NC_Oracles = false;
bool NC_AllThreads = false;
pthread_mutex_t NC_lock = PTHREAD_MUTEX_INITIALIZER;

// Global yield used for synchronization
pt_yield yield;

// Various Simulator helper functions/variables
bool needToStop=false;
bool simPaused=false;
bool buildmode=false;
int steps=0;
void simStop() { needToStop=true; }
void simPause(bool state) { simPaused = state; steps = 0; }
bool simGetPaused() { return simPaused; }
void simAdvanceSteps(int n) { steps += (int)(n*(worldPtr->sim_time_step/worldPtr->phys_time_step)); }
bool building(){return buildmode;}

//watchpoint support
WPManager *wp = NULL;

///////////////////////////////////////////////////////////////////////////////
// Physics and Graphics Setup
//

#include <ode/ode.h>
#include "graphics/graphics.h"
#include "graphics/internal.h"

#ifdef __USE_DRAWSTUFF__
#include "WorldBuilder.hxx"
WorldBuilder builder;
static int show_contacts = 0;	// show contact points when rendering
#endif

// select correct drawing functions

#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#endif

#define MAX_CONTACTS 4		// maximum number of contact points per body

static dJointGroupID contactgroup;
static dGeomID ground_plane;

char locase (char c) {
  if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
  else return c;
}

void selectCallback(float x, float y, float z, float dx, float dy, float dz)
{
  const dReal *dpos;
  float pos[3];
  Catom *ctm = NULL;
  float a, b, c;
  float mt, md;
  float dist_sq = -1.0f;
  float t;
  
  hash_map<const unsigned long, CatomSim *, 
    hash<const unsigned long>, equl>::iterator c_i;
  for ( c_i = worldPtr->catomHash.begin();
	c_i != worldPtr->catomHash.end(); c_i++ ) {
    dpos = dGeomGetPosition(c_i->second->shape);
    pos[0] = dpos[0], pos[1] = dpos[1], pos[2] = dpos[2];
    a = dx*dx + dy*dy + dz*dz;
    b = 2.0f*((x-pos[0])*dx+(y-pos[1])*dy+(z-pos[2])*dz);
    c = (x-pos[0])*(x-pos[0])+(y-pos[1])*(y-pos[1])+(z-pos[2])*(z-pos[2]);
    mt = -b/(2.0f*a);
    md = a*mt*mt + b*mt + c;
    if (md < 1.0f) {
      t = (x-pos[0])*(x-pos[0]) + (y-pos[1])*(y-pos[1]) + (z-pos[2])*(z-pos[2]);
      if (t < dist_sq || dist_sq < 0.0f) {
	dist_sq = t;
	ctm = &(c_i->second->C);
      }
    }
  }
  if (ctm) {
    ctm->selected();
  }
}

// this is called by dSpaceCollide when two objects in space are
// potentially colliding.

static void collideCallback (void *data, dGeomID o1, dGeomID o2)
{
  int i;
  double mu = worldPtr->friction;
  // if (o1->body && o2->body) return;
  
  // exit without doing anything if the two bodies are connected by a joint
  dBodyID b1 = dGeomGetBody(o1);
  dBodyID b2 = dGeomGetBody(o2);
  if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
  if ( worldPtr->use_ground_friction && (o1==ground_plane || o2==ground_plane))
    mu=worldPtr->ground_friction;
  
  dContact contact[MAX_CONTACTS];   // up to MAX_CONTACTS contacts per box-box
  for (i=0; i<MAX_CONTACTS; i++) {
    contact[i].surface.mode = dContactBounce | dContactSoftCFM;
    contact[i].surface.mu = mu;
    contact[i].surface.mu2 = 0;
    contact[i].surface.bounce = worldPtr->bounce;
    contact[i].surface.bounce_vel = .1;
    contact[i].surface.soft_cfm = 0.00005;
  }
  if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
			   sizeof(dContact))) {
#ifdef __USE_DRAWSTUFF__
    dMatrix3 RI;
    dRSetIdentity (RI);
    const dReal ss[3] = {0.02,0.02,0.02};
#endif
    for (i=0; i<numc; i++) {
      dJointID c = dJointCreateContact (sim_world,contactgroup,contact+i);
      dJointAttach (c,b1,b2);
#ifdef __USE_DRAWSTUFF__
      if (show_contacts) {
        dsSetColor (0,0,2);
        dsDrawBox (contact[i].geom.pos,RI,ss);
      }
#endif
    }
  }
}

#define CONFIG_X86_TSC 1
#define rdtscll(val) \
     __asm__ __volatile__("rdtsc" : "=A" (val))
// from #include <asm/timex.h>
typedef unsigned long long cycles_t;
static inline cycles_t get_cycles (void)
{
#ifndef CONFIG_X86_TSC
  return 0;
#else
  unsigned long long ret;
  
  rdtscll(ret);
  return ret;
#endif
}

//
//  End Physics and Graphics Section
//
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
//
// Main DPR Simulator Loop Function
//

static void simLoop (int pause, int buildmode)
{
  static cycles_t time_a=0, time_b=0;
  static cycles_t time_col=0, time_phys=0, time_mag=0, time_nei=0;
  static cycles_t time_sim=0, time_save=0, time_premag=0, time_gr=0;
  
  time_b = get_cycles();
  time_gr = time_b-time_a;

  if (buildmode) {
  }
  else if(!pause) {

    // SAMPLE: adding lines to the world
    //worldPtr->addLine(50,50);
    //worldPtr->listLines();
    //cout << endl;

    // create the thread ordering for this round
    worldPtr->createThreadSchedule(); 

    // execute this round
    while (current_time >= next_step_time) {
      if (worldPtr->current_time >= worldPtr->timesteps) simStop();
      else {
#ifndef __USE_DRAWSTUFF__
        cout << "DPRSim: Running Simulation step: (" 
	     << worldPtr->current_time+1 << "/" 
	     << worldPtr->timesteps << ")" << endl;
#endif
#ifdef __USE_DRAWSTUFF__

	if (!(worldPtr->search_key_value_list("USE_GTK")=="false")) {
	  update_widget("simProgressBar", 
			"GtkProgressBar", 
			((float)(worldPtr->current_time+1)/(float)worldPtr->timesteps));
	  char msg[50];
          sprintf(msg, "Simulation Step: (%d/%d) [mag: %d, phys: %d, sim: %d, gr: %d]",
                  ((int)worldPtr->current_time+1),
                  ((int)worldPtr->timesteps),
                  (int)(time_mag/1000000),
                  (int)(time_phys/1000000),
                  (int)(time_sim/1000000),
                  (int)(time_gr/1000000));

	  update_widget("simProgressBar", "GtkProgressBar", msg);
	  
	  update_widget("modulesList", "GtkEntry", worldPtr->modules_list);
	  update_widget("featureMap", "GtkEntry", worldPtr->featureMapType);
	  
	  // Set the GUI with current experimental settings
	  if (worldPtr->current_time < 1) {
	    update_widget("experimentFile", "GtkEntry", worldPtr->experimentFile);
	    update_widget("worldFile", "GtkEntry", worldPtr->worldFile);
	    
	    sprintf(msg, "%d", worldPtr->num_catoms);
	    update_widget("numCatoms", "GtkEntry", msg);
	    sprintf(msg, "%d", worldPtr->timesteps);
	    update_widget("timeSteps", "GtkEntry", msg);
	    sprintf(msg, "%f", worldPtr->phys_time_step);
	    update_widget("physicsTimeStep", "GtkEntry", msg);
	    sprintf(msg, "%f", worldPtr->sim_time_step);
	    update_widget("simTimeStep", "GtkEntry", msg);
	    sprintf(msg, "%d", worldPtr->save);
	    update_widget("saveEveryNSteps", "GtkEntry", msg);
	    
	    update_widget("modulesList", "GtkEntry", worldPtr->modules_list);
	    update_widget("featureMap", "GtkEntry", worldPtr->featureMapType);
	  }
	}

#endif

        // Determine neighbors using mag_space collision detection
        time_a = get_cycles();
        CatomSim::updateAllNeighbors();
        time_b = get_cycles();
        time_nei = time_b-time_a;
	
        // TODO: Power routing analysis here

#ifdef __THREADED__	
        // Master yields to let oracles and catoms run
        do_master_yield( &yield );
        // Yield again to let oracles run
        do_master_yield( &yield );
	// stuff for Seth
	// int nextRun = okToRunIndex + 1;
	// if (nextRun == wrapAroundIndex) okToRunIndex = 0; else okToRunIndex = nextRun;
#endif

#ifndef __THREADED__
	// NON THREADED VERSION OF CATOM EXECTUTION

	for(unsigned int i = 0; i < worldPtr->num_catoms; i++) {
	  catomFunction(&(worldPtr->catomHash[worldPtr->thread_schedule[i]]->C));
	}

	// END NON THREADED VERSION
#endif

        time_a = get_cycles();
        time_sim = time_a-time_b;
	
        // All catom threads have yielded.  This is where the main 
        // thread does the work
	
        if DPR_THREADING_DEBUG
          cout << "Master thread gaining control (" << worldPtr->current_time 
	       << ")..." << endl;
		
		//do on-tick watchpoint actions here
		if (wp) wp->step();
	
	// If the Save option is set we want to generate a file at the 
        // bottom of every N loops (where N is the value of "save")
        if( worldPtr->save && !((worldPtr->current_time+1)%worldPtr->save) ) {
          worldPtr->saveWorld();
        }
	time_b = get_cycles();
        time_save = time_b-time_a;
        CatomSim::applyMagnetStates();
        time_a = get_cycles();
        time_premag = time_a-time_b;
#ifndef __USE_DRAWSTUFF__
	cout << "     col " << (time_col/1000000) 
	     << " mag " << (time_mag/1000000) 
	     << " phys " << (time_phys/1000000) 
	     << " nei " << (time_nei/1000000) 
	     << " sim " << (time_sim/1000000) 
	     << " save " << (time_save/1000000) 
	     << " premag " << (time_premag/1000000) 
	     << " gr " << (time_gr/1000000) << "\n";
#endif
        worldPtr->current_time++;
      }
      next_step_time += worldPtr->sim_time_step;
    }
    
    if ( worldPtr->use_physics ) {
      
      // Do collision check and add forces for contacts
      time_a = get_cycles();
      dSpaceCollide (sim_space,0,&collideCallback);
      time_b = get_cycles();
      time_col = time_b-time_a;
      
      // add magnet forces through mag_space collision detection
      CatomSim::applyAllMagnetForces();
      
      // add magic forces
      CatomSim::applyAllMagicForces();
      time_a = get_cycles();
      time_mag = time_a-time_b;
      
      // step physics forward in time
      dWorldQuickStep (sim_world,worldPtr->phys_time_step);
      current_time += worldPtr->phys_time_step;
      
      // remove all contact joints
      dJointGroupEmpty (contactgroup);
      time_b = get_cycles();
      time_phys = time_b-time_a;
    } else {
      current_time += worldPtr->sim_time_step;
    }
  }
  
  time_a = get_cycles();
}

//
// End Main Simulator Loop Function
//
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
// void usage()
//
// Purpose: saves current state data in the new file specified
//

static void usage() {
  string progname = "dprsim";
#ifdef __USE_DRAWSTUFF__
  progname = "dprsimgr";
#endif
  cerr << "Usage:\t./"<<progname<<" -e <experiment file> [-w <world file>]"<<endl;

#ifdef __USE_DRAWSTUFF__
  cerr<<"\t./dprsimgr -b [-w <world file>]" << endl;
#endif
}


///////////////////////////////////////////////////////////////////////////////
//
// main()
//
// The main loop of DPRSim.  The function parses the commandline and 
// then loops according to the number of times provided by the timesteps 
// option.  Currently, it is up to the catom programmers to define the notion 
// of timesteps and steplength.  Depending on the provided options, the world 
// is saved to file at the end of each loop (if specified).  While the 
// world is being loaded, the catom threads are suspended until loading is 
// finished.  This is to reduce the headache the catom programmers might have 
// to deal with (the notion of "which catom comes first").  It uses pt_yield 
// and functions defined in SmartYield.hxx to achieve synchronization.
//

int main( int argc, char **argv ) {

#ifdef __USE_DRAWSTUFF__
  simcb = *(dsGetCallbacks());
#endif  
  // Declare and construct the simulated world to be populated
  CatomWorld world;
  worldPtr = &world;

  // Local temp variables
  int c = 0;
#ifdef __THREADED__
  int ret;
#endif
  //string outFile = "";
  //unsigned int pos;
  const string format = "%=0^+!#7.0n";    

  if (simcb.preStart)
    simcb.preStart();
  worldPtr->getView = simcb.getView;
  worldPtr->threadStart = simcb.threadStart;
  
  /////////////////////////////////////////////////////////////////////////////
  // 1) Parse the Commandline
  //    a) get the Experiment file
  //    b) get the World file
  
  cout << endl << "DPRSim: Parsing the commandline..." << endl;
  
  if( argc < 2 ) {
    usage();
    exit(1);
  }
  
  vector<string> options;
  
  while ( (c = getopt (argc, argv, "-w:e:b")) != -1 ) {
    switch(c) {
    case 1:    // arguments that are not options, i.e. not of form -x
      options.push_back( optarg );
      cout << "DPRSim: Adding " << optarg << " to experiment options\n";
      break;
    case 'b':  //special case for "-b" builder mode
      buildmode = true;
      world.experimentFile = "DATA/BUILDMODE.exp";
      cout << "DPRSim: Loading Experiment File DATA/BUILDMODE.exp..."<< endl;
      do {
        fstream fs_exp;
        string line;
        fs_exp.open( "DATA/BUILDMODE.exp", ios::in );
        if( !fs_exp.good() ) {
          cerr << "ERROR: File not found: DATA/BUILDMODE.exp" << endl;
          exit(1);
        }
        while( fs_exp.good() ) {
          getline( fs_exp, line );
          options.push_back( line );
        }
        fs_exp.close();
      } while (0);
#ifdef __USE_DRAWSTUFF__
      builder.setWorld(worldPtr);
      builder.setLattice("cubic"); //this is the default for now, it should be set in BUILDMODE.exp
#endif  
      break;
    case 'w':  // special case for "-w DATA/foo.world" option
      if (optarg[0]=='/') options.push_back( string("WORLD=")+string(optarg) );
      else options.push_back( string("WORLD=./")+string(optarg) );
      break;
    case 'e':  // special case for "-e DATA/bar.exp" option
      world.experimentFile = optarg;
      cout << "DPRSim: Loading Experiment File " << optarg << "..."<< endl;
      do {
        fstream fs_exp;
        string line;
        fs_exp.open( optarg, ios::in );
        if( !fs_exp.good() ) {
          cerr << "ERROR: File not found: " << optarg << endl;
          exit(1);
        }
        while( fs_exp.good() ) {
          getline( fs_exp, line );
          options.push_back( line );
        }
        fs_exp.close();
      } while (0);
      break;
    default:
      usage();
      exit(1);
    }
  }    
  
  /////////////////////////////////////////////////////////////////////////////
  // 2) Load the options from the Experiment file
  //    a) get the number of timesteps to run
  //    b) get how often to save
  //    c) get whether to visualize or not
  //    d) get the Code Module List for this experiment
  //    e) get the current world variables (gravity etc...)
  
  cout << endl << "DPRSim: Parsing Experiment Options..." << endl;
  
  world.loadExperiment( options );
  
  
  /////////////////////////////////////////////////////////////////////////////
  // 3) Load the info from the World file
  //    a) get the current geometry information
  //    b) get the current state of running modules if applicable
  
  cout << endl << "DPRSim: Loading World Data..." << endl;  

  // new initializer -- nthreads=0; updated as threads created
  init_yield( 0, &yield );  

  // init thread specific data
  if (pthread_key_create(&_tcatom, NULL)) die("Can't create _tcatom");
  if (pthread_key_create(&_tcatomsim, NULL)) die("Can't create _tcatomsim");

  /////////////////////////////////////////////////////////////////////////////
  // 4) Create the Catom objects and associated threads
  //   -a) prepare the debugging support
  //    a) load the proper geometrical info into the catoms
  //    b) load the proper code modules
  //    c) initialize the code modules
  
  // Prepare debugging support
  dprdebug_open("/tmp/dprdebug");

  world.loadWorld();

	// prepare watchpoint support 
  string watchpoint_str = world.search_key_value_list("WP");
  if (watchpoint_str != "") wp = new WPManager(watchpoint_str);

  // Display a general description of this simulation
  cout << "\nDPRSim: Running Experiment: " << world.experimentFile << endl
       << "DPRSim: Using World: " << world.worldFile << endl
       << "DPRSim: Starting Simulation: " << world.timesteps << " timesteps, " 
       << world.num_catoms << " catoms." << endl
       << "DPRSim: Running Modules: " << world.modules_list << endl;
  if ( world.save )
    cout << "DPRSim: Saving every (" << world.save << ") timesteps." << endl;
  else
    cout << "DPRSim: Not saving intermediate steps." << endl;
  if( world.pov_visualize )
    cout << "DPRSim: Rendering (" << world.res_x << "x" << world.res_y
	 <<") visualization for each saved file.\n" << endl;
  else
    cout << "DPRSim: Not rendering visualization.\n" << endl;


#ifdef __USE_DRAWSTUFF__

  // Update the Interface

  
#endif


  
  // Yield here for two purposes
  //
  // 1) Align the condvar; the next time master yields, 
  //    the appropriate condvar will be signaled.
  // 2) Ensure that all threads yield at the same (first) yield point.
#ifdef __THREADED__
  do_master_yield( &yield );
  
  if DPR_THREADING_DEBUG
    cout << "master gaining control (1st)..." << endl;
#endif
  
  // Start the central control "oracle" code in CatomWorld
  void *args;
  world.oracle( args );
  

  /////////////////////////////////////////////////////////////////////////////
  // 5) Run the simulation
  //    a) run on timestep with appropriate code modules
  //    b) save new state file if apprpropriate
  //    c) visualize if appropriate
  //    d) repeat (5)
  
  // Initialize ODE Physics Engine
  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (sim_world,0,0,-world.gravity);
  dWorldSetCFM (sim_world,1e-5);
  dWorldSetERP (sim_world,0.2);
  dWorldSetContactMaxCorrectingVel (sim_world,0.1);
  dWorldSetContactSurfaceLayer (sim_world,0.1);
  ground_plane = dCreatePlane (sim_space,0,0,1,0);
  
  // ensure one extra "sim" step to run simulationStart() and oracles
  world.current_time=-1;
  next_step_time -= worldPtr->sim_time_step;

#ifndef __THREADED__
  // WORKER THREAD VERSION
  // call simulationStart()
  if(!building()){
    for(unsigned int i = 0; i < worldPtr->num_catoms; i++) {
      CatomSim* catomSim = worldPtr->catomHash[worldPtr->thread_schedule[i]];
      vector<CodeModule*>::iterator moduleIterator = catomSim->codeModules.begin();
      while(moduleIterator != catomSim->codeModules.end()) {
	(*moduleIterator)->simulationStart();
	moduleIterator++;
      }
    }
  }
  // END WORKER THREAD VERSION
#endif

  ///////////////////////////////
  // BEGIN MAIN SIMULATOR LOOP //
  ///////////////////////////////
  
  if (simcb.start)
    simcb.start();

  while (needToStop==0) {
    if (simcb.tick && !steps)
      simcb.tick();
    
    simLoop((simPaused && !steps),  buildmode);
    steps--;
    if (steps < 0)
      steps = 0;
  }
  
  if (simcb.end)
    simcb.end();

  /////////////////////////////
  // END MAIN SIMULATOR LOOP //
  /////////////////////////////

  // Loop done
  isComplete = true;
  
#ifdef __THREADED__  
  pthread_mutex_lock(&yield.mutex);
  
  // If halt_all is called
  if( yield.pausemaster ) {
    pthread_cond_broadcast( &yield.master_yielded );
    pthread_cond_wait( &yield.resumemaster, &yield.mutex );
  }
  
  // Wake threads up to exit
  pthread_cond_broadcast( &yield.condvar[1 - yield.currentcondvar] );
  
  // Book-keeping -- might not be necessary
  yield.currentcondvar = 1 - yield.currentcondvar;
  yield.nyielded = 0;
  pthread_mutex_unlock( &yield.mutex );
  
  // Join all catom threads back to main thread
  for( unsigned int i = 0; i < catomThreadList.size() && !buildmode; i++ ) {
    if( (ret = pthread_join( catomThreadList[i], NULL )) != 0 ) {
      cerr << "Error: unable to join thread :: " << strerror(errno) << endl;
      exit(0);
    }
    
    if DPR_THREADING_DEBUG
      cout << "Thread #" << catomThreadList[i] 
	   << " terminated." << endl;
  }
#endif

#ifndef __THREADED__
  // WORKER THREAD VERSION
  // call simulationEnd()
  if(!building()){
    for(unsigned int i = 0; i < worldPtr->num_catoms; i++) {
      CatomSim* catomSim = worldPtr->catomHash[worldPtr->thread_schedule[i]];
      vector<CodeModule*>::iterator moduleIterator = catomSim->codeModules.begin();
      while(moduleIterator != catomSim->codeModules.end()) {
	(*moduleIterator)->simulationEnd();
	moduleIterator++;
      }
    }
  }
  // END WORKER THREAD VERSION
#endif
  
  ////////////////////////////////////////////////////////
  // All catom threads must be terminated by this point //
  ////////////////////////////////////////////////////////
  
  // Always save the world at the end of the program if in simulation
  if(!buildmode) {
    world.current_time = world.timesteps + 1;
    world.saveWorld();
  }

#ifdef __THREADED__  
  // Free memory taken up by yield
  destroy_yield( &yield );
  
  if( (ret = pthread_mutex_destroy( &CatomWorld::worldLock )) != 0 ) {
    cerr << "Error: unable to destry mutex :: " << strerror(ret) << endl;
    exit(0);
  }
#endif
  
  cout << "\nDPRSim: Simulation Ended.\n" << endl;
  
  return 0;
}
