source: orxonox.OLD/orxonox/branches/trackManager/src/track_manager.cc @ 3516

/* 
   orxonox - the future of 3D-vertical-scrollers

   Copyright (C) 2004 orx

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2, or (at your option)
   any later version.

   ### File Specific:
   main-programmer: Benjamin Grauer
   co-programmer: ...
*/


#include "track_manager.h"

#include "p_node.h"

#include <stdarg.h>
#include "p_node.h"

using namespace std;

/**
   \brief initializes a TrackElement (sets the default values)
*/
TrackElement::TrackElement(void)
{
  this->isFresh = true;
  this->isHotPoint = false;
  this->isSavePoint = false;
  this->isFork = false;
  this->isJoined = false;
  this->mainJoin = false;
  this->cond; //!< todo think!!
  this->ID = -1;
  this->startingTime = 0; //!< \todo eventually set this to the max time of TrackManager.
  this->duration = 1;
  this->endTime = 1;
  this->jumpTime = 0;
  this->curveType = BEZIERCURVE;
  this->nodeCount = 0;
  this->childCount = 0;
  this->name = NULL;
  this->curve = NULL;
  this->children = NULL;
  this->condFunc = &TrackElement::random;
}

/** 
    \brief destroys all alocated memory)
    \todo eventually when deleting a TrackElement you would not like to delete all its preceding TrackElements
*/
TrackElement::~TrackElement(void)
{
  if (this->name)
    delete []name;
  if (this->curve)
    delete this->curve;
  if ((!this->isJoined &&this->childCount > 0) || (this->isJoined && this->mainJoin))
    {
      for (int i=0; i < this->childCount; i++)
        delete this->children[i];
      delete this->children;
    }
}

/**
   \brief Searches through all the TrackElements for trackID.
   \param trackID The ID to search for.
   \returns The TrackElement if Found, NULL otherwise.
   
   \todo make this more modular, to search for different things
*/
TrackElement* TrackElement::findByID(unsigned int trackID)
{
  // return if Found.
  if (this->ID == trackID)
    return this;
  // search on.
  if (this->childCount > 0)
    for (int i=0; i < this->childCount; i++)
      {
        TrackElement* tmpElem;
        if ((tmpElem = this->children[i]->findByID(trackID)))
          return tmpElem;
      }
  else return NULL;
}


/**
   \brief checks if there are any BackLoops in the Track
   \param trackElem the trackElement to check about
   it simply does this by looking if the current trackElem is found again somewhere else in the Track
*/
bool TrackElement::backLoopCheck(TrackElement* trackElem)
{
  if (this->childCount == 0)
    return true;
  else
    {
      for (int i = 0; i < this->childCount; i++)
        if(!this->children[i]->backLoopCheck(trackElem))
          return false;
      
      return true;
    }
}

int TrackElement::lowest(void* nothing)
{
  return 0;
}

int TrackElement::highest(void* nothing)
{ 
  return this->childCount-1;
}

int TrackElement::random(void* nothing)
{
  int i = (int)floor ((float)rand()/(float)RAND_MAX * (float)this->childCount);
  if (i >= this->childCount)
    return this->childCount-1;
  else 
    return i;
}


int TrackElement::leftRight(void* node)
{
  PNode* tmpNode = (PNode*)node;

  if (tmpNode->relCoordinate.z < 0)
    return 0;
  else 
    return 1;
}


// todo
int TrackElement::nearest(void* node)
{
  PNode* tmpNode = (PNode*)node;

  Vector nodeRelCoord = tmpNode->relCoordinate;
  float minDist = 100000000;
  int nodeNumber = 0;
  for (int i = 0; i < this->childCount; i++)
    {
      float dist = (nodeRelCoord - this->children[i]->curve->getNode(4)).len();
      if (dist < minDist)
        {
          minDist = dist;
          nodeNumber = i;
        }
    }
  PRINTF(3)("PathDecision with nearest algorithm: %d\n", nodeNumber);
  return nodeNumber;
}



