/***************************************************************************
   File:        PBMSimScene.cpp                                                            
   Created:     02/04/2002                                                            
   Author:      Maxim Garber                                                
                Computer Science Department                                 
                University of North Carolina - Chapel Hill                  
                garber@cs.unc.edu                                           
                                                                            
   Description: Simulation scene file.                                                            
                                                                            
----------------------------------------------------------------------------.
   Copyright 2001 Maxim Garber                                              
   UNC Motion Planning Research Group                                       
                                                                            
                                                                            
*****************************************************************************/
#include "PBMSimScene.h"
#include "vec3fv.hpp"
#include <algorithm>


/****************************************************************************
   Function   : PBMSimScene                                          
   Description: Constructor                                                            
****************************************************************************/
PBMSimScene::PBMSimScene()
{
  _enableCollision = true;
  _enableStats = false;
  ResetStats();
}

/****************************************************************************
   Function   : PBMSimScene                                          
   Description: Constructor, for a bounded scene.
            
              Scene is centered at the origin, with x and z in [-width/2, width/2]
              and y in [-height/2, height/2] 
   
     Initializes wall objects at the scene boundary.
****************************************************************************/
PBMSimScene::PBMSimScene(float width, float height)
{
  _enableCollision = true;
  _enableStats = false;
  ResetStats();

  _sceneWidth  = width;
  _sceneHeight = height;

   ///////////////////////////////////////////////////////
   // initialize the scene walls for collision purposes
   float normal[3], up[3], right[3], position[3];

   // floor
   Set3fv(normal, 0, 1, 0);
   Set3fv(up, 0, 0, -1);
   Set3fv(right, 1, 0, 0);
   Set3fv(position, 0, -height/2, 0);
   PBMWall* wall = new PBMWall(position, normal, right, up, width, height);
   AddWall(wall);
   
   // ceiling
   Set3fv(normal, 0, -1, 0);
   Set3fv(up, 0, 0, 1);
   Set3fv(right, 1, 0, 0);
   Set3fv(position, 0, height/2, 0);
   wall = new PBMWall(position, normal, right, up, width, height);
   AddWall(wall);

   // back
   Set3fv(normal, 1, 0, 0);
   Set3fv(up, 0, 1, 0);
   Set3fv(right, 0, 0, -1);
   Set3fv(position, -width/2, 0, 0);
   wall = new PBMWall(position, normal, right, up, width, height);
   AddWall(wall);
  

   // front
   Set3fv(normal, -1, 0, 0);
   Set3fv(up, 0, 1, 0);
   Set3fv(right, 0, 0, 1);
   Set3fv(position, width/2, 0, 0);
   wall = new PBMWall(position, normal, right, up, width, height);
   AddWall(wall);

   // left
   Set3fv(normal, 0, 0, 1);
   Set3fv(up, 0, 1, 0);
   Set3fv(right, 1, 0, 0);
   Set3fv(position, 0, 0, -width/2);
   wall = new PBMWall(position, normal, right, up, width, height);
   AddWall(wall);

   // right
   Set3fv(normal, 0, 0, -1);
   Set3fv(up, 0, 1, 0);
   Set3fv(right, -1, 0, 0);
   Set3fv(position, 0, 0, width/2);
   wall = new PBMWall(position, normal, right, up, width, height);
   AddWall(wall);

}


/****************************************************************************
   Function   : ~PBMSimScene                                         
   Description: Destructor                                                            
****************************************************************************/
PBMSimScene::~PBMSimScene()
{
  
  std::list<PBMProjectile*>::iterator p = _projectiles.begin();
  for(; p!= _projectiles.end(); p++)
      delete(*p);
  _projectiles.clear();


  std::list<PBMWall*>::iterator w = _walls.begin();
  for(; w!= _walls.end(); w++)
      delete(*w);
  _walls.clear();
}


bool _ProjectileActive(PBMProjectile* projectile)
{
  float position[3]; projectile->GetPosition(position);
  return fabs(position[0]) < 10 &&
         fabs(position[1]) < 10 &&
         fabs(position[2]) < 10;
}

/****************************************************************************
   Function   : Update                                            
   Description: Updates the objects in the scene and cull inactive obejcts                                                          
****************************************************************************/
void PBMSimScene::Update(float timeStep)
{

  // update all projectiles
  std::list<PBMProjectile*>::iterator p = _projectiles.begin();
  for(; p!= _projectiles.end(); p++)
      (*p)->Update(timeStep);

  // test collision
  if(_enableCollision)
  {
    if(_enableStats) _stats.wallCollisionTimer.Start();
    _ProcessWallCollisions();
    if(_enableStats) _stats.wallCollisionTimer.Stop();

    if(_enableStats) _stats.objectCollisionTimer.Start();
    _ProcessObjectCollisions();
    if(_enableStats)  _stats.objectCollisionTimer.Stop();
  }

  // cull inactive objects
  std::list<PBMProjectile*>::iterator newEnd = std::stable_partition(_projectiles.begin(), _projectiles.end(), _ProjectileActive);
  for(p = newEnd; p != _projectiles.end(); p++)
  {
    PBMProjectile *obj = *p;
    delete obj;
  }
  if (newEnd != _projectiles.end())
    _projectiles.erase(newEnd, _projectiles.end());
}

