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

#include "Catom.hxx"
#include "CatomSim.hxx"
#include "CatomWorld.hxx"
#include "Util.hxx"

#include <errno.h>

extern CatomWorld *worldPtr;



#if 0
//Functions for Catom message passthrough
Point3D calcOffsetFromFeatureCubic(featureID origin, featureID dest, Catom* c)
{
  Point3D oloc = c->getLocation(origin);
  Point3D dloc = c->getLocation(dest);
  
  Point3D offset = dloc-oloc;
  debugMsg("OffsetFromFeature: %d -> %d => %s", origin, dest, offset.toString());
  return offset;
}


featureID calcFeatureFromOffsetCubic(featureID recieved, RelPoint3D offset, Catom* c)
{
  Point3D rloc = c->getLocation(recieved);
  //  cout<<"recieved at "<<recieved<<" at "<<rloc.getX()<<" "<<rloc.getY()<<" "<<rloc.getZ()<<endl;
  Point3D result = -rloc + offset;
  //  cout<<"resultant vector = "<<result.getX()<<" "<<result.getY()<<" "<<result.getZ()<<endl;
  featureID nf = c->getNearestFeature(result+c -> getLocation());
  debugMsg("FeatureFromOffset: %d -> %s => %d", recieved, offset.toString(), nf);
  return nf;
}
#endif


///////////////////////////////////////////////////////////////////////////////
//                                                                           //
// MailboxManager Implementations                                            //
//                                                                           //
///////////////////////////////////////////////////////////////////////////////

MailboxManager::MailboxManager() {
#ifndef __CYGWIN__
  pthread_mutexattr_t mutex_attrs;
  pthread_mutexattr_init(&mutex_attrs);
  pthread_mutexattr_settype(&mutex_attrs, PTHREAD_MUTEX_RECURSIVE);
  pthread_mutex_init(&boxesLock, &mutex_attrs);
  pthread_mutexattr_destroy(&mutex_attrs);
#else
  boxesLock = PTHREAD_RECURSIVE_MUTEX_INITIALIZER_NP;
#endif
}

MailboxManager::~MailboxManager() {
  lock();
  
  // Clear all messages from mailboxes
  map<MailboxID, queue<Message*> >::iterator iter = mailboxes.begin();
  while( iter != mailboxes.end() ) {
    MailboxID boxID = iter->first;
    
    while(!mailboxes[boxID].empty()) {
      delete mailboxes[boxID].front();
      mailboxes[boxID].pop();
    }
    
    iter++;
  }

  // Clear all messages from messagesForNextTick
  iter = messagesForNextTick.begin();
  while( iter != messagesForNextTick.end() ) {
    MailboxID boxID = iter->first;
    
    while(!messagesForNextTick[boxID].empty()) {
      delete messagesForNextTick[boxID].front();
      messagesForNextTick[boxID].pop();
    }
    
    iter++;
  }

  unlock();

  pthread_mutex_destroy(&boxesLock);
}

void MailboxManager::lock() {
  int err = pthread_mutex_lock(&boxesLock);
  if(err) {
    cerr << "can't lock mutex : " << strerror(errno) << endl;
    exit(1);
  }
}

void MailboxManager::unlock() {
  int err = pthread_mutex_unlock(&boxesLock);
  if(err) {
    cerr << "can't unlock mutex : " << strerror(errno) << endl;
    exit(1);
  }
}

// Release the messagesForNextTick
void MailboxManager::newTick() {
  lock();
  
  map<MailboxID, queue<Message*> >::iterator iter = messagesForNextTick.begin();
  
  // might be nice to introduce some randomness into this beyond
  // just that which comes from the hash table
  while( iter != messagesForNextTick.end() ) {
    MailboxID boxID = iter->first;
    
    while(!messagesForNextTick[boxID].empty()) {
      mailboxes[boxID].push(messagesForNextTick[boxID].front());
      messagesForNextTick[boxID].pop();
    }
    
    iter++;
  }
  
  unlock();
}

///////////////////////////////////////////////////////////////////////////////
// MailboxManager::fileMessage(Message *)
//
// Purpose: Inserts the given message into the appropriate queue.
//
// Returns: Success status of the insertion.
//