/////////////////////////////////////
///// TRACKMANAGER //////////////////
/////////////////////////////////////
/**
   \brief standard constructor

*/
TrackManager::TrackManager(void)
{
  this->setClassName ("TrackManager");

  PRINTF(3)("Initializing the TrackManager\n");
  this->firstTrackElem = new TrackElement();
  this->firstTrackElem->ID = 1;
  this->currentTrackElem = firstTrackElem;
  this->localTime = 0;
  this->maxTime = 0;
  this->trackElemCount = 1;
  this->bindSlave = NULL;
}


/**
   \brief standard destructor

*/
TrackManager::~TrackManager(void)
{
  PRINTF(3)("Destruct TrackManager\n");

  PRINTF(3)("Deleting all the TrackElements\n");
  delete this->firstTrackElem;

  // we do not have a TrackManager anymore
  singletonRef = NULL;
}

TrackManager* TrackManager::singletonRef = NULL;

/**
   \returns The reference on the TrackManager.

   If the TrackManager does not exist, it will be created.
*/
TrackManager* TrackManager::getInstance(void) 
{
  if (TrackManager::singletonRef)
    return TrackManager::singletonRef;
  else
    return TrackManager::singletonRef = new TrackManager();
}

/**
   \brief reserves Space for childCount children
   \param childCount The Count of children to make space for.
*/
void TrackManager::initChildren(unsigned int childCount)
{
  this->currentTrackElem->childCount = childCount;
  this->currentTrackElem->mainJoin = true;
  this->currentTrackElem->children = new TrackElement*[childCount];
  for (int i=0; i<childCount; i++)
    {
      this->currentTrackElem->children[i] = new TrackElement();
      this->currentTrackElem->children[i]->ID = ++trackElemCount;
      this->currentTrackElem->children[i]->startingTime = this->currentTrackElem->endTime + this->currentTrackElem->jumpTime;
      this->addPoint(this->currentTrackElem->curve->getNode(this->currentTrackElem->curve->getNodeCount()), this->currentTrackElem->children[i]);
    }
}

/**
   \brief Searches for a given trackID.
   \param trackID the trackID to search for.
   \returns The TrackElement #trackID if found, NULL otherwise.
*/
TrackElement* TrackManager::findTrackElementByID(unsigned int trackID) const
{
  return firstTrackElem->findByID(trackID);
}

// INITIALIZE //

/**
   \brief Sets the trackID we are working on.
   \param trackID the trackID we are working on
*/
void TrackManager::workOn(unsigned int trackID)
{
  TrackElement* tmpElem = findTrackElementByID(trackID);
  if (tmpElem)
    this->currentTrackElem = tmpElem;
  else
    printf("TrackElement not Found, leaving unchanged\n");
  printf("now Working on %d\n", this->currentTrackElem->ID);

}

/**
   \brief Sets the Type of the Curve
   \brief curveType The Type to set
*/
void TrackManager::setCurveType(CurveType curveType, TrackElement* trackElem)
{
  if (!trackElem->isFresh)
    {
      PRINTF(2)("It is not possible to change the type of a Curve after you have have appended some points to it\n");
      return;
    }
  trackElem->curveType = curveType;
  switch (curveType)
    {
    case BEZIERCURVE:
      trackElem->curve = new BezierCurve();
      break;
    case UPOINTCURVE:
      trackElem->curve = new UPointCurve();
      break;
    }
}

/**
   \brief Sets the duration of the current path in seconds.
   \param time The duration in seconds.
*/

void TrackManager::setDuration(float time)
{
  this->currentTrackElem->duration = time;
  this->currentTrackElem->endTime = this->currentTrackElem->startingTime + time;
}

/**
   \brief adds a point to the current TrackElement
   \param newPoint The point to add.
*/
bool TrackManager::addPoint(Vector newPoint)
{
  return this->addPoint(newPoint, this->currentTrackElem);
}

/**
   \brief adds a point to trackElem
   \param newPoint The point to add.
   \param trackElem The TrackElement to add the Point to
*/
bool TrackManager::addPoint(Vector newPoint, TrackElement* trackElem)
{
  if (trackElem->isFresh)
    {
      this->setCurveType(BEZIERCURVE, trackElem);
      trackElem->isFresh = false;
    }
  trackElem->curve->addNode(newPoint);
  trackElem->nodeCount++;
}

