/*
   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_ground_walk.h"
#include "collision_reaction.h"

#include <vector>

#include "debug.h"

#include "aabb.h"

#include "cr_defs.h"

using namespace std;


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


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


/**
 * caluculates and applys the reaction to a specific collision
 *  @param collision the collision
 */
void CRPhysicsGroundWalk::reactToCollision(Collision* collision)
{
  CollisionEvent* ce = collision->getCollisionEvents().front();
  Vector normal = ce->getGroundNormal();
  // normal.normalize();


  float height;
  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;
  }


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

  float CR_MAX_WALK_HEIGHT = 2.0f;
  float CR_THRESHOLD = 0.2f;

  //height = collPos.y - box->halfLength[1];
  //PRINTF(0)("height: %f          , model height: %f\n", height, box->halfLength[1]);
  // PRINTF(0)(" ground normal: %f, %f, %f\n", normal.x, normal.y, normal.z);
  //

  switch( ce->getType())
  {

    case COLLISION_TYPE_AXIS_X:

      break;

    case COLLISION_TYPE_AXIS_Y:

      height = collPos.y - box->halfLength[1];

      PRINTF(0)("height: %f\n", height);
      PRINTF(0)("in wall %i\n", ce->isInWall());

      // object is beneath the plane (ground)
      if( height <= 0.0f )
      {
        entity->shiftCoor(Vector(0, -height, 0));
      }
      // object is already in the wall
      else if( ce->isInWall())
      {
        entity->setAbsCoor(entity->getLastAbsCoor());
      }
      break;
      break;


    case COLLISION_TYPE_AXIS_Z:

      break;

  }








#if 0
  if( box != NULL)
    height = ( ce->getCollisionPosition() - collision->getEntityB()->getAbsCoor() )*(-1.0f) ;
  else
    height = ce->getCollisionPosition() - collision->getEntityB()->getAbsCoor() ;


  if( box != NULL)
  {


    if(ce->getCollisionPosition().x <= 0.9 && ce->getGroundNormal().len() <= 1.4f)
    {
      collision->getEntityB()->setAbsCoor(collision->getEntityB()->getLastAbsCoor());
      return;
    }
    if(ce->getCollisionPosition().z <= 0.9 && ce->getGroundNormal().len() <= 1.4f)
    {
      collision->getEntityB()->setAbsCoor(collision->getEntityB()->getLastAbsCoor());
      return;
    }

    if(ce->getGroundNormal().len() <= 0.1f)
    {
      collision->getEntityB()->setAbsCoor(collision->getEntityB()->getLastAbsCoor());
      return;
    }


    if(ce->getGroundNormal().len() >= 1.4f)
    {
      downspeed++;
      collision->getEntityB()->setAbsCoor(collision->getEntityB()->getAbsCoor() + Vector(0.0,-0.08*downspeed,0.0));
      return;
    }


    if(height.y > box->halfLength[1] + 0.0f ) // Above ground
    {
      if(height.y < box->halfLength[1] + 2.3f) // Snap in
      {
        downspeed = 0;
        collision->getEntityB()->setAbsCoor(collision->getEntityB()->getAbsCoor() - Vector(0.0,height.y  - box->halfLength[1] - 0.0f,0.0));
      } else
      {
        downspeed++;
        collision->getEntityB()->setAbsCoor(collision->getEntityB()->getAbsCoor() + Vector(0.0,-0.08*downspeed,0.0));
      }

    }
    else
    {
      if(height.y <  box->halfLength[1] + 0.0f   /* && height.y  >  - 55.0f*/) // below ground
      {
        //if(downspeed <= 0) downspeed =1;
        collision->getEntityB()->setAbsCoor(collision->getEntityB()->getAbsCoor() + Vector(0.0, -height.y  +  box->halfLength[1] + 2.0f,0.0));
        //collision->getEntityB()->setVelocity(Vector(0.0,0.0,0.0));
        downspeed = 0;
      }

    }

  }// if(box!= NULL)
#endif
  /*
  PRINTF(0)("Collision with Ground: \n");
  collision->getEntityB()->getAbsCoor().debug();
  collision->getEntityB()->setVelocity(Vector());
  collision->getEntityB()->setAbsCoor(this->lastPositions[1]);

  */

}




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