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

#include <iostream>

#include "CatomSim.hxx"
#include "CodeModuleList.hxx"
#include "CatomWorld.hxx"
#include "SmartYield.hxx"

using namespace std;
extern void* catomMain(void *vargp);
extern CatomWorld *worldPtr;
extern pt_yield yield;

dMass CatomSim::catom_mass_struct;
dMass* CatomSim::catom_mass_ptr=0;
double CatomSim::catom_mass=1;
double CatomSim::catom_radius=1;
double CatomSim::magnet_depth=0.1;
double CatomSim::magnet_max_force=100;
double CatomSim::magnet_radius=1;

CatomSim::CatomSim(long unsigned int id, Point3D loc, 
		   double qx, double qy, double qz, 
		   uint8 t_red, uint8 t_green, uint8 t_blue, 
		   uint8 t_alpha ) :
  C(id), 
  red(t_red),
  green(t_green), 
  blue(t_blue), 
  alpha(t_alpha)
{
  body = dBodyCreate( worldPtr->dyn_world );
  dBodySetData( body, (void*) this );
  if (catom_mass_ptr==0) {
    if (worldPtr->use_cylinder)
      dMassSetCylinderTotal( &catom_mass_struct, catom_mass, 3,
        catom_radius, 2*catom_radius );
    else
      dMassSetSphereTotal( &catom_mass_struct, catom_mass, catom_radius );
    catom_mass_ptr = &catom_mass_struct;
  }
  dBodySetMass( body, catom_mass_ptr );
  dBodySetPosition( body, loc.getX(), loc.getY(), loc.getZ() );
  // set rotation!!
  dQuaternion Q;
  dQFromAxisAndAngle( Q, qx, qy, qz, dSqrt( qx*qx + qy*qy + qz*qz ) );
  dBodySetQuaternion( body, Q );

  if (worldPtr->use_cylinder) {
    shape = dCreateCylinder(  worldPtr->coll_space, catom_radius, catom_radius*2 );
    shape2 = dCreateCylinder(  worldPtr->neigh_space, 1.5*catom_radius, catom_radius*2 );
  } else {
    shape = dCreateSphere( worldPtr->coll_space, catom_radius );
    shape2 = dCreateSphere( worldPtr->neigh_space, 1.5*catom_radius );
  }

  dGeomSetData( shape, (void*) this );
  dGeomSetBody( shape, body );
  dGeomSetData( shape2, (void*) this );
  dGeomSetBody( shape2, body );
  
  // create spheres in mag_space for each feature
  feature_container = dHashSpaceCreate( worldPtr->mag_space );
  dGeomSetData((dGeomID) feature_container, (void*) this );
  //  dCreateSphere( feature_container, 2*catom_radius );
  feature_points = new dGeomID[NUM_FEATURES+1];
  magnet_state = new char[NUM_FEATURES+1];
  unsigned int i;
  for (i=1; i<=NUM_FEATURES; i++) {
    Point3D tmppos = (C.getFeatureMap())[i].getLocation();
    double tmprad = catom_radius-magnet_depth;
    dGeomID tmpobj = dCreateSphere( 0, magnet_radius );
    dGeomSetData( tmpobj, (void*) i );
    feature_points[i] = dCreateGeomTransform( feature_container );
    dGeomTransformSetGeom( feature_points[i], tmpobj );
    dGeomTransformSetInfo( feature_points[i], 1 );
    dGeomTransformSetCleanup( feature_points[i], 1 );
    dGeomSetData( feature_points[i], (void*) this );
    dGeomSetPosition( tmpobj, tmprad*tmppos.getX(), 
		      tmprad*tmppos.getY(), tmprad*tmppos.getZ() );
    dGeomSetBody( feature_points[i], body );
    magnet_state[i] = 0;
  }
  
  //  magic_force = Point3D(10,0,1) - loc;

#ifdef __THREADED__
  int ret;
  pthread_t tid;

  // Set stack size
  pthread_attr_t attrs;
  pthread_attr_init(&attrs);
  pthread_attr_setstacksize(&attrs, (1<<15));

  incr_nthreads( &yield );  // new: need to increment number of threads that yield
  
  // create a thread for this catom by catomMain
  if( (ret = pthread_create(&tid, &attrs, catomMain, &C)) != 0 ) {
    cerr << "Error: unable to create thread" << " :: " 
	 << strerror(ret) << endl;
    exit(0);
  }
  
  // store thread ID in the list in CatomWorld
  catomThreadList.push_back(tid);
#endif
}