/**
   \brief adds save/splitpoint.
   \param newPoint The point to add.
   \returns A Pointer to a newly appended Curve
*/
int TrackManager::addHotPoint(Vector newPoint)
{
  printf("setting up a HotPoint\n");
  if (this->currentTrackElem->isFresh)
    {
      this->setCurveType(BEZIERCURVE);
      this->currentTrackElem->isFresh = false;
    }

  // \todo HotPoint Handling.
  this->currentTrackElem->curve->addNode(newPoint);
  this->currentTrackElem->nodeCount++;
  this->initChildren(1);
  this->currentTrackElem = this->currentTrackElem->children[0];
}

/**
   \brief Sets the last HotPoint into a savePoint.
   \returns A Pointer to a newly appended Curve
   
   If no HotPoint was defined the last added Point will be rendered into a savePoint. \n
   If the HotPoint was defined as a fork the Point will \b not be set into a savePoint.
*/
int TrackManager::setSavePoint(void)
{
  printf("setting up a SavePoint.\n");
  if (this->currentTrackElem->isFork || this->currentTrackElem->isSavePoint)
    return this->currentTrackElem->children[1]->ID;
  this->currentTrackElem->isSavePoint = true;
  this->currentTrackElem->isHotPoint = true;

  this->initChildren(1);
  this->currentTrackElem = this->currentTrackElem->children[0];
}

/**
   \brief adds some interessting non-linear movments through the level.
   \param count The Count of childrens the current HotPoint will have.

   If no HotPoint was defined the last added Point will be rendered into a fork. \n
   If the HotPoint was defined as a savePoint the Point will \b not be set into a fork.
*/
void TrackManager::fork(unsigned int count, ...)
{
  int* trackIDs = new int[count];
  this->forkV(count, trackIDs);
  va_list ID;
  va_start (ID, count);
  for(int i = 0; i < count; i++)
    {
      *va_arg (ID, int*) = trackIDs[i];
    }
  va_end(ID);  
  delete []trackIDs;
}

/**
   \brief adds some interessting non-linear movments through the level.
   \param count The Count of childrens the current HotPoint will have.
   \param trackIDs A Pointer to an Array of ints which will hold the trackID's (the user will have to reserve space for this).

   \see void TrackManager::fork(int count, ...)

   \todo initialisation is wrong!! also in setSavePoint.
*/
void TrackManager::forkV(unsigned int count, int* trackIDs)
{
  printf("Forking with %d children\n", count);
  if (this->currentTrackElem->isSavePoint)
    return;
  this->currentTrackElem->isFork = true;
  this->currentTrackElem->isHotPoint = true;
  for(int i = 0; i < count; i++)
    trackIDs[i]=this->trackElemCount+1+i;
  this->initChildren(count);
}

/**
   \brief decides under what condition a certain Path will be chosen.
   \param cond the CONDITION of the decision
   \param subject the Subject that will be decided upon with CONDITION cond.
*/
void TrackManager::condition(CONDITION cond, void* subject)
{
  this->condition(this->currentTrackElem->ID, cond, subject);
}
/**
   \brief decides under what condition a certain Path will be chosen.
   \param groupID the ID on which to choose the preceding move
   \param cond the CONDITION of the decision
   \param subject the Subject that will be decided upon with CONDITION cond.
*/
void TrackManager::condition(unsigned int groupID, CONDITION cond, void* subject)
{
  TrackElement* tmpElem = this->findTrackElementByID(groupID);
  
  switch (cond)
    {
    case LOWEST:
      tmpElem->condFunc = &TrackElement::lowest;
      break;
    case HIGHEST:
      tmpElem->condFunc = &TrackElement::highest;
      break;
    case RANDOM: 
      tmpElem->condFunc = &TrackElement::random;
      break;
    case LEFTRIGHT:
      tmpElem->condFunc = &TrackElement::leftRight;
      break;
    case NEAREST:
      tmpElem->condFunc = &TrackElement::nearest;
      break;
    case ENEMYKILLED:
      break;
    }
  tmpElem->subject=subject;
}