/****************************************************************************
   Function   : Draw                                              
   Description: Draws all of the objects in the scene                                                            
****************************************************************************/
void PBMSimScene::Draw()
{
  // draw all objects
  std::list<PBMProjectile*>::iterator i = _projectiles.begin();
  for(; i!= _projectiles.end(); i++) (*i)->Draw();

  // draw all walls
  std::list<PBMWall*>::iterator w = _walls.begin();
  for(; w!= _walls.end(); w++)       (*w)->Draw();
}


/****************************************************************************
   Function   : ResetStats                                   
   Description: Reset the stats structure                                                            
****************************************************************************/
void 
PBMSimScene::ResetStats()
{
  _stats.numObjectCollisions = 
  _stats.numObjectCollisionTests = 
  _stats.numWallCollisions = 
  _stats.numWallCollisionTests = 0;

  _stats.objectCollisionTimer.Reset();
  _stats.wallCollisionTimer.Reset();
}


/****************************************************************************
   Function   : _ProcessWallCollisions                       
   Description: Process all wall collisions
   
     Tests each object against each wall.
****************************************************************************/
void 
PBMSimScene::_ProcessWallCollisions()
{
  // test for wall collisions
  PQP_Model *mp, *mw;
  Frame3    pFrame, wFrame; 
  
  std::list<PBMProjectile*>::iterator p = _projectiles.begin();
  for(; p!= _projectiles.end(); p++)
  {
    PBMProjectile* proj = (*p);
    std::list<PBMWall*>::iterator w;
    for(w = _walls.begin(); w!= _walls.end(); w++)
    {
      if(_enableStats) _stats.numWallCollisionTests++;
      
      PBMWall* wall = (*w);
      wall->GetCollisionGeometry(mw, wFrame);
      proj->GetCollisionGeometry(mp, pFrame);
      
      PQP_CollideResult *result = new PQP_CollideResult;
      PQP_Collide(result, pFrame.r, pFrame.p, mp, wFrame.r, wFrame.p, mw, PQP_FIRST_CONTACT);
      
      if(result->Colliding())
      {
        if(_enableStats) _stats.numWallCollisions++;

        // reverse projectile velocity
        float v[3]; proj->GetVelocity(v);
        float n[3]; wall->GetNormal(n);

        float mag = DotProd3fv(n, v);
        
        if(mag >= -0.1) // resting contact
          continue;

        ScalarMult3fv(n, n, -2*mag);
        Add3fv(v, n, v);

        proj->SetVelocity(v);
      }
    }
  }
}



/****************************************************************************
   Function   : _ProcessObjectCollisions                     
   Description: Process All Inter Object Collisions
             
               Uses O(n^2) inter-object testing 
****************************************************************************/
void 
PBMSimScene::_ProcessObjectCollisions()
{
  // test for wall collisions
  PQP_Model *m1, *m2;
  Frame3    Frame1, Frame2;
  
  std::list<PBMProjectile*>::iterator p1 = _projectiles.begin();
  for(; p1!= _projectiles.end(); p1++)
  {
    PBMProjectile* proj1 = (*p1);
    std::list<PBMProjectile*>::iterator p2 = p1;
    p2++;
    for(; p2!= _projectiles.end(); p2++)
    {
      if(_enableStats) _stats.numObjectCollisionTests++;
      
      PBMProjectile* proj2 = (*p2);
      proj1->GetCollisionGeometry(m1, Frame1);
      proj2->GetCollisionGeometry(m2, Frame2);
      
      PQP_CollideResult *result = new PQP_CollideResult;
      PQP_Collide(result, Frame1.r, Frame1.p, m1, Frame2.r, Frame2.p, m2, PQP_FIRST_CONTACT);
      
      if(result->Colliding())
      {
        if(_enableStats) _stats.numObjectCollisions++;

        // collision normal is just the vector between the objects
        float n[3];    proj2->GetPosition(n);
        float temp[3]; proj1->GetPosition(temp);
        Subtract3fv(n, n, temp);
        Normalize3fv(n);

        // compute relative velocity
        float v1[3]; proj1->GetVelocity(v1);
        float v2[3]; proj2->GetVelocity(v2);
        Subtract3fv(temp, v1, v2);

        // compute collision velocity magnitude
        float mag = DotProd3fv(n, temp);      
        if(mag <= 0.1) // resting contact
          continue;

        // set object1's velocity
        ScalarMult3fv(n, n, -mag);
        Add3fv(v1, n, v1);
        proj1->SetVelocity(v1);

        // set object2's velocity
        ScalarMult3fv(n, n, -1);
        Add3fv(v2, n, v2);
        proj2->SetVelocity(v2);
      }
    }
  }
}