//This "duplicate" constructor is meant only for WorldBuilder use
//it may discard info WorldBuilder doesn't need!
CatomSim::CatomSim(CatomSim* c, unsigned long GUID): 
  C(GUID), 
  // Physics stuff
  body(c->body),
  shape(c->shape),
  shape2(c->shape2),
  feature_container(c->feature_container),
  // Visualization information  
  red(c->red),
  green(c->green), 
  blue(c->blue), 
  alpha(c->alpha)
{
  // create spheres in mag_space for each feature
  feature_container = dHashSpaceCreate( worldPtr->mag_space );
  dGeomSetData((dGeomID) feature_container, (void*) this );
  //  dCreateSphere( feature_container, 2*catom_radius );
  feature_points = new dGeomID[NUM_FEATURES+1];
  magnet_state = new char[NUM_FEATURES+1];
  unsigned int i;
  for (i=1; i<=NUM_FEATURES; i++) {
    Point3D tmppos = (C.getFeatureMap())[i].getLocation();
    double tmprad = catom_radius-magnet_depth;
    dGeomID tmpobj = dCreateSphere( 0, magnet_radius );
    dGeomSetData( tmpobj, (void*) i );
    feature_points[i] = dCreateGeomTransform( feature_container );
    dGeomTransformSetGeom( feature_points[i], tmpobj );
    dGeomTransformSetInfo( feature_points[i], 1 );
    dGeomTransformSetCleanup( feature_points[i], 1 );
    dGeomSetData( feature_points[i], (void*) this );
    dGeomSetPosition( tmpobj, tmprad*tmppos.getX(), 
		      tmprad*tmppos.getY(), tmprad*tmppos.getZ() );
    dGeomSetBody( feature_points[i], body );
    magnet_state[i] = 0;
  }

#ifdef __THREADED__
  int ret;
  pthread_t tid;

  // Set stack size
  pthread_attr_t attrs;
  pthread_attr_init(&attrs);
  pthread_attr_setstacksize(&attrs, (1<<15));

  incr_nthreads( &yield );  // new: need to increment number of threads that yield

  // create a thread for this catom by catomMain
  if( (ret = pthread_create(&tid, &attrs, catomMain, &C)) != 0 ) {
    cerr << "Error: unable to create thread" << " :: " 
	 << strerror(ret) << endl;
    exit(0);
  }
#endif
}

CatomSim::~CatomSim() {
    
  vector<CodeModule*>::iterator pos;
  for( pos = codeModules.begin(); pos != codeModules.end(); pos++ ) {
    delete (*pos);
  }

  /*
  set<CatomSim*>::iterator post;
  for( post = nearCatoms.begin(); post != nearCatoms.end(); post++ ) {
    delete (*post);
  }
  */
  
}


string CatomSim::toString() {
  string str;
  stringstream ss;
  const dReal* loc = dBodyGetPosition(body);
  
  ss << C.getID() << ","
     << loc[0] << ","
     << loc[1] << ","
     << loc[2] << ","
     << C.amPowered() << ","
     << "1" << ","
     << "2" << ","
     << (double)red/255 << ","
     << (double)green/255 << ","
     << (double)blue/255 << ","
     << (double)alpha/255;
  ss >> str;
  
  return str;
}

void CatomSim::dumpNetworkingStats() {
  worldPtr->oStart();
  cerr << worldPtr->current_time << " Network " << C.getID() << ":";
  for(unsigned int i=1; i<=NUM_FEATURES; i++) {
    cerr << " (";
    Feature* contact = &(C.getFeatureMap()[i]);
    cerr << contact->getNetworkAdapter()->stats_msgsSent << "/"
	 << contact->getNetworkAdapter()->stats_msgsRcvd << ")";
  }
  cerr << endl << flush;
  worldPtr->oEnd();
}