/**
   \brief joins some tracks together again. 
   \param count The count of Paths to join.

   Join will set the localTime to the longest time a Path has to get to this Point. \n
   Join will join all curves to the first curve, meaning that all the tangents will be matched.
*/
void TrackManager::join(unsigned int count, ...)
{
  int* trackIDs = new int [count];
  va_list ID;
  va_start (ID, count);
  for(int i = 0; i < count; i++)
    {
      trackIDs[i] = va_arg (ID, int);
    }
  va_end(ID);
  this->joinV(count, trackIDs);
  delete []trackIDs;
}

/**
   \brief joins some tracks together again. 
   \param count The count of Paths to join.
   \param trackIDs an Array with the trackID's to join

   \see void TrackManager::join(int count, ...)
*/
void TrackManager::joinV(unsigned int count, int* trackIDs)
{
  printf("Joining %d tracks and merging to Track %d\n", count, trackIDs[0]);

  // checking if there is a back-loop-connection and ERROR if it is.
  TrackElement* tmpTrackElem = this->findTrackElementByID(trackIDs[0]);
  if (!tmpTrackElem->backLoopCheck(tmpTrackElem))
    PRINTF(1)("Backloop connection detected at joining trackElements\n");

  // chanching work-on to temporary value. going back at the end.
  int tmpCurrentWorkingID = this->currentTrackElem->ID;
  this->workOn(trackIDs[0]);
  TrackElement* firstJoint = this->currentTrackElem;
  float tmpLatestTime = firstJoint->endTime;

  Vector tmpEndPoint = firstJoint->curve->getNode(firstJoint->curve->getNodeCount());
  Vector tmpTangentPoint = firstJoint->curve->getNode(firstJoint->curve->getNodeCount()-1);
  Vector tmpc2Point = firstJoint->curve->getNode(firstJoint->curve->getNodeCount()-2);
  firstJoint->isJoined = true;
  //  firstJoint->mainJoin = true;
  if(!firstJoint->isHotPoint)
    this->setSavePoint();
  // Timing:
  for (int i = 0; i < count; i++)
    {
      TrackElement* tmpJoinElem = this->findTrackElementByID(trackIDs[i]);
      if (tmpJoinElem->childCount == 0
          && tmpJoinElem->endTime > tmpLatestTime)
        tmpLatestTime = tmpJoinElem->endTime;
    }
  // time the main Join.
  firstJoint->jumpTime = tmpLatestTime - firstJoint->endTime;
  
  // Joining:
  for (int i = 1; i < count; i++)
    {
      TrackElement* tmpJoinElem = this->findTrackElementByID(trackIDs[i]);
      if (tmpJoinElem->childCount > 0)
        printf("!!This Curve has children, and as such will not be joined!!\n You can try joining other childless TrackElements to this one!");
      else
        {
          this->addPoint(tmpc2Point, tmpJoinElem);
          this->addPoint(tmpTangentPoint, tmpJoinElem);
          this->addPoint(tmpEndPoint, tmpJoinElem);
          // time all other Joins
          tmpJoinElem->jumpTime = tmpLatestTime - tmpJoinElem->endTime;
          
          //Copying Joint-Info
          tmpJoinElem->children = firstJoint->children;
          tmpJoinElem->childCount = firstJoint->childCount;
          tmpJoinElem->isSavePoint = firstJoint->isSavePoint;
          tmpJoinElem->isFork = firstJoint->isFork;

          tmpJoinElem->isJoined = true;
        }
    }
  if(firstJoint->childCount > 0)
    for(int i = 0; i < firstJoint->childCount; i++)
      {
        printf("Setting startingTime of %d to %f.\n", firstJoint->children[i]->ID, tmpLatestTime);
        firstJoint->children[i]->startingTime = tmpLatestTime;
        firstJoint->children[i]->endTime = tmpLatestTime+firstJoint->children[i]->duration;
      } 

  // returning to the TrackElement we were working on.
  this->workOn(tmpCurrentWorkingID);
}

