#include <iostream>
#include <string.h>

#include "BoundingBox.hxx"
#include "CatomSim.hxx"

CODE_MODULE_DECLARATION( BoundingBox, BoundingBoxModule );

using namespace std;

#define BB_MAX_X      19
#define BB_MAX_REPX   10
#define BB_MIN_X     -19
#define BB_MIN_REPX  -10

#define BB_MAX_Y      19
#define BB_MAX_REPY   10
#define BB_MIN_Y     -9
#define BB_MIN_REPY   0

#define BB_MAX_Z      20
#define BB_MAX_REPZ   15
#define BB_MIN_Z       0
#define BB_MIN_REPZ    2

// Recommended to see this:
//   sBOX,0,20,15,0,0,0,80,80,0,100,10000,40,4,30
//   sBOX,0,-10,15,0,0,0,80,80,0,100,10000,40,4,30
//   sBOX,20,5,15,0,0,0,80,80,0,100,100000,4,30,30
//   sBOX,-20,5,15,0,0,0,80,80,0,100,100000,4,30,30



StateFile::Module* BoundingBoxModule::StateFileConstructor() { signpost dprdebug_block_signpost(__FILE__,__LINE__,__FUNCTION__); 

  myModule_ = StateFile::Module::getInstance( name_ );
  myModule_->setSkip( true );

  // Check to see if this module has already been setup
  if (myModule_->getVersion())
    return myModule_;

  // If not, set up this module for the first time
  myModule_->setVersion( 1 );
  myModule_->setVersionRange( 1 );

  return myModule_;
}

void BoundingBoxModule::newTick() { signpost dprdebug_block_signpost(__FILE__,__LINE__,__FUNCTION__); 
  
  Catom * me = &(worldPtr->catomHash[hostCatom]->C);

  dBodyID body = worldPtr->catomHash[hostCatom]->body;
  Point3D pos = me->getLocation();
  Point3D cfor;
  bool change = false;

  if ( pos.getX() > BB_MAX_X ) {
    pos.setX( BB_MAX_X - (pos.getX() - BB_MAX_X) );
    cfor.setX( dBodyGetForce(body)[0] / -2 );
    change = true;
  }
  if ( pos.getY() > BB_MAX_Y ) {
    pos.setY( BB_MAX_Y - (pos.getY() - BB_MAX_Y) );
    cfor.setY( dBodyGetForce(body)[1] / -2 );
    change = true;
  }
  if ( pos.getZ() > BB_MAX_Z ) {
    pos.setZ( BB_MAX_Z - (pos.getZ() - BB_MAX_Z) );
    cfor.setZ( dBodyGetForce(body)[2] / -2 );
    change = true;
  }

  if ( pos.getX() < BB_MIN_X ) {
    pos.setX( BB_MIN_X + (BB_MIN_X - pos.getX() ) );
    cfor.setX( dBodyGetForce(body)[0] / -2 );
    change = true;
  }  
  if ( pos.getY() < BB_MIN_Y ) {
    pos.setY( BB_MIN_Y + (BB_MIN_Y - pos.getY() ) );
    cfor.setY( dBodyGetForce(body)[1] / -2 );
    change = true;
  }
  if ( pos.getZ() < BB_MIN_Z ) {
    pos.setZ( BB_MIN_Z + (BB_MIN_Z - pos.getZ() ) );
    cfor.setZ( dBodyGetForce(body)[2] / -2 );
    change = true;
  }

  if (change) {
    dBodySetPosition( body, pos.getX(), pos.getY(), pos.getZ() );
    dBodyAddForce( body, cfor.getX(), cfor.getY(), cfor.getZ() );
  }

}

