// Filename: physicsCollisionHandler.cxx
// Created by:  drose (16Mar02)
//
////////////////////////////////////////////////////////////////////
//
// PANDA 3D SOFTWARE
// Copyright (c) Carnegie Mellon University.  All rights reserved.
//
// All use of this software is subject to the terms of the revised BSD
// license.  You should have received a copy of this license along
// with this source code in a file named "LICENSE."
//
////////////////////////////////////////////////////////////////////

#include "physicsCollisionHandler.h"
#include "collisionNode.h"
#include "collisionEntry.h"
#include "collisionPolygon.h"
#include "config_collide.h"
#include "config_physics.h"
#include "actorNode.h"
#include "dcast.h"

TypeHandle PhysicsCollisionHandler::_type_handle;

////////////////////////////////////////////////////////////////////
//     Function: PhysicsCollisionHandler::Constructor
//       Access: Public
//  Description:
////////////////////////////////////////////////////////////////////
PhysicsCollisionHandler::
PhysicsCollisionHandler() {
  _almost_stationary_speed = 0.1f;
  _static_friction_coef=0.9f;
  _dynamic_friction_coef=0.5f;
  set_horizontal(false);
}

////////////////////////////////////////////////////////////////////
//     Function: PhysicsCollisionHandler::Destructor
//       Access: Public, Virtual
//  Description:
////////////////////////////////////////////////////////////////////
PhysicsCollisionHandler::
~PhysicsCollisionHandler() {
}

////////////////////////////////////////////////////////////////////
//     Function: PhysicsCollisionHandler::apply_friction
//       Access: 
//  Description: The vel parameter will be modified in place to
//               account for friction.
////////////////////////////////////////////////////////////////////
void PhysicsCollisionHandler::
apply_friction(ColliderDef &def, LVector3& vel, const LVector3& force,
    PN_stdfloat angle) {
  if (vel!=LVector3::zero()) {
    PN_stdfloat friction_coefficient=0.0f;
    // Determine the friction:
    if (vel.length()<_almost_stationary_speed) {
      physics_debug("  static friction");
      friction_coefficient=_static_friction_coef;
    } else {
      physics_debug("  dynamic friction");
      friction_coefficient=_dynamic_friction_coef;
    }
    // Apply the friction:
    physics_debug("  vel pre  friction "<<vel<<" len "<<vel.length());
    PN_stdfloat friction=friction_coefficient*angle;
    physics_debug("  friction "<<friction);
    if (friction<0.0f || friction>1.0f) {
      cerr<<"\n\nfriction error "<<friction<<endl;
      friction=1.0f;
    }
    #if 0
    PN_stdfloat dt=ClockObject::get_global_clock()->get_dt();
    vel *= (1.0f-friction) * dt * dt;
    #else
    vel *= 1.0f-friction;
    #endif
    physics_debug("  vel post friction "<<vel<<" len "<<vel.length());
  }
}