/**
   \brief finalizes the TrackSystem. after this it will not be editable anymore

   \todo check for any inconsistencies, output errors
*/
void TrackManager::finalize(void)
{
  for (int i = 1; i<= trackElemCount ;i++)
    {
      TrackElement* tmpElem = findTrackElementByID(i);
      if (tmpElem->childCount>0 && tmpElem->mainJoin)
        {
          for (int j = 0; j < tmpElem->childCount; j++)
            {
              
              // c1-continuity
              tmpElem->children[j]->curve->addNode(tmpElem->children[j]->curve->getNode(0) +
                                                   ((tmpElem->children[j]->curve->getNode(0) - 
                                                    tmpElem->curve->getNode(tmpElem->curve->getNodeCount()-1))
                                                    ),2);
              tmpElem->children[j]->nodeCount++;
              // c2-continuity
              tmpElem->children[j]->curve->addNode((tmpElem->curve->getNode(tmpElem->curve->getNodeCount())-
                                                    tmpElem->curve->getNode(tmpElem->curve->getNodeCount()-1)) * 4 +
                                                   tmpElem->curve->getNode(tmpElem->curve->getNodeCount()-2), 3);
              tmpElem->children[j]->nodeCount++;                                                    
              printf("accelerations: %d-in: count: %d, %f, %f, %f\n                  %d-out: count: %d %f, %f, %f\n",
                     tmpElem->ID, tmpElem->nodeCount,
                     tmpElem->curve->calcAcc(0.999).x, tmpElem->curve->calcAcc(0.999).y, tmpElem->curve->calcAcc(0.999).z,
                     tmpElem->children[j]->ID, tmpElem->children[j]->nodeCount,
                     tmpElem->children[j]->curve->calcAcc(0).x, tmpElem->children[j]->curve->calcAcc(0).y, tmpElem->children[j]->curve->calcAcc(0).z);
            }
        }
    }
}


// RUNTIME //

/**
   \brief calculates the Position for the localTime of the Track.
   \returns the calculated Position
*/
Vector TrackManager::calcPos() const
{
  //  PRINTF(0)("TrackElement:%d, localTime: %f\n",this->currentTrackElem->ID, this->localTime);
  return this->currentTrackElem->curve->calcPos((this->localTime-this->currentTrackElem->startingTime)/this->currentTrackElem->duration);
}

/**
   \brief calculates the Rotation for the localTime of the Track.
   \returns the calculated Rotation
*/
Vector TrackManager::calcDir() const
{
  return this->currentTrackElem->curve->calcDir((this->localTime - this->currentTrackElem->startingTime)/this->currentTrackElem->duration);
}

/**
   \brief Advances the local-time of the Track around dt
   \param dt The time about which to advance.

   This function also checks, if the TrackElement has to be changed.
*/
void TrackManager::tick(float dt)
{
  dt /= 1000;
  printf("CurrentTrackID: %d, LocalTime is: %f, timestep is: %f\n", this->currentTrackElem->ID, this->localTime, dt);
  if (this->localTime <= this->firstTrackElem->duration)
    this->jumpTo(this->localTime);
  this->localTime += dt;
  if (this->localTime > this->currentTrackElem->endTime
      && this->currentTrackElem->children)
    {
      if (this->currentTrackElem->jumpTime > 0)
        this->jumpTo(this->localTime + this->currentTrackElem->jumpTime);
      this->currentTrackElem = this->currentTrackElem->children[this->choosePath(this->currentTrackElem)];
    }
  if (this->bindSlave)
    {
      Vector tmp = this->calcPos();
      Quaternion quat = Quaternion(this->calcDir(), Vector(this->currentTrackElem->curve->calcAcc((localTime-this->currentTrackElem->startingTime)/this->currentTrackElem->duration).x,1,this->currentTrackElem->curve->calcAcc((localTime-this->currentTrackElem->startingTime)/this->currentTrackElem->duration).z)); 
      this->bindSlave->setAbsCoor(&tmp);
      this->bindSlave->setAbsDir(&quat);
    }
}