bool CatomSim::updateFileModule( StateFile::ByteData32* content, uint8 version ) {
  
  switch(version) {
  case 1: {
    content->setLength(8); // could rely on this to be set once per simulation...
    uint8 *buffer = content->getData();
    const dReal* loc = dBodyGetPosition( body );
    Point3D location( loc[0], loc[1], loc[2] );
    
    buffer[0] = (uint8)((int8)location.getX());
    buffer[1] = (uint8)((int8)location.getY());
    buffer[2] = (uint8)((int8)location.getZ());
    
    buffer[3] = red;
    buffer[4] = green;
    buffer[5] = blue;
    buffer[6] = alpha;
    
    buffer[7] = 0;
    break;
  }
  case 2: {
    float x,y,z,a;
    content->setLength(57);
    uint8 *data = content->getData();

    // LOCATION
    const dReal* loc = dBodyGetPosition( body );
    x = loc[0];
    y = loc[1];
    z = loc[2];
    unsigned char *bp;
    bp = (unsigned char*)&x;
    data[0] = bp[0];
    data[1] = bp[1];
    data[2] = bp[2];
    data[3] = bp[3];
    bp = (unsigned char*)&y;
    data[4] = bp[0];
    data[5] = bp[1];
    data[6] = bp[2];
    data[7] = bp[3];
    bp = (unsigned char*)&z;
    data[8] = bp[0];
    data[9] = bp[1];
    data[10] = bp[2];
    data[11] = bp[3];
    
    // COLOR
    data[12] = red;
    data[13] = green;
    data[14] = blue;
    data[15] = alpha;

    // BATTERY
    data[16] = 0;

    // VELOCITY
    const dReal* vel = dBodyGetLinearVel( body );
    x = vel[0];
    y = vel[1];
    z = vel[2];
    bp = (unsigned char*)&x;
    data[17] = bp[0];
    data[18] = bp[1];
    data[19] = bp[2];
    data[20] = bp[3];
    bp = (unsigned char*)&y;
    data[21] = bp[0];
    data[22] = bp[1];
    data[23] = bp[2];
    data[24] = bp[3];
    bp = (unsigned char*)&z;
    data[25] = bp[0];
    data[26] = bp[1];
    data[27] = bp[2];
    data[28] = bp[3];

    // ANGULAR VELOCITY
    const dReal* angVel = dBodyGetAngularVel( body );
    x = angVel[0];
    y = angVel[1];
    z = angVel[2];
    bp = (unsigned char*)&x;
    data[29] = bp[0];
    data[30] = bp[1];
    data[31] = bp[2];
    data[32] = bp[3];
    bp = (unsigned char*)&y;
    data[33] = bp[0];
    data[34] = bp[1];
    data[35] = bp[2];
    data[36] = bp[3];
    bp = (unsigned char*)&z;
    data[37] = bp[0];
    data[38] = bp[1];
    data[39] = bp[2];
    data[40] = bp[3];

    // ROTATION
    const dReal* rot = dBodyGetQuaternion( body );
    x = rot[0];
    y = rot[1];
    z = rot[2];
    a = rot[3];
    bp = (unsigned char*)&x;
    data[41] = bp[0];
    data[42] = bp[1];
    data[43] = bp[2];
    data[44] = bp[3];
    bp = (unsigned char*)&y;
    data[45] = bp[0];
    data[46] = bp[1];
    data[47] = bp[2];
    data[48] = bp[3];
    bp = (unsigned char*)&z;
    data[49] = bp[0];
    data[50] = bp[1];
    data[51] = bp[2];
    data[52] = bp[3];
    bp = (unsigned char*)&a;
    data[53] = bp[0];
    data[54] = bp[1];
    data[55] = bp[2];
    data[56] = bp[3];

    break;
  }

  default: 
    return false;
  }
  return true;
}


string CatomSim::toPovObject() {
  string str;
  stringstream ssa, ssc;
  const dReal* loc = dBodyGetPosition( body );
  
  ssa << "sphere{<"
      << loc[0] << "," 
      << loc[1] << ","
      << loc[2] << ">,1";
  string tempa_str;
  ssa >> tempa_str;
  
  string tempb_str = " texture { pigment { color rgbt<";
  
  ssc << (double)red/255.0 << "," 
      << (double)green/255.0 << "," 
      << (double)blue/255.0 << "," 
      << (double)alpha/255.0 << ">}}}";
  string tempc_str;
  ssc >> tempc_str;
  
  str = tempa_str + tempb_str + tempc_str;
  
  return str;
}


bool CatomSim::isMobile() {
  return true;  // Everything is mobile for now
}


bool CatomSim::isCollided( CatomSim* S ) {
  const dReal* loc = dBodyGetPosition( body );
  if( ( dGeomSpherePointDepth( S->shape, loc[0], loc[1], loc[2] ) 
	+ catom_radius )  > EPSILON )
    return true;
  else
    return false;
}

bool CatomSim::isTouching( CatomSim* S ) {
  const dReal* loc = dBodyGetPosition( body );
  if( ( dGeomSpherePointDepth( S->shape, loc[0], loc[1], loc[2] ) 
	+ catom_radius )  > -EPSILON )
    return true;
  else
    return false;
}

///////////////////////////////////////////////////////////////////////////////
// Neighbors update functions
//

static void nearNeighbor( void *data, dGeomID o1, dGeomID o2 ) {
  CatomSim *c1 = (CatomSim*) dGeomGetData( o1 );
  CatomSim *c2 = (CatomSim*) dGeomGetData( o2 );
  if (c1==c2) return;
  const dReal* loc = dBodyGetPosition( c1->body );
  double dist = -dGeomSpherePointDepth( c2->shape, loc[0], loc[1], loc[2] );
  if ( dist < 2 * c1->catom_radius ) {
    c1->nearCatoms.insert(c2);
    c2->nearCatoms.insert(c1);
    //    cerr << "P: " << c1 << " " << c2 << "\n";
  }
  if ( dist > c1->catom_radius+EPSILON || dist < c1->catom_radius-EPSILON )
    return;
  dVector3 apos;
  Point3D rpos;
  unsigned int i;
  int id1=0, id2=0;
  double dist1=c1->catom_radius, dist2=c1->catom_radius;
  for (i=1; i<=NUM_FEATURES; i++) {
    rpos = (c1->C.getFeatureMap())[i].getLocation();
    dBodyGetRelPointPos( c1->body, c1->catom_radius*rpos.getX(), 
			 c1->catom_radius*rpos.getY(), c1->catom_radius*rpos.getZ(), apos );
    dist = -dGeomSpherePointDepth( c2->shape, apos[0], apos[1], apos[2] );
    if ( dist<dist1 && dist<0.2*c1->catom_radius ) { dist1=dist; id1=i; }
    dBodyGetRelPointPos( c2->body, c2->catom_radius*rpos.getX(), 
			 c2->catom_radius*rpos.getY(), c2->catom_radius*rpos.getZ(), apos );
    dist = -dGeomSpherePointDepth( c1->shape, apos[0], apos[1], apos[2] );
    if ( dist<dist2 && dist<0.2*c2->catom_radius ) { dist2=dist; id2=i; }
  }
/*
  if (id1 && id2) {
    if ( c1->C.addNeighbor( id1, c2->C.getID() )==-1 ) return;
    if ( c2->C.addNeighbor( id2, c1->C.getID() )==-1 ) 
      c1->C.removeNeighbor( c2->C.getID() );
    //    else cerr << "N: " << c1 << " " << c2 << "\n";
  }
*/
  c1->C.addNeighbor( id1, c2->C.getID(), id2 );
  c2->C.addNeighbor( id2, c1->C.getID(), id1 );

}

void CatomSim::updateAllNeighbors() {
  hash_map<const unsigned long, CatomSim *, hash<const unsigned long>, equl>::iterator it;
  for ( it=worldPtr->catomHash.begin(); it!=worldPtr->catomHash.end(); it++ ) {
    it->second->C.clearNeighbors();
    it->second->nearCatoms.clear();
//    dGeomSetCategoryBits( (dGeomID) it->second->feature_container, 0x02 );
//    dGeomSetCollideBits( (dGeomID) it->second->feature_container, 0x02 );
  }
  dSpaceCollide( worldPtr->neigh_space, 0, nearNeighbor );
}



// Functions to update Neighbors List before and after magic moves
// You better be holding a lock on the world or you could be very sorry

void CatomSim::preMoveUnlinkNeighbors() {
  set<CatomSim*>::iterator it;
  for ( it=nearCatoms.begin(); it!=nearCatoms.end(); it++ ) {
    (*it)->nearCatoms.erase( this );
  }
  nearCatoms.clear();
  unsigned int i;
  catomID cid;
  for ( i=0; i<NUM_NEIGHBORS; i++ ) {
    cid = C.getNthNeighbor(i);
    if ( cid==0 ) break;
    worldPtr->catomHash[cid]->C.removeNeighbor( C.getID() );
  }
  C.clearNeighbors();
}

void CatomSim::postMoveRelinkNeighbors() {
  dSpaceCollide2( shape2, (dGeomID) worldPtr->neigh_space, 0, nearNeighbor );
}


///////////////////////////////////////////////////////////////////////////////
// Add magnet and magic forces functions
//

