/*
   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: Patrick Boenzli
   co-programmer: ...
*/

#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION_REACTION

#include "collision.h"
#include "collision_event.h"

#include "physics_interface.h"

#include "world_entity.h"
#include "cr_physics_full_walk.h"
#include "collision_reaction.h"

#include <vector>

#include "debug.h"

#include "aabb.h"

#include "cr_defs.h"




/**
 *  standard constructor
 */
CRPhysicsFullWalk::CRPhysicsFullWalk ()
    : CollisionReaction()
{
  this->setClassID(CL_CR_PHYSICS_FULL_WALK, "CRPhysicsFullWalk");
}


/**
 *  standard deconstructor
 */
CRPhysicsFullWalk::~CRPhysicsFullWalk ()
{}


/**
 * caluculates and applys the reaction to a specific collision
 *  @param collision the collision
 */
void CRPhysicsFullWalk::reactToCollision(Collision* collision)
{

  AABB* box = collision->getEntityB()->getModelAABB();
  WorldEntity* entity = collision->getEntityB();

  if( box == NULL)
  {
    PRINTF(2)("this model has no aabb box so there is no correct collision reaction implemented. skipping\n");
    return;
  }


  float CR_MAX_WALK_HEIGHT = 15.0f;
  float CR_THRESHOLD = 0.2f;

  float height = 0.0f;
  float front = 0.0f;
  float back = 0.0f;
  float right = 0.0f;
  float left = 0.0f;


  const std::vector<CollisionEvent*>* collisionEvents = &(collision->getCollisionEvents());
  std::vector<CollisionEvent*>::const_iterator it = collisionEvents->begin();
  for(; it != collisionEvents->end(); it++)
  {

    CollisionEvent* ce = (*it);
    Vector normal = ce->getGroundNormal();

    // calculate the collision position
    Vector collPos =  collision->getEntityB()->getAbsCoor()  + box->center - ce->getCollisionPosition();

    // test the 3 axis differently
    switch( ce->getType())
    {
        /* collision in the X-AXIS */
      case COLLISION_TYPE_AXIS_X:
        front = collPos.len() - box->halfLength[0];

        // object is beneath the plane (ground)
        if( front <= 0.0f )
        {
          Vector dirX = entity->getAbsDirX();
          dirX.y = 0.0f;
          dirX.normalize();
          Vector backoff = dirX * front;

          entity->shiftCoor(backoff);
        }
        else if( ce->isInWall())
        {
          // object is already in the wall
          entity->setAbsCoor(entity->getLastAbsCoor());
        }
        break;

      case COLLISION_TYPE_AXIS_X_NEG:
        back = collPos.len() - box->halfLength[0];

        // object is beneath the plane (ground)
        if( back <= 0.0f)
        {
          Vector dirX = entity->getAbsDirX();
          dirX.y = 0.0f;
          dirX.normalize();
          Vector backoff = dirX * back * -1.0f;

          entity->shiftCoor(backoff);
        }
        else if( ce->isInWall())
        {
          // object is already in the wall
          entity->setAbsCoor(entity->getLastAbsCoor());
        }
        break;


        /* collision in the Y-AXIS */
      case COLLISION_TYPE_AXIS_Y_NEG:
        // calulate the height above ground
        height = collPos.len() - box->halfLength[1];


        // object is beneath the plane (ground)
        //         if(height >= 0.0f && height <= 0.0001f) break ;// Do nothing
        if( height < 0.0f && -height < CR_MAX_WALK_HEIGHT)
        {
          entity->shiftCoor(Vector(0.0f, -height + 0.00001, 0.0f));
          entity->setOnGround(true);
        }
        // object is already in the wall
        else if( ce->isInWall())
        {
          entity->setAbsCoor(entity->getLastAbsCoor());
          PRINTF(0)("ground collision: reset pos\n");
        }
        else
        {
          // entity is not on ground
          entity->setOnGround(false);
        }
        break;


        /* collision in the Z-AXIS */
      case COLLISION_TYPE_AXIS_Z:

        right = collPos.len()  - box->halfLength[2];

        // object is beneath the plane (ground)
        if( right <= 0.0f )
        {
          Vector dirZ = entity->getAbsDirZ();
          dirZ.y = 0.0f;
          dirZ.normalize();
          Vector backoff = dirZ * right;
          entity->shiftCoor(backoff);
        }
        else if( ce->isInWall())
        {
          // object is already in the wall
          entity->setAbsCoor(entity->getLastAbsCoor());
        }
        break;


        // collision in the z-axis
      case COLLISION_TYPE_AXIS_Z_NEG:

        left = collPos.len()  - box->halfLength[2];

        // object is beneath the plane (ground)
        if( left <= 0.0f )
        {
          Vector dirZ = entity->getAbsDirZ();
          dirZ.y = 0.0f;
          dirZ.normalize();
          Vector backoff = dirZ * left*-1.0f;
          entity->shiftCoor(backoff);
        }
        // object is already in the wall
        else if( ce->isInWall())
        {
          entity->setAbsCoor(entity->getLastAbsCoor());
        }
        break;
    }
  }
  //PRINTF(0)("collision distances: x: %f, y: %f, z: %f\n", front, height, side);







}




/**
 * use this to do some collision offline calculations, only called for bContinuousPoll == true
 */
void CRPhysicsFullWalk::update(WorldEntity* owner)
{}