/**
   \brief Jumps to a certain point on the Track.
   \param time The time on the Track to jump to.

   This should be used to Jump backwards on a Track, because moving forward means to change between the Path. (it then tries to choose the default.)
   Max is trackLengthMax.
*/
void TrackManager::jumpTo(float time)
{
  if (time == 0)
    this->currentTrackElem = this->firstTrackElem;
  this->localTime = time;
}

/**
   \brief a Function that decides which Path we should follow.
   \param trackElem The Path to choose.
   
*/
int TrackManager::choosePath(TrackElement* trackElem)
{
  return (trackElem->*(trackElem->condFunc))(trackElem->subject);
}

/**
   \brief Sets the PNode, that should be moved along the Tack
   \param bindSlave the PNode to set
*/
void TrackManager::setBindSlave(PNode* bindSlave)
{
  if (!this->bindSlave)
    this->bindSlave = bindSlave;
}


// DEBUG //

/**
   \brief Imports a model of the Graph into the OpenGL-environment.
   \param dt The Iterator used in seconds for Painting the Graph.

   This is for testing facility only. Do this if you want to see the Path inside the Level.
   eventually this will all be packed into a gl-list.
*/
void TrackManager::drawGraph(float dt) const
{

  for (int i = 1; i <= trackElemCount; i++)
    {
      glBegin(GL_LINE_STRIP);
      TrackElement* tmpElem = this->findTrackElementByID(i);
      if (tmpElem->curve)
        for(float f = 0.0; f < 1.0; f+=dt)
          {
            //      printf("%f, %f, %f\n",trackManager->calcPos().x, trackManager->calcPos().y, trackManager->calcPos().z);
            Vector tmpVector = tmpElem->curve->calcPos(f);
            glVertex3f(tmpVector.x, tmpVector.y, tmpVector.z);
          }
  glEnd();
    }
}

/**
   \brief outputs debug information about the trackManager
   \param level how much debug
*/
void TrackManager::debug(unsigned int level) const
{
  PRINT(0)("=========================================\n");
  PRINT(0)("= CLASS TRACKMANAGER::debug information =\n");
  PRINT(0)("=========================================\n");
  //  PRINT(0)("Status is: %
  PRINT(0)(" Consists of %d elements\n", this->trackElemCount);
  PRINT(0)(" localTime is: %f\n", this->localTime);
  if (level >= 2)
    {
      for (int i = 1; i <= trackElemCount; i++)
        {
          TrackElement* tmpElem = this->findTrackElementByID(i);
          PRINT(0)("--== TrackElement:%i ==--", tmpElem->ID);
          if(tmpElem->name)
            PRINT(0)("Name: %s::", tmpElem->name);
          if(tmpElem->isFresh)
            PRINT(0)("  -- has not jet eddited in any way --\n");
          PRINT(0)("\n   TimeTable: startingTime=%f; endTime=%f; duration=%f; jumpTime=%f\n", tmpElem->startingTime, tmpElem->endTime, tmpElem->duration, tmpElem->jumpTime);
          PRINT(0)("   consists of %d Points\n", tmpElem->nodeCount);
          if (tmpElem->childCount == 0)
            PRINT(0)("   has no child\n");
          else if (tmpElem->childCount == 1)
            PRINT(0)("   has 1 child: =%d=\n", tmpElem->children[0]->ID);
          else if (tmpElem->childCount > 1)
            {
              PRINT(0)("   has %d children: ", tmpElem->childCount);
              for(int i = 0; i < tmpElem->childCount; i++)
                PRINT(0)("=%d= ", tmpElem->children[i]->ID);
              PRINT(0)("\n");
            }

          if(tmpElem->isHotPoint)
            PRINT(0)("   is a special Point:\n");
          if(tmpElem->isSavePoint)
            PRINT(0)("    is a SavePoint\n");
          if(tmpElem->isFork)
            {
              PRINT(0)("    is A Fork with with %d children.\n", tmpElem->childCount);
            }
          if(tmpElem->isJoined)
            PRINT(0)("   is Joined at the End\n");

          if(!tmpElem->backLoopCheck(tmpElem)) /* this should not happen */
            PRINT(2)(" THERE IS A BACKLOOP TO THIS ELEMENT\n");
        }
    }
  PRINT(0)("-----------------------------------------\n");
}