#include <Inventor/nodes/SoAnnotation.h>
#include <Inventor/nodes/SoBaseColor.h>
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoDrawStyle.h>
#include <Inventor/nodes/SoLightModel.h>
#include <Inventor/nodes/SoLineSet.h>
#include <Inventor/nodes/SoMaterial.h>
#include <Inventor/nodes/SoMatrixTransform.h>
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoSphere.h>
#include <Inventor/nodes/SoTranslation.h>
#include <assert.h>
#include <Inventor/cve/SoComputer.h>
#include <Inventor/cve/SoNetwork.h>
#include "Ball.h"
#include "CollisionAssert.h"


extern float tableSize;
extern float tableSize2;
float Ball::radius = 1;
static SoSeparator *circle = NULL;
extern int ballCommits;
extern int ballAborts;
extern int abortSteps;
extern int numCollisions;
extern SbBool abortStepDone;
extern SbList<Ball*> ballList;
extern Ball *myBall;


CollisionAssert* Ball::collisionAssert = NULL;

SB_SCENE_ASSERT_SOURCE(CollisionAssert);



SbName commitCbName = "BallCommitCallback";
SbName abortCbName = "BallAbortCallback";

static SbBool collisionHappened = FALSE;
static int collisionBall1;
static int collisionBall2;
static SbTimeStamp collisionNextTS;



static int debugTransactions = -1;

int cve_debugTransactions()
{
  if (debugTransactions != -1)
    return debugTransactions;

  const char *val = getenv("CVE_DEBUG_TRANSACTIONS");
  debugTransactions = val ? atoi(val) : 0;

  return debugTransactions;
}


static void setCommitTimeStamp(SoField *f, const SbTimeStamp &ts)
{
  if (cve_debugTransactions() >= 3) {
    printf("Changing commit timestamp of 0x%x to %s.\n", uint32_t(f), ts.getString().getString());
    printf("Required TS: %s, largest TS: %s, currentCommit TS: %s.\n", ts.getString().getString(), ts.getComputer()->getLargestTimeStamp().getString().getString(), f->getCveTimeStamp(SoField::COMMITTED).getString().getString());
  }

  // set timestamp
  f->setCommitTimeStamp(ts);
  ts.getComputer()->updateLargestTimeStamp(ts);
  assert(ts.getComputer()->getLargestTimeStamp() >= ts);
  
  SbReadSetItemList &rl = f->getCveStorage()->readList;
  int i,c = rl.getLength();
  if (cve_debugTransactions() >= 3)  printf("ReadListLength: %i.\n", c);
  
  for (i=0; i<c; i++) {
    SbTransaction *t = rl[i]->transaction;

    if (cve_debugTransactions() >= 3) {
      printf("%i before:\n%s ", i, t->getTimeStamp().getString().getString());
      t->dumpTransactionCPs();
    }

    // update commit predictors
    rl[i]->updateCommitPredictor();
    if (t->updateReadSetCommitPredictor())
      if (t->updateCurrentCommitPredictor())
        t->propagateCommitPredictor();

    if (cve_debugTransactions() >= 3) {
      printf("%i after:\n%s ", i, t->getTimeStamp().getString().getString());
      t->dumpTransactionCPs();
      printf(".\n");
    }
  }
}



static void onCommit(SbTransaction *tr, const SbName &callbackName, void *buf, size_t bufsize)
{
  ballCommits++;
  
  assert(collisionHappened == FALSE);
}
static void onAbort(SbTransaction *tr, const SbName &callbackName, void *buf, size_t bufsize)
{
  ballAborts++;
  if (!abortStepDone) {
    abortSteps++;
    abortStepDone = TRUE;
  }

  if (collisionHappened) {
    collisionHappened = FALSE;
    numCollisions++;

    setCommitTimeStamp(&ballList[collisionBall1]->matrix->matrix, collisionNextTS);
    setCommitTimeStamp(&ballList[collisionBall1]->speedVec->translation, collisionNextTS);
    setCommitTimeStamp(&ballList[collisionBall2]->matrix->matrix, collisionNextTS);
    setCommitTimeStamp(&ballList[collisionBall2]->speedVec->translation, collisionNextTS);
  }
}



