/***************************************************************************
 *   Copyright (C) 2007 by A.J. Tavakoli                                   *
 *                                                                         *
 *   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 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

#include "Physics.h"


// static constants for physics engine
const Vector3 Physics::gravity(0.0f, -1024.0f, 0.0f);
const Vector3 Physics::gravityDir(0.0f, -1.0f, 0.0f);
const float Physics::stepSize = 18.0f;
const int Physics::hitFloor = 0x1;
const int Physics::hitWall  = 0x2;


void Physics::beginFrame(BSP &bsp) {
  // iterate through entities and apply physics rules to each one
  for ( size_t i=0; i < bsp.getNumEntities(); i++ ) {
    Entity *entity = bsp.getEntity(i);

    // set acceleration of entity to zero vector
    entity->setAcc( Vector3(0.0f, 0.0f, 0.0f) );

    // set force on entity to force of gravity alone
    entity->setForce(gravity);

    // for some entities, remove component of velocity that is
    // parallel to xz plane.
    if ( entity->isGrounded() &&
         entity->getMovementType() == Entity::MOVEMENT_TYPE_WALK )
      entity->setVel( Vector3(0.0f, entity->getVel()[1], 0.0f) );
  } // for (entities)
}


void Physics::endFrame(BSP &bsp, float dt) {
  // iterate through entities and apply physics rules to each one
  for ( size_t i=0; i < bsp.getNumEntities(); i++ ) {
    Entity *entity = bsp.getEntity(i);

    // compute acceleration of entity from net force
    // being exerted on it
    entity->setAcc( entity->getForce()*entity->getMass() );

    // update velocity of entity based on its acceleration
    entity->setVel(entity->getVel() + entity->getAcc()*dt);

    // move entity to new position
    switch ( entity->getMovementType() ) {
    case Entity::MOVEMENT_TYPE_NORMAL:
      move(entity, entity->getVel()*dt, bsp);
      break;
    case Entity::MOVEMENT_TYPE_WALK:
      walkMove(entity, entity->getVel()*dt, bsp);
      break;
    }

    // set grounded flag (hack)
    entity->setGrounded ( entity->getForce()*gravityDir < gravity.mag() - EPSILON );
  }
}


// attempts to move entity from its current position to position
// given by its position plus displacement vector.  moves entity
// as far as possible before a collision occurs.
int Physics::move(Entity *entity,
                  const Vector3 &displacement,
                  const BSP &bsp,
                  float *moveDist) {
  int retVal = 0;
  float hitFraction = 1.0f;
  Vector3 hitNormal(0.0f, 0.0f, 0.0f);

  Point3 newPos = entity->getPos() + displacement*hitFraction;
  bool collision =  bsp.checkCollisions(entity, entity->getPos(), newPos,
                                        hitFraction, hitNormal);

  // if there was a collision, move entity as far as possible before collision.
  if ( collision ) {
    hitFraction -= EPSILON;

    if ( hitFraction < 0.0f )
      hitFraction = 0.0f;

    newPos = entity->getPos() + displacement*hitFraction;

    if ( Math3D::rabs(hitNormal[1]) < EPSILON )
      retVal |= hitWall;
    else if ( hitNormal[1] > 0.7f )
      retVal |= hitFloor;

    exertFloorForce(entity, hitNormal);
  }

  entity->setPos(newPos);

  if ( moveDist != NULL )
    (*moveDist) = displacement.mag()*hitFraction;

  return retVal;
}


// called after certain entities collide with static geometry to make them
// 'slide' along the surface.  returns distance moved.
int Physics::slideMove(Entity *entity,
                       const Vector3 &displacement,
                       const BSP &bsp,
                       float *moveDist) {
  int retVal = 0;

  if ( moveDist != NULL )
    (*moveDist) = 0.0f;

  float hitFraction = 0.0f;
  Vector3 hitNormal(0.0f, 0.0f, 0.0f);
  bsp.checkCollisions(entity,
                      entity->getPos(),
                      entity->getPos() + displacement,
                      hitFraction,
                      hitNormal);  

  entity->setPos( entity->getPos() + displacement*(hitFraction - EPSILON) );

  if ( moveDist != NULL )
    (*moveDist) = hitFraction*displacement.mag();

  // was there a collision?
  if ( hitFraction < 1.0f - EPSILON ) {
    Vector3 currDisplacement = displacement;
    for ( int i=0; i < 4 && hitFraction < 1.0f - EPSILON; i++ ) {
      // determine type of hit
      if ( Math3D::rabs(hitNormal[1]) < EPSILON )
        retVal |= hitWall;
      else if ( hitNormal[1] > 0.7f )
        retVal |= hitFloor;

      exertFloorForce(entity, hitNormal);

      currDisplacement = currDisplacement - hitNormal*(hitNormal*currDisplacement);

      hitFraction = 0.0f;
      bsp.checkCollisions(entity,
                          entity->getPos(),
                          entity->getPos() + currDisplacement,
                          hitFraction,
                          hitNormal);

      hitFraction -= EPSILON;

      if ( hitFraction > 0.0f ) {
        entity->setPos(entity->getPos() + currDisplacement*hitFraction);

        if ( moveDist != NULL )
          (*moveDist) += hitFraction*currDisplacement.mag();
      }
    }
  }

  // return value indicating what type (if any) of geometry was hit
  // (wall, step, ground)
  return retVal;
}


void Physics::walkMove(Entity *entity,
                       const Vector3 &displacement,
                       const BSP &bsp) {
   Point3 beforeSlide = entity->getPos();
  Vector3 beforeSlideForce = entity->getForce();
  Vector3 beforeSlideVel   = entity->getVel();

  float slideDist = 0.0f;
  Point3 oldPos = entity->getPos();
  int hitType = slideMove(entity, displacement, bsp, &slideDist);

  // if entity collided with something, attempt to step up
  // (enables entity to walk up stairs)
  if ( slideDist*slideDist < displacement.sqrMag() - EPSILON &&
      (hitWall & hitType) &&
       entity->isGrounded() ) {
    // store entity's original position before trying to move up step
    Point3 beforeStep = entity->getPos();
    Vector3 beforeStepForce = entity->getForce();
    Vector3 beforeStepVel   = entity->getVel();
    bool    beforeStepGnd   = entity->isGrounded();

    entity->setPos(beforeSlide);
    entity->setForce(beforeSlideForce);
    entity->setVel(beforeSlideVel);

    // try to move entity up
    Vector3 upMove = Vector3(0.0f, stepSize, 0.0f);
    move(entity, upMove, bsp);

    // try to move entity forward
    Vector3 forwardMove = Vector3(displacement[0], 0.0f, displacement[2]);
    float stepDist = 0.0f;
    slideMove(entity, forwardMove, bsp, &stepDist);

    // if could not step forward any further than move before step,
    // revert back to state before attempting to step
    if ( stepDist < slideDist + EPSILON ) {
      entity->setPos(beforeStep);
      entity->setForce(beforeStepForce);
      entity->setVel(beforeStepVel);
      entity->setGrounded(beforeStepGnd);
    }
    else {
      // Carmack does the following here:
      // - adds friction based on view angle
      // - move entity down as far as possible
      // - set grounded flag

      // move entity as far down as possible
      move(entity, Vector3(0.0f, -stepSize, 0.0f), bsp);

      // not setting grounded flag since it is already set in exertFloorForce()
    }
  }
}


void Physics::exertFloorForce(Entity *entity,
                              const Vector3 &normal) {
  // subtract component of force that is perpendicular to surface
  if ( entity->getForce().sqrMag() > EPSILON )
    entity->setForce( entity->getForce() - normal*(entity->getForce()*normal) );

  // subtract component of velocity that is perpendicular to surface
  if ( entity->getVel().sqrMag() > EPSILON )
    entity->setVel( entity->getVel() - normal*(entity->getVel()*normal) );

  if ( normal[1] > 0.7f )
    entity->setGrounded(true);
}