////////////////////////////////////////////////////////////////////
//     Function: PhysicsCollisionHandler::apply_net_shove
//       Access: Protected, Virtual
//  Description: 
////////////////////////////////////////////////////////////////////
void PhysicsCollisionHandler::
apply_net_shove(ColliderDef &def, const LVector3& net_shove,
    const LVector3 &force) {
  CollisionHandlerPusher::apply_net_shove(def, net_shove, force);
  if (force == LVector3::zero()) {
    return;
  }
  if (def._target.is_empty()) {
    return;
  }
  ActorNode *actor;
  DCAST_INTO_V(actor, def._target.node());
  LVector3 vel=actor->get_physics_object()->get_velocity();
  if (vel == LVector3::zero()) {
    return;
  }
  physics_debug("apply_linear_force() {");
  physics_debug("  vel            "<<vel<<" len "<<vel.length());
  physics_debug("  net_shove      "<<net_shove<<" len "<<net_shove.length());
  physics_debug("  force          "<<force<<" len "<<force.length());
  LVector3 old_vel=vel;

  // Copy the force vector while translating it 
  // into the physics object coordinate system:
  LVector3 adjustment=force;
  physics_debug(
      "  adjustment set "<<adjustment<<" len "<<adjustment.length());

  //NodePath np(def._node);
  //CPT(TransformState) trans = np.get_net_transform();
  //adjustment=adjustment*trans->get_mat();
  //physics_debug(
  //    "  adjustment trn "<<adjustment<<" len "<<adjustment.length());

  adjustment=adjustment*actor->get_physics_object()->get_lcs();
  physics_debug(
      "  adjustment lcs "<<adjustment<<" len "<<adjustment.length());

  adjustment.normalize();
  physics_debug(
      "  adjustment nrm "<<adjustment<<" len "<<adjustment.length());

  PN_stdfloat adjustmentLength=-(adjustment.dot(vel));
  physics_debug("  adjustmentLength "<<adjustmentLength);
  PN_stdfloat angle=-normalize(old_vel).dot(normalize(force));
  physics_debug("  angle "<<angle);
  // Are we in contact with something:
  if (angle>0.0f) {
    physics_debug("  positive contact");
    #if 0
    cerr<<"vel "<<vel<<endl;
    cerr<<"net_shove "<<net_shove<<endl;
    cerr<<"force "<<force<<endl;
    actor->get_physics_object()->add_impact(force, -vel);
    #else
    adjustment*=adjustmentLength;
    physics_debug(
        "  adjustment mul "<<adjustment<<" len "<<adjustment.length());
    
    // This adjustment to our velocity will not reflect us off the surface,
    // but will deflect us parallel (or tangent) to the surface:
    vel+=adjustment;
    physics_debug("  vel+adj "<<vel<<" len "<<vel.length());
    
    apply_friction(def, vel, force, angle);
    #endif
  } else if (adjustmentLength==0.0f) {
    physics_debug("  brushing contact");
  } else {
    physics_debug("  negative contact");
  }

  #ifndef NDEBUG //[
  if (IS_THRESHOLD_EQUAL(vel.length(), old_vel.length(), 0.0001f)) {
    // This is a check to see if vel is staying the same:
    physics_debug(
        "  vel is about the same length:  "
        <<vel.length()<<" ~ "<<old_vel.length());
  } else if (vel.length() > old_vel.length()) {
    // This is a check to avoid adding engergy:
    physics_debug(
        "  vel got larger  "<<vel.length()<<" > "<<old_vel.length());
  } else {
    // This is a check to avoid losing engergy:
    physics_debug(
        "  vel got smaller  "<<vel.length()<<" < "<<old_vel.length());
  }
  if (vel.length() > 10.0f) {
    // This is a check to see if the velocity is higher than I expect it
    // to go.  The check value is arbitrary.
    physics_debug("  vel.length() > 10.0f  "<<vel.length());
  }
  #endif //]

  physics_debug("  force "<<force<<" len "<<force.length());
  physics_debug("  vel "<<vel<<" len "<<vel.length());
  physics_debug("}");
  actor->set_contact_vector(adjustment);
  actor->get_physics_object()->set_velocity(vel);
}

////////////////////////////////////////////////////////////////////
//     Function: PhysicsCollisionHandler::apply_linear_force
//       Access: Protected, Virtual
//  Description: 
////////////////////////////////////////////////////////////////////
void PhysicsCollisionHandler::
apply_linear_force(ColliderDef &def, const LVector3 &force) {
}

////////////////////////////////////////////////////////////////////
//     Function: PhysicsCollisionHandler::validate_target
//       Access: Protected, Virtual
//  Description: Called internally to validate the target passed to
//               add_collider().  Returns true if acceptable, false
//               otherwise.
////////////////////////////////////////////////////////////////////
bool PhysicsCollisionHandler::
validate_target(const NodePath &target) {
  if (!CollisionHandlerPhysical::validate_target(target)) {
    return false;
  }
  nassertr_always(target.node()->is_of_type(ActorNode::get_class_type()), false);
  return true;
}