SbBool CollisionAssert::check(const SbTransaction *t, SbBool checkForCommit)
{
#if 1
  int i,j;
  for (i=0; i<ballList.getLength(); i++)
    for (j=i+1; j<ballList.getLength(); j++) {
      
      // skip non-active balls
      if (!ballList[i]->isActive() || !ballList[j]->isActive())
        continue;

      ((SbTransaction*)t)->updateReadSetCommitPredictor(); // FIXME: This should not be neccessary, but there is some hidden bug somewhere. PCJohn-2007-07-06
      if (!t->getReadSetCommitPredictor())
        return FALSE;

      // get ball positions
      const SbMatrix &m1 = ballList[i]->matrix->matrix.getCveValue(t->getTimeStamp(), FALSE);
      SbVec3f pos1(m1[3][0], m1[3][1], m1[3][2]);
      const SbMatrix &m2 = ballList[j]->matrix->matrix.getCveValue(t->getTimeStamp(), FALSE);
      SbVec3f pos2(m2[3][0], m2[3][1], m2[3][2]);

      // compute collision
      if ((pos1-pos2).sqrLength() <= SbSqr(2*Ball::radius)) {
        
        if (!checkForCommit)
          return FALSE;

        SbTimeStamp nextTS(t->getTimeStamp().getTime(), t->getTimeStamp().getComputerIdentity(),
                           t->getTimeStamp().getCounter() + 1);

        // handle myBall
        if (ballList[i] == myBall || ballList[j] == myBall) {
          
          #if 0 // this is not possible because internal algorithms are not able to handle scheduling
          // of a new transaction during the processing of another transaction
          myBall->setPosAndSpeed(myBall->getPos(SoField::COMMITTED), -myBall->getSpeed(SoField::COMMITTED), SoField::COMMITTED);
          #else
          //SbVec3f speed = myBall->getSpeed();
          //SbRotation rot(speed, pos1-pos2);
          //speed = speed;
          //rot *=2;
          //rot.multVec(speed, speed);
          
          myBall->collisionHappened = TRUE;
          myBall->collisionPos = myBall->getPos(SoField::COMMITTED);
          myBall->collisionSpeed = -myBall->getSpeed(SoField::COMMITTED);
          #endif
        }
        
        assert(collisionHappened == FALSE);
        collisionHappened = TRUE;
        collisionBall1 = i;
        collisionBall2 = j;
        collisionNextTS = nextTS;

        return FALSE;
      }
    }
#endif

  return TRUE;
}



Ball::Ball()
{
  active = FALSE;
  collisionHappened = FALSE;
  createSceneGraph();
  static SbBool firstTime = TRUE;
  if (firstTime) {
    SbTransaction::registerCallback(commitCbName, &onCommit);
    SbTransaction::registerCallback(abortCbName, &onAbort);
    firstTime = FALSE;
  }
}


Ball::~Ball()
{
  destroySceneGraph();
}


SbVec3f Ball::getPos(int version) const
{
  assert(myGraph && "Call appendSceneGraph() first.");

  SbMatrix m = matrix->matrix.getCveValue(version);
  return SbVec3f(m[3][0], m[3][1], m[3][2]);
}


SbVec3f Ball::getSpeed(int version) const
{
  assert(myGraph && "Call appendSceneGraph() first.");

  return speedVec->translation.getCveValue(version);
}


SbTransaction* Ball::setPos(const SbVec3f &pos)
{
  assert(myGraph && "Call appendSceneGraph() first.");

  SbMatrix m = matrix->matrix.getValue();
  m[3][0] = pos[0];
  m[3][1] = pos[1];
  m[3][2] = pos[2];

  SbTransaction *t = new SbTransaction;
  SbIntList list;
  list.append(0);
  list.append(0);
  //printf("READ: numPredicted: %i, numTotal %i\n", 
  //       matrix->matrix.getCveNumValues(TRUE), matrix->matrix.getCveNumValues(FALSE));
  matrix->matrix.transactionRead(SoField::LATEST, t, this->getSceneGraph(), list, TRUE);
  matrix->matrix.transactionWrite(m, t, this->getSceneGraph(), list);
  t->appendAssert(collisionAssert);
  t->setCommitCallback(commitCbName);
  t->setAbortCallback(abortCbName);
  t->schedule();
  
  SbTimeStamp next = SbTimeStamp::generate(SoNetwork::getDefaultDistributionGroup());
  SbTimeStamp tmp(next.getTime(), next.getComputerIdentity(), next.getCounter()-1);
  assert(tmp==t->getTimeStamp());

  return t;
}