bool MailboxManager::fileMessage( Message* msg ) {
  // If no delay, handle right now
  if(!boxHasDelay[msg->mailboxID])
    return handleMessage(msg);
  
  // Otherwise, queue in messagesForNextTick for later handling
  
  lock();

  messagesForNextTick[msg->mailboxID].push(msg);
  
  unlock();
  
  return true;
}

///////////////////////////////////////////////////////////////////////////////
// MailboxManager::handleMessage()
//
// Purpose: Calls handler for any arbitrary message.  
//          Called from within catom code OS thread.
// 
// Returns: whether a message was handled
//

bool MailboxManager::handleMessage() {
  map<MailboxID, queue<Message*> >::iterator iter = mailboxes.begin();
  
  // might be nice to introduce some randomness into this beyond
  // just that which comes from the hash table
  while( iter != mailboxes.end() ) {
    if( handleMessage(iter->first) )
      return true;
    iter++;
  }
  
  return false;
}

///////////////////////////////////////////////////////////////////////////////
// MailboxManager::handleMessage(MailboxID)
//
// Purpose: Calls handler for a particular mailbox
//
// Returns: whether a message was handled
//

bool MailboxManager::handleMessage( MailboxID boxID ) {
  lock();
  
  if(mailboxes[boxID].empty()) {
    unlock();
    return false;
  }

  Message* msg = mailboxes[boxID].front();
  mailboxes[boxID].pop();

  unlock();

  return handleMessage(msg);
}

///////////////////////////////////////////////////////////////////////////////
// MailboxManager::handleMessage(Message *)
//
// Purpose: Immediately calls hander for a particular message.  
//          Doesn't involve mailboxes, just dispatches
//
// Returns: whether the message was handled
//

bool MailboxManager::handleMessage( Message* msg ) {
  //worldPtr->oStart();
  //cerr << "Received message in handleMessage\n";
  //worldPtr->oEnd();
  if( !msg )
    return false;
  
  pair<CodeModule*, msgHandlerPtr> handler = messageHandlers[msg->mailboxID];
  
  if( !handler.first || !handler.second ) {
    worldPtr->oStart();
    cerr << "No CodeModule/handler found to handle message for " << msg->mailboxID <<"!  Make sure you register your handlers (usually in simulationStart()) before sending any messages of this type.\n";
    worldPtr->oEnd();
    exit(-1);
  }
  
  if(!(handler.first->*(handler.second))(msg)) {
    delete msg;
  }
  
  return true;
}

///////////////////////////////////////////////////////////////////////////////
// MailboxManager::registerHandler(MailboxID, CodeModule*, msgHandlerPtr)
//
// Purpose: Defines the handler for a particular mailbox name
//

void MailboxManager::registerHandler( MailboxID boxName, CodeModule* module, 
				      msgHandlerPtr fnPtr, bool hasDelay ) {
  make_pair(module, fnPtr);
  lock();
  messageHandlers[boxName] = make_pair( module, fnPtr );
  boxHasDelay[boxName] = hasDelay;
  unlock();
}


///////////////////////////////////////////////////////////////////////////////
//                                                                           //
// END MailboxManager Implementation                                         //
//                                                                           //
///////////////////////////////////////////////////////////////////////////////


SpeculationManager::SpeculationManager(catomID _hostCatom) : hostCatom(_hostCatom) {
  for(featureID source=1; source<= NUM_FEATURES; source++) {
    for(featureID dest=0; dest<=NUM_FEATURES; dest++) {
      history[source][dest] = 0;
    }
    lastUsed[source]=0xffffffffffffffffull;
  }
  
  specLastUsedCorrect = 0;
  specLastUsedWrong = 0;
  specMostUsedCorrect = 0;
  specMostUsedWrong = 0;
}

SpeculationManager::~SpeculationManager() {

}

void SpeculationManager::newTick() {
  // Clear out stats after first aggregations
  if(worldPtr->current_time == 124) {
    specLastUsedCorrect = 0;
    specLastUsedWrong = 0;
    specMostUsedCorrect = 0;
    specMostUsedWrong = 0;
  }
}

void SpeculationManager::simulationEnd() {
  string speculationDebug = worldPtr->search_key_value_list("DEBUGSPECULATION");
  if(speculationDebug == "1") {
    dumpStats();
  }
}

void SpeculationManager::dumpStats() {
  worldPtr->oStart();
  
  // Basic routing stats
  cerr << worldPtr->current_time << " Routing " << hostCatom << ":";
  for(featureID source=1; source<= NUM_FEATURES; source++) {
    cerr << " source " << source << "( ";
    
    for(featureID dest=0; dest<=NUM_FEATURES; dest++) {
      if(history[source][dest]) {
	cerr << dest << ":" << history[source][dest] << " ";
	history[source][dest] = 0;
      }
    }
    
    cerr << ")";
  }
  cerr << endl;

  // Speculation approaches
  cerr << worldPtr->current_time << " Speculation " << hostCatom << ":";
  cerr << " lastUsed(" << specLastUsedCorrect << " " << specLastUsedWrong << ")";
  cerr << " mostUsed(" << specMostUsedCorrect << " " << specMostUsedWrong << ")";
  cerr << endl;
  
  worldPtr->oEnd();
}

featureID SpeculationManager::speculateDestFromSource(featureID source) {
  // TODO: do something brilliant here
  return 0;
}

void SpeculationManager::messageRouted(featureID source, featureID dest) {
  if(lastUsed[source] != 0xffffffffffffffffull) {
    if(lastUsed[source] == dest)
      specLastUsedCorrect++;
    else
      specLastUsedWrong++;
  }

  featureID mostUsedGuess = 0;
  unsigned long long mostUsedCount = lastUsed[0];
  for(featureID fid = 1; fid<=NUM_FEATURES; fid++) {
    if(history[source][fid] > mostUsedCount) {
      mostUsedGuess = fid;
      mostUsedCount = history[source][fid];
    }
  }
  if(mostUsedCount > 0) {
    if(mostUsedGuess == dest)
      specMostUsedCorrect++;
    else
      specMostUsedWrong++;
  }

  lastUsed[source] = dest;
  history[source][dest]++;
}

///////////////////////////////////////////////////////////////////////////////
//                                                                           //
// Catom Method Implementation                                               //
//                                                                           //
// The following methods are all that are available to the Catom             //
// programmer in their Code Modules                                          //
//                                                                           //
// Catom::Catom(CatomID)                                                     //
// Catom::~Catom()                                                           //
//                                                                           //
// Catom::getID()                                                            //
// Catom::getLocation()                                                      //
// Catom::getBatteryLife()                                                   //
// Catom::getNeighborCenter()                                                //
// Catom::getNeighbors()                                                     //
// Catom::getNeighbor(featureID)                                             //
//                                                                           //
// Catom::amPowered()                                                        //
// Catom::amOnFloor()                                                        //
// Catom::amOnSurface()                                                      //
// Catom::amMobile()                                                         //
//                                                                           //
// Catom::moveTo(Catom*, featureID)                                          //
//                                                                           //
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
// Catom::Catom(unsigned long)
// 
// Purpose: Catom object constructor
//

Catom::Catom( catomID i ) : speculationManager(i) {
	ID = i;
	batteryLife = 1;
  map = new Feature[NUM_FEATURES+1];
  neighbors = new featureID[NUM_NEIGHBORS];
  neighborID = new catomID[NUM_NEIGHBORS];
  sel = false;
  get_feature_map( LATTICE_TYPE, i, map );
}

///////////////////////////////////////////////////////////////////////////////
// Catom::~Catom()
//
// Purpose: Catom object destructor
//

Catom::~Catom() {
  delete [] map;
  delete [] neighbors;
  delete [] neighborID;
  //  delete &mailboxManager;
  //delete &speculationManager;
}

///////////////////////////////////////////////////////////////////////////////
// Catom::getID()
//
// Returns: Unique identifier for this catom
//

catomID Catom::getID() {
  return ID;
}

///////////////////////////////////////////////////////////////////////////////
// Catom::getFeatureMap()
//
// Returns: Feature map
//