static void nearMagnet( void *data, dGeomID o1, dGeomID o2 ) {
  if ( dGeomIsSpace(o1) || dGeomIsSpace(o2) ) {
    //if ( !dGeomIsSpace(o1) || !dGeomIsSpace(o2) ) cerr<<"HERE\n";
    dSpaceCollide2( o1, o2, data, &nearMagnet );
  }
  CatomSim *c1 = (CatomSim*) dGeomGetData( o1 );
  CatomSim *c2 = (CatomSim*) dGeomGetData( o2 );
  if (c1==c2) return; //{ cerr<< "HERE1\n"; return; }
  if ( dGeomGetClass( o1 ) != dGeomTransformClass || 
       dGeomGetClass( o2 ) != dGeomTransformClass ) 
    return; //{ cerr<< "HERE2\n"; return; }
  dGeomID m1 = dGeomTransformGetGeom( o1 );
  dGeomID m2 = dGeomTransformGetGeom( o2 );
  Point3D p1 = Point3D( dGeomGetPosition( m1 ) );
  Point3D p2 = Point3D( dGeomGetPosition( m2 ) );
  dBodyID b1 = dGeomGetBody( o1 );
  dBodyID b2 = dGeomGetBody( o2 );
  dVector3 loc;
  dBodyGetRelPointPos( b1, p1.getX(), p1.getY(), p1.getZ(), loc );
  p1 = Point3D( loc );
  dBodyGetRelPointPos( b2, p2.getX(), p2.getY(), p2.getZ(), loc );
  p2 = Point3D( loc );
  Point3D fv = p1-p2;
  double dist = fv.norm();
  if (dist>2*CatomSim::magnet_radius) return; //{ cerr<< "HERE3\n"; return; }
  long f1 = (long) dGeomGetData( m1 );
  long f2 = (long) dGeomGetData( m2 );
  if ( (c1->magnet_state[f1] * c2->magnet_state[f2]) == 0 ) return;
  double fs;
  if (dist>=2*CatomSim::magnet_depth)
    fs  = CatomSim::magnet_max_force * CatomSim::magnet_depth 
          * CatomSim::magnet_depth * 4 / (dist * dist);
  else fs = CatomSim::magnet_max_force * dist / (CatomSim::magnet_depth * 2);
  if (fs > CatomSim::magnet_max_force) fs = CatomSim::magnet_max_force;
  fv *= ((double)(c1->magnet_state[f1] * c2->magnet_state[f2])) * fs / dist;
  dBodyAddForceAtPos( b1, fv.getX(), fv.getY(), fv.getZ(),
		      p1.getX(), p1.getY(), p1.getZ() );
  dBodyAddForceAtPos( b2, -fv.getX(), -fv.getY(), -fv.getZ(),
		      p2.getX(), p2.getY(), p2.getZ() );
  //  cerr << "M: " << c1 << " " << f1 << " " << c2 << " " << f2 << "\n";
}

void CatomSim::applyMagnetStates() {
  //return;
  hash_map<const unsigned long, CatomSim *, hash<const unsigned long>, equl>::iterator it;
  unsigned int i;
  bool any_on;
  for ( it=worldPtr->catomHash.begin(); it!=worldPtr->catomHash.end(); it++ ) {
    any_on = false;
    for ( i=1; i<=NUM_FEATURES; i++ ) {
      if ( it->second->magnet_state[i] ) {
        dGeomSetCategoryBits( it->second->feature_points[i], 0x01 );
        dGeomSetCollideBits( it->second->feature_points[i], 0x01 );
        any_on = true;
      } else {
        dGeomSetCategoryBits( it->second->feature_points[i], 0x00 );
        dGeomSetCollideBits( it->second->feature_points[i], 0x00 );
      }
    }
    if ( any_on) {
      dGeomSetCategoryBits( (dGeomID) it->second->feature_container, 0x02 );
      dGeomSetCollideBits( (dGeomID) it->second->feature_container, 0x02 );
    } else {
      dGeomSetCategoryBits( (dGeomID) it->second->feature_container, 0x00 );
      dGeomSetCollideBits( (dGeomID) it->second->feature_container, 0x00 );
    }
  }
}

void CatomSim::applyAllMagnetForces() {
  dSpaceCollide( worldPtr->mag_space, 0, &nearMagnet );
}

void CatomSim::applyAllMagicForces() {
  hash_map<const unsigned long, CatomSim *, hash<const unsigned long>, equl>::iterator it;
  for ( it=worldPtr->catomHash.begin(); it!=worldPtr->catomHash.end(); it++ ) {
    dBodyAddForce( it->second->body, it->second->magic_force.getX(), 
		   it->second->magic_force.getY(), it->second->magic_force.getZ() );
    dBodyAddTorque( it->second->body, it->second->magic_torque.getX(), 
		    it->second->magic_torque.getY(), it->second->magic_torque.getZ() );
  }
}