SbTransaction* Ball::setPosAndSpeed(const SbVec3f &pos, const SbVec3f &speed, int readVersionIndex)
{
  assert(myGraph && "Call appendSceneGraph() first.");

  SbMatrix m = matrix->matrix.getValue();
  m[3][0] = pos[0];
  m[3][1] = pos[1];
  m[3][2] = pos[2];

  SbTransaction *t = new SbTransaction;
  SbIntList list;
  list.append(0);
  list.append(0);
  matrix->matrix.transactionRead(readVersionIndex, t, this->getSceneGraph(), list, TRUE);
  matrix->matrix.transactionWrite(m, t, this->getSceneGraph(), list);
  list.set(1,(void*)3);
  speedVec->translation.transactionRead(readVersionIndex, t, this->getSceneGraph(), list, TRUE);
  speedVec->translation.transactionWrite(speed, t, this->getSceneGraph(), list);
  
  t->appendAssert(collisionAssert);
  t->setCommitCallback(commitCbName);
  t->setAbortCallback(abortCbName);
  t->schedule();
  
  SbTimeStamp next = SbTimeStamp::generate(SoNetwork::getDefaultDistributionGroup());
  SbTimeStamp tmp(next.getTime(), next.getComputerIdentity(), next.getCounter()-1);
  assert(tmp==t->getTimeStamp());

  return t;
}


SbTransaction* Ball::timeTick(SbTime dt)
{
  assert(myGraph && "Call appendSceneGraph() first.");

  SbVec3f movement = getSpeed() * float(dt.getValue());

  SbVec3f pos = getPos();
  SbVec3f speed = getSpeed();
  pos += movement;

  if (pos[0] < -tableSize2+radius) {
    pos[0] = -tableSize2+radius;
    speed[0] = -speed[0];
  }
  if (pos[0] > +tableSize2-radius) {
    pos[0] = +tableSize2-radius;
    speed[0] = -speed[0];
  }

  if (pos[1] < -tableSize2+radius) {
    pos[1] = -tableSize2+radius;
    speed[1] = -speed[1];
  }
  if (pos[1] > +tableSize2-radius) {
    pos[1] = +tableSize2-radius;
    speed[1] = -speed[1];
  }

  return setPosAndSpeed(pos, speed);
}


void Ball::createSceneGraph()
{
  myGraph = new SoGroup;
  myGraph->ref();

  SoSeparator *model = new SoSeparator;
  ((SoGroup*)myGraph)->addChild(model);

  matrix = new SoMatrixTransform;
  model->addChild(matrix);

  material = new SoMaterial;
  material->diffuseColor.setValue(0.f,0.f,0.5f);
  model->addChild(material);

  SoSphere *sphere = new SoSphere;
  sphere->radius.setValue(1.f);
  model->addChild(sphere);

  pendings = new SoGroup;
  ((SoGroup*)myGraph)->addChild(pendings);

  speedVec = new SoTranslation;
  model->addChild(speedVec);
}


void Ball::destroySceneGraph()
{
  myGraph->unref();
}


void Ball::activate(SoSeparator *root)
{
  assert(active == FALSE && "Activating already active ball.");

  root->addChild(myGraph);
  active = TRUE;
}


void Ball::deactivate(SoSeparator *root)
{
  assert(active == TRUE && "Deactivating already non-active ball.");

  int i = root->findChild(myGraph);
  if (i >= 0) root->removeChild(i);
  else assert(false);

  active = FALSE;
}


int randi(int max)
{
  return (int)((float)rand() * (float)max/(float)RAND_MAX);
}



float randf(float max)
{
  return (float(rand()) / float(RAND_MAX)) * max;
}


SbVec3f randVec(float maxLen)
{
  SbVec3f v;
  do
    v.setValue(randf(maxLen)*2-maxLen, randf(maxLen)*2-maxLen, randf(maxLen)*2-maxLen);
#if 0
  while (v.length() >= maxLen); // optimized (to remove sqrt):
#else
  while (v[0]*v[0] + v[1]*v[1] + v[2]*v[2] >= maxLen*maxLen);
#endif
  return v;
}


static const SbVec3f initialPos[] = { 
  SbVec3f(+1.3f,+1.3f,0.f),
  SbVec3f(-1.3f,+1.3f,0.f),
  SbVec3f(-1.3f,-1.3f,0.f),
  SbVec3f(+1.3f,-1.3f,0.f),
  SbVec3f(-0.0f,-0.0f,0.f),
};


void Ball::randomInit(int ballIndex)
{
  assert(ballIndex < int(sizeof(initialPos)/sizeof(SbVec3f)) && 
         "Ball index points after the initialPos array. Please, enlarge the array.");
  SbMatrix m = matrix->matrix.getValue();
  const SbVec3f &pos = initialPos[ballIndex];
  m[3][0] = pos[0];
  m[3][1] = pos[1];
  m[3][2] = pos[2];
  matrix->matrix.setValue(m);

#if 0
  SbVec3f speed(randf(10.f), randf(10.f), 0.f);
#else
  SbVec3f speed;
  switch (ballIndex) {
    case 0: speed.setValue( 5, 0, 0); break;
    case 1: speed.setValue(-5, 0, 0); break;
    case 2: speed.setValue( 0, 5, 0); break;
    case 3: speed.setValue( 0,-5, 0); break;
  }
  speed.normalize();
  speed *= 10.f;
#endif
  speedVec->translation = speed;
}



//#define USE_CURVES
#ifdef USE_CURVES
#define RADIUS 2.
#define RADK1 (RADIUS*sin(30/180*MP_I))
#define RADK2 (RADIUS*sin(30/180*M_PI))
const char circleModel[] =
"#Inventor V2.1 ascii\n"
""
"Annotation {"
"  DrawStyle { lineWidth 4 }"
"  Complexity { value 0.8 }"
"  "
"  Coordinate3 {"
"    point [ "
"       0      2 0,"
"      -1.732 -1 0,"
"      +1.732 -1 0,"
"    ]"
"  }"
"  "
"  IndexedNurbsCurve {"
"    numControlPoints 9"
"    coordIndex [ 0, 1, 2, 0, 1, 2, 0, 1, 2, 0,  1,  2,  0,  1, ]"
"    knotVector [ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, ]"
"  }"
"}";
#endif


static SoSeparator* createCircle(const SbVec3f &position, SoGroup *root)
{
  if (!circle) {
#ifdef USE_CURVES
    SoInput in;
    in.setBuffer((void*)&circleModel, strlen(circleModel));
    circle = SoDB::readAll(&in);

    if (!circle) {
      SoReadError::post(&in, "Can not read circle model.");
      exit(-1);
    }
#else
    circle = new SoAnnotation;
    SoDrawStyle *ds = new SoDrawStyle;
    ds->lineWidth.setValue(4.f);
    circle->addChild(ds);
    SoCoordinate3 *c = new SoCoordinate3;
    float angle;
    int i=0;
    const float r=0.7f;
    for (angle=0; angle<6.30; angle+=float(2.f*M_PI/15.f))
      c->point.set1Value(i++, SbVec3f(-r*float(cos(angle)), r*float(sin(angle)), 0.f));
    circle->addChild(c);
    SoLineSet *lset = new SoLineSet;
    lset->numVertices.set1Value(0, c->point.getNum());
    circle->addChild(lset);
#endif
    circle->ref();
  }

  SoSeparator *myCircle = new SoSeparator;

  SoTranslation *translation = new SoTranslation;
  translation->translation.setValue(position);
  myCircle->addChild(translation);

  SoBaseColor *bc = new SoBaseColor;
  bc->rgb.setValue(0.3f,0.3f,0.3f);
  myCircle->addChild(bc);

  SoLightModel *lm = new SoLightModel;
  lm->model.setValue(SoLightModel::BASE_COLOR);
  myCircle->addChild(lm);

  myCircle->addChild(circle);
  root->addChild(myCircle);

  return myCircle;
}


static void deleteCircle(SoSeparator *myCircle, SoGroup *root)
{
  root->removeChild(myCircle);
}


void Ball::updateCircles()
{
  SbList<SoSeparator*> oldList(circleList);
  circleList.truncate(0);

  int i,c = matrix->matrix.getCveNumValues(FALSE);
  for (i=0; i<c; i++) {
    const SbMatrix &m = matrix->matrix.getCveValue(i, FALSE);
    const SbMat &mat = m.getValue();
    SbVec3f pos(mat[3][0], mat[3][1], mat[3][2]);
    int ii,cc = oldList.getLength();
    for (ii=0; ii<cc; ii++) {
      if (((SoTranslation*)oldList[ii]->getChild(0))->translation.getValue() == pos) {
        circleList.append(oldList[ii]);
        oldList.remove(ii);
        goto hit;
      }
    }
    circleList.append(createCircle(pos, pendings));
hit:
    SbBool predicted = matrix->matrix.cveIsPredicted(i);
    if (predicted) // commit-predicted are marked light-gray
      ((SoBaseColor*)circleList[circleList.getLength()-1]->getChild(1))->rgb.setValue(0.6f,0.6f,0.6f);
    else // abort-predicted are marked dark-gray
      ((SoBaseColor*)circleList[circleList.getLength()-1]->getChild(1))->rgb.setValue(0.3f,0.3f,0.3f);
  }

  c = oldList.getLength();
  for (i=0; i<c; i++)
    deleteCircle(oldList[i], pendings);
}


void Ball::cleanup()
{
  if (circle)
    circle->unref();
}