Feature* Catom::getFeatureMap() {
  return map;
}

///////////////////////////////////////////////////////////////////////////////
// Catom::getFeature(featueID f)
//
// Returns my feature with featureID f
//

Feature* Catom::getFeature(featureID f) {
  return &(map[f]);
}

///////////////////////////////////////////////////////////////////////////////
// Catom::getLocation()
//
// Returns: Location of current catom
//

WorldPoint3D Catom::getLocation() {
//  const dReal* loc = dBodyGetPosition( worldPtr->catomHash[ID]->body );
//  return Point3D( loc[0], loc[1], loc[2] );
  return WorldPoint3D( dBodyGetPosition( worldPtr->catomHash[ID]->body ) );
}

///////////////////////////////////////////////////////////////////////////////
// Catom::getBatteryLife()
//
// Returns: The internal value for batteryLife
//

double Catom::getBatteryLife() {
  return batteryLife;
}

///////////////////////////////////////////////////////////////////////////////
// Catom::getNeighbors()
//
// Returns: The internal value for batteryLife
//

void Catom::getNeighbors() {
  // This is depreciated!
}

///////////////////////////////////////////////////////////////////////////////
// Catom::getNeighborCenter()
//
// Returns: The Location of a neighbor attached to my feature f
//          (Even if the nieghbor doesn't exist!)
//

Point3D Catom::getNeighborCenter( featureID f ) {
  Point3D fl = map[f].getLocation();
  double r = 2*CatomSim::catom_radius;
  dVector3 loc;
  dBodyGetRelPointPos( worldPtr->catomHash[ID]->body,
		r*fl.getX(), r*fl.getY(), r*fl.getZ(), loc );
  return Point3D( loc[0], loc[1], loc[2] );
}

///////////////////////////////////////////////////////////////////////////////
// Catom::getNearestFeature( Point3D p, catomID hostid=0 )
//
// Returns: FeatureID of feature closest to absolute location p
//
//

featureID Catom::getNearestFeature( Point3D p, catomID hostid ) {
  unsigned int i, best;
  double dist, bestdist=1000000;
  if ( hostid==0 ) hostid=ID;
  CatomSim* c = worldPtr->catomHash[hostid];
  Feature* featuremap = c->C.getFeatureMap();
  Point3D rloc, absloc;
  dVector3 loc;
  for (i=1; i<=NUM_FEATURES; i++) {
    rloc = featuremap[i].getLocation();
    dBodyGetRelPointPos( c->body, rloc.getX(), rloc.getY(), rloc.getZ(), loc );
    absloc = Point3D( loc );
    dist = absloc.distanceFrom( p );
    if (dist<bestdist) {
      bestdist=dist;
      best = i;
    }
  }
  return best;
}

///////////////////////////////////////////////////////////////////////////////
// Catom::getFeatureTouching( neighborid,  hostID=0  )
//
// Returns: feature touching catom neighborid
//
//

featureID Catom::getFeatureTouching( catomID neighborid, catomID hostID ) {
  unsigned int i;
  for ( i=0; i<NUM_NEIGHBORS; i++ )
    if ( neighborID[i] == neighborid ) return neighbors[i];
  return 0;
}

///////////////////////////////////////////////////////////////////////////////
// Catom::setColor(r,g,b,a)
//
// Set the visual color of the catom.  Valid input values are [0..1]
//

void Catom::setColor( uint8 r, uint8 g, uint8 b, uint8 a ) {
  worldPtr->catomHash[ID]->red = r;
  worldPtr->catomHash[ID]->green = g;
  worldPtr->catomHash[ID]->blue = b;
  worldPtr->catomHash[ID]->alpha = a;
  return;
}


///////////////////////////////////////////////////////////////////////////////
// Catom::getNeighbor(featureID)
//
// Returns: Pointer to the Catom object in contact with feature f
//          (or NULL if there is no Catom)
//

catomID Catom::getNeighbor( featureID f ) {
  unsigned int i;
  for (i=0; i<NUM_NEIGHBORS; i++) {
    if (neighbors[i]==f) return neighborID[i];
  }
  return 0;
}


///////////////////////////////////////////////////////////////////////////////
// Catom::clearNeighbors()
// empties neighbor list.  should not normally be used, only when moving
//	should be called by magic move, physics, etc.

void Catom::clearNeighbors() {
  unsigned int i;
  for (i=0; i<NUM_NEIGHBORS; i++) {
    neighbors[i]=0;
    neighborID[i]=0;
  }
  for (i=1; i<=NUM_FEATURES; i++) {
    map[i].clearRemoteFeatures();
  }
}


///////////////////////////////////////////////////////////////////////////////
// Catom::removeNeighbor( catomID targetCatom )
// removes neighbor from list if it is there.  
//      should not normally be used, only when moving
//	should be called by magic move, physics, etc.

void Catom::removeNeighbor( catomID targetCatom ) {
  unsigned int i,j;
  for (i=0,j=0; i<NUM_NEIGHBORS; i++,j++) {
    if ( neighborID[i]==targetCatom) {
      map[neighbors[i]].clearRemoteFeatures();
      j++;
    } 
    if (j>=NUM_NEIGHBORS) {
      neighbors[i]=0;
      neighborID[i]=0;
    } else if (i!=j) {
      neighbors[i] = neighbors[j];
      neighborID[i] = neighborID[j];
    }
  }
}


///////////////////////////////////////////////////////////////////////////////
// Catom::addNeighbor( featureID f, catomID targetCatom, featureID t_f )
// adds neighbor to the list.  
//      returns -1 if feature already present
//      should not normally be used, only when moving
//	should be called by magic move, physics, etc.

int Catom::addNeighbor( featureID f, catomID targetCatom, featureID t_f ) {
  unsigned int i;
  map[f].addRemoteFeature( &(worldPtr->catomHash[targetCatom]->C.map[t_f]) );
  for (i=0; i<NUM_NEIGHBORS; i++) {
    if (neighbors[i]==0) {
      neighbors[i]=f;
      neighborID[i]=targetCatom;
      return 0;
    }
    if (neighbors[i]==f) break;
  }
  return -1;
}


///////////////////////////////////////////////////////////////////////////////
// Catom::setBatteryLife(double)
//
// Purpose: Manually updates internal battery Data with supplied value
//

void Catom::setBatteryLife( double b ) { 
  batteryLife = b; 
}

///////////////////////////////////////////////////////////////////////////////
// Catom::amPowered()
//
// Returns: True if batteryLife is non-zero
//

bool Catom::amPowered() {
  return (batteryLife > 0);
}

///////////////////////////////////////////////////////////////////////////////
// Catom::amOnFloor()
//
// Purpose: In reality this will be done through some sensing, but this is
//          sufficient in simulation. Checks to see if your center is within 
//          one Catom radius of the floor
//
// Returns: True if this catom is on the floor
//

bool Catom::amOnFloor() {
  return ( dBodyGetPosition( worldPtr->catomHash[ID]->body )[2] < (CatomSim::catom_radius + EPSILON));
}

///////////////////////////////////////////////////////////////////////////////
// Catom::amOnSurface()
//
// Purpose: In reality this may be more sophisticated based on the contents 
//          of neighbors[] but for now it just checks that you are in contact 
//          with less than NUM_FEATURES/2 neighbors
//
// Returns: True if this catom is on the surface of the emsemble
//

bool Catom::amOnSurface() {
  // Update the neighbor list
  getNeighbors();
  
  // Check to see if less than half of your features are active
  if( neighbors[NUM_FEATURES/2] == 0 )
    return true;
  else
    return false;
}

///////////////////////////////////////////////////////////////////////////////
// Catom::amMobile()
//
// Purpose: The same as amOnSurface() for now.
//
// Returns: True if this catom has any possible move
//

bool Catom::amMobile() {
  return amOnSurface();
}

///////////////////////////////////////////////////////////////////////////////
// Catom::moveTo(catomID targetCatom, featureID targetFeature)
//
// Purpose: Actually move the Catom in the simulated world.
//
// There are no rules associated with the values for Catom* and featureID.
// Therefore, we call this Magic-Move.  When you call this function you WILL
// be moved to the location you ask for instantly.
//
// In the future the simulator may only allow target to be a catom you are 
// currently in contact with.  And more agressively, dest may be forced to 
// only be a direct featureNeighbor on the FM, only allowing small, direct 
// hops.
// 
// Returns: True if the move was successful
//
// WARNING: You ought to hold a lock on the world when using this function
//  or restrict concurrent use appropriately.  Otherwise BAD THINGS can happen.
//
// Automatic locking features use two flags for lock and unlock:
//   true true (default): lock and unlock
//   true false:  lock, and leave locked
//   false false: leave lock alone
//   false true:  don't call lock, but unlock on exit
// This allows doing multiple magic move calls atomically with respect
//   to other magic moves
//

static pthread_mutex_t magic_move_lock = PTHREAD_MUTEX_INITIALIZER;

bool Catom::moveTo( catomID target, featureID tarFeat, 
                    bool roll, bool lock, bool unlock ) {
  
  if (lock) pthread_mutex_lock(&magic_move_lock);
  // Calculate new real location
  Point3D newLoc = worldPtr->catomHash[target]->C.getNeighborCenter(tarFeat);
  // Update orientation as needed
  //  This assumes sphere-in-sphere rolling about an immediate neighbor
  if (roll) {
    dQuaternion orient, rot;
    Point3D curLoc = getLocation();
    Point3D neiLoc = worldPtr->catomHash[target]->C.getLocation();
    double lenA = (curLoc-neiLoc).norm();
    double lenB = (newLoc-neiLoc).norm();
    double lenC = (newLoc-curLoc).norm();
    if ( lenC < lenA+lenB ) {
      Point3D axis = (curLoc-neiLoc).cross(newLoc-neiLoc);
      axis /= axis.norm();
      double angle = acos( (lenA*lenA + lenB*lenB - lenC*lenC)/(2*lenA*lenB) );
      dQFromAxisAndAngle( rot, axis.getX(), axis.getY(), axis.getZ(), 2*angle );
      dQMultiply0( orient, rot, 
         dBodyGetQuaternion( worldPtr->catomHash[ID]->body ) );
      return moveTo( newLoc, orient, false, unlock );
    }
  }
  return moveTo( newLoc, NULL, false, unlock );
}

// Warp to location variants

bool Catom::moveTo( Point3D dest, dQuaternion orient, bool lock, bool unlock ) {
  return moveTo( dest.getX(), dest.getY(), dest.getZ(), orient, lock, unlock );
}

bool Catom::moveTo( double dest_x, double dest_y, double dest_z, 
                    dQuaternion orient, bool lock, bool unlock ) {

  if (lock) pthread_mutex_lock(&magic_move_lock);
  // Collision Detection Here --CJH
  // If collided, do not update position and return false.
  
  // Remove old neighbors from this catom's list and vice versa
  worldPtr->catomHash[ID]->preMoveUnlinkNeighbors();
  // Update CatomSim location
  dBodySetPosition(  worldPtr->catomHash[ID]->body, dest_x, dest_y, dest_z );
  // Kill velocities
  dBodySetLinearVel( worldPtr->catomHash[ID]->body, 0, 0, 0 );
  dBodySetAngularVel( worldPtr->catomHash[ID]->body, 0, 0, 0 );
  // Update orientation if needed
  if (orient) {
    dBodySetQuaternion( worldPtr->catomHash[ID]->body, orient );
  }
  // Update new neighbors
  worldPtr->catomHash[ID]->postMoveRelinkNeighbors();
  if ( unlock ) pthread_mutex_unlock(&magic_move_lock);

  return true;
}

int selectedCatom = 0;

void Catom::selected() {
  this->sel = !this->sel;
  selectedCatom = this->ID;
}

bool Catom::speak(int speakSize, char *speakBuf) {
  if (this->sel) {
    snprintf(speakBuf, speakSize, "GUID=%d (%3.3f,%3.3f,%3.3f)", this->getID(), this->getLocation().getX(), this->getLocation().getY(), this->getLocation().getZ());
    return true;
  }
  return false;
}
