/*****************************
File:      CollisionGroup.cpp
Language:   C++ (header)
Project:    H3DNetworkingUtils
The contents of this file are subject to the Mozilla Public License
Version 1.1 (the "License"); you may not use this file except in
compliance with the License. You may obtain a copy of the License at   
http://www.mozilla.org/MPL/

Software distributed under the License is distributed on an "AS IS"
basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See the
License for the specific language governing rights and limitations
under the License.

The Original Code is H3DNetworkingUtils v1.0.

The Initial Developer of the Original Code is CSIRO.
Portions created by the Initial Developer are Copyright (C) 1009 CSIRO. All Rights Reserved.
Contributor(s):
    Chris Gunn  <Chris.Gunn@csiro.au> <ChrisJGunn@gmail.com>
***************************/

#include "H3DNetworkingUtils/CollisionGroup.h"
#include "H3DNetworkingUtils/CollidableDynamic.h"
#include <strstream>

using namespace std;
using namespace H3D;
using namespace H3DNetworkingUtils;

// ----------------------------------------------------------
CollisionGroup::CollisionGroup( ) :
collidingChildrenA (new MFChild),
collidingChildrenB (new MFChild),
userMsg(new MFString),
  first_collide (true), 
  check_seg_num (-1),
  collision_found (true)
{}

// -------------------------------------------------
H3DNodeDatabase CollisionGroup::database(
  "CollisionGroup", 
  &(newInstance<CollisionGroup>),
  typeid(CollisionGroup)); 

namespace CollisionGroupInternals {   
  FIELDDB_ELEMENT( CollisionGroup, collidingChildrenA,       INPUT_OUTPUT ); 
  FIELDDB_ELEMENT( CollisionGroup, collidingChildrenB,       INPUT_OUTPUT ); 
  FIELDDB_ELEMENT( CollisionGroup, userMsg,       OUTPUT_ONLY ); 
}

// ----------------------------------------------------------
inline void CollisionGroup::traverseSG( TraverseInfo & ti ) {
  X3DChildNode::traverseSG( ti );
  
  collision_found = false;
  
  // iterate through colliding children
  for ( MFChild::const_iterator i = collidingChildrenA->begin(); 
      i != collidingChildrenA->end(); i++ ) {
    // collide with itself
    X3DChildNode *childA = static_cast<X3DChildNode*>(*i);
    ///detectCollision( childA );
    // iterate through colliding children in list B
    for ( MFChild::const_iterator j = collidingChildrenB->begin(); 
      j != collidingChildrenB->end(); j++ ) {
      X3DChildNode *childB = static_cast<X3DChildNode*>(*j);
      if (childB != childA) {
        detectCollision( childA, childB );
      }
      if (collision_found) {
         break;
      }
    }
    if (collision_found) {
      break;
    }
  }
  first_collide = false;
}


// ----------------------------------------------------------
///void CollisionGroup::detectCollision( X3DChildNode *child ) {

/* gives Bus Error for BodyDynamics with SphereHierarchies ...
 BodyDynamic *dyn = dynamic_cast< BodyDynamic *>(child);
  if ( dyn ) {  
    detectCollision( dyn );
  }
*/
///  ElasticChain *chain = dynamic_cast< ElasticChain *>(child);
///  if ( chain ) {
///    detectCollision( chain );
///  }

///}



// ----------------------------------------------------------
void CollisionGroup::detectCollision( X3DChildNode *childA, X3DChildNode *childB ) {

   if ( dynamic_cast< CollidableDynamic *>(childA) ) {
      if ( dynamic_cast< CollidableDynamic *>(childB) ) {
         // dyn - dyn
         detectCollision( static_cast<CollidableDynamic *>(childA), 
                          static_cast<CollidableDynamic *>(childB) );
      }
   }
}


// ----------------------------------------------------------
// performs collisions within a dynamic - does nothing
///void CollisionGroup::detectCollision( CollidableDynamic *dyn ) {
///  cout << "detectCollision( Dynamic )" << endl;
  // nothing
///}



// ----------------------------------------------------------
// performs collisions between two dynamics
void CollisionGroup::detectCollision( CollidableDynamic *dynA,
    CollidableDynamic *dynB ) {
  
///  cerr << "detectCollision CollidableDynamic *dynA, CollidableDynamic *dynB" << endl;
  // iterate through collidable_objects of first dynamic
  // collide each with second dynamic
       for ( CollidableDynamic::MFCollidableGeometry::const_iterator
      i = dynA->collisionSpheres->begin(); 
      i != dynA->collisionSpheres->end(); i++ ) {
    detectCollision( static_cast<CollisionGeometry*>(*i), dynB ); // geo, dyn 
    if (collision_found) {
       break;
    }
  }  
  
}

// ----------------------------------------------------------
// performs collisions between a geometry and a dynamic
void CollisionGroup::detectCollision( CollisionGeometry *geo,
    CollidableDynamic *dyn ) {
  
///  cerr << "detectCollision CollisionGeometry *geo, CollidableDynamic *dyn" << endl;
  // iterate through collidable_objects of dyn
    // collide each with geo
  for ( CollidableDynamic::MFCollidableGeometry::const_iterator
      i = dyn->collisionSpheres->begin(); 
      i != dyn->collisionSpheres->end(); i++ ) {
    detectCollision( geo,  static_cast<CollisionGeometry*>(*i)); // geo, geo 
    if (collision_found) {
       break;
    }
  } 
  
}

// ----------------------------------------------------------
void CollisionGroup::detectCollision( CollidableDynamic *dyn, 
    CollisionGeometry *geo ) {
  detectCollision( geo, dyn );
}


// ----------------------------------------------------------
void CollisionGroup::detectCollision( CollisionGeometry *geoA,
    CollisionGeometry *geoB ) {
  
///    cerr << "detectCollision( CollisionGeometry *geoA, CollisionGeometry *geoB" << endl;
      if ( dynamic_cast<CollisionSphereHierarchy *>(geoA) ) {
    // hierarchy-?
    
    if ( dynamic_cast<CollisionSphereHierarchy *>(geoB) ) { 
      // hierarchy-hierarchy
      detectCollision( static_cast<CollisionSphereHierarchy *>(geoA), 
                       static_cast<CollisionSphereHierarchy *>(geoB) );
    } else if ( dynamic_cast< CollisionSphere *>(geoB) ) {
      // hierarchy-sphere
      detectCollision( static_cast<CollisionSphereHierarchy *>(geoA),
                       static_cast< CollisionSphere *>(geoB) );
    } 
  } else if (dynamic_cast< CollisionSphere *>(geoA)) {
    // sphere-?
    
      if ( dynamic_cast<CollisionSphereHierarchy *>(geoB) ) {
        // sphere-hierarchy
        detectCollision( static_cast< CollisionSphere *>(geoA),
                         static_cast<CollisionSphereHierarchy *>(geoB) );
      } else if (dynamic_cast< CollisionSphere *>(geoB)) {
        // sphere-sphere
        detectCollision( static_cast< CollisionSphere *>(geoA), 
                           static_cast< CollisionSphere *>(geoB) );
      } 
  } 

} 
  
  
// ----------------------------------------------------------
// Sphere - Sphere
void CollisionGroup::detectCollision( CollisionSphere *sphereA,
                                       CollisionSphere *sphereB ) {
   // check for intersection
   Vec3f penetration_dir;
   Vec3f global_contact_pt;
   H3DFloat penetration_mag;
   if ( checkIntersection( sphereA, sphereB, penetration_dir, penetration_mag, global_contact_pt ) ) { // sphere, sphere
      if (penetration_mag > 0.000001) { // must be significant ie > 1 mm
         Vec3f penetration_vec = penetration_dir * penetration_mag;
         CollidableDynamic * coll_A = sphereA->getDyn();
         CollidableDynamic * coll_B = sphereB->getDyn();
         Vec3f posA = coll_A->position->getValue();
         Vec3f posB = coll_B->position->getValue();

         // tell geometry to shift
         H3DFloat ave_stiffness = (coll_A->collisionStiffness->getValue() +
        		      coll_B->collisionStiffness->getValue()) * 0.5f;
         H3DFloat mA = coll_A->mass->getValue();
         H3DFloat mB = coll_B->mass->getValue();

         Vec3f vA = coll_A->velocity->getValue();
         Vec3f vB = coll_B->velocity->getValue();

         // Only provide a collision separating force if the two dynamics are moving towards each other.
         // If they are moving away, we may have already had a collision, and provided those forces in an 
         // earlier cycle and they haven't quite escaped intersection yet. We don't want ot provide forces 
         // a second time.
         H3DFloat speed_A_along_normal = vA.dotProduct(penetration_dir);
         H3DFloat speed_B_along_normal = vB.dotProduct(penetration_dir);
         if (speed_A_along_normal > speed_B_along_normal) {
            if (first_collide) {
               doFirstCollideMessage(coll_A, coll_B, penetration_vec);
            } else {
               Vec3f force_A, force_B;
               bool penetration_component = true; 
               if (penetration_component) {
                  //penetration_method component
                  H3DFloat k = 1 / (mA + mB) * ave_stiffness;   
                  force_A = penetration_vec * (-mB * k);
                  force_B = penetration_vec * (mA * k);
               }
               bool collision_component = true;
               if (collision_component) {
                  // Conservation of momentum method (Physics for Game Developers, David Bourg, P96)
                  // Calc velocities along the line of action (penetration)
                  H3DFloat s_A = speed_A_along_normal;
                  H3DFloat s_B = speed_B_along_normal;
                  H3DFloat old_mom_A = s_A * mA;
                  H3DFloat old_mom_B = s_B * mB;

                  H3DFloat s_r = s_B - s_A; // reletive speed along penetration dir
                  H3DFloat e = ave_stiffness;
                  H3DFloat J = s_r * (e + 1) / (1.0f/mA + 1.0f/mB) ;
                  H3DFloat required_delta_momA = J;
                  H3DFloat required_delta_momB = -J;

                  H3DFloat new_mom_A = old_mom_A + required_delta_momA;
                  H3DFloat new_mom_B = old_mom_B + required_delta_momB;

                  // We need forces to achieve these delta_mom's in one graphics cycle.
                  Scene * scene = *Scene::scenes.begin();
                  H3DFloat frame_rate = scene->frameRate->getValue();
                  float cycle_time = 1 / frame_rate;
                  // From F = ma   and a = delta_v/time:
                  force_A += penetration_dir * (required_delta_momA) / cycle_time;
                  force_B += penetration_dir * (required_delta_momB) / cycle_time;

                  H3DFloat tot_new_mom = fabs(new_mom_A) + fabs(new_mom_B);
                  H3DFloat tot_current_mom = fabs(old_mom_A) + fabs(old_mom_B);
                  if (tot_new_mom > tot_current_mom) {
                     // Sorry, I can't figure out why this gets to here. We shouldn't be adding energy to the system.
                     // As a workaround, the only thing i can do is to scale the values back.
                     H3DFloat scale = tot_current_mom / tot_new_mom * e;
                     force_A *= scale;
                     force_B *= scale;
                  }
               }

               coll_A->addImpulse(force_A);
   	         coll_B->addImpulse(force_B);

               BoxBound * bndAP = dynamic_cast<BoxBound*>(coll_A->bound->getValue());
               BoxBound * bndBP = dynamic_cast<BoxBound*>(coll_B->bound->getValue());
               Vec3f torqueA = (global_contact_pt - bndAP->center->getValue()) % force_A;
               Vec3f torqueB = (global_contact_pt - bndBP->center->getValue()) % force_B;

               coll_A->addImpulseTorque(torqueA * 10);
   	         coll_B->addImpulseTorque(torqueB * 10);
            }
         }
         collision_found = true; // When one sphere collision has occured, that is enough.
      }
   }
}
  

// checks for intersection between two spheres
// if true, collision points from the centre of sphereA to that of
// sphereB, with magnitude representing the overlap, in global coords
// ----------------------------------------------------------
bool CollisionGroup::checkIntersection( CollisionSphere *sphereA,
                                        CollisionSphere *sphereB, 
                                        Vec3f  &penetration_dir, H3DFloat & penetration_mag, 
                                        Vec3f & global_contact_point ) {
      
  // Try this
  bool ret = false;
  Vec3f AG = sphereA->getGlobalPosition();
  Vec3f BG = sphereB->getGlobalPosition();
///  cerr << "checkIntersection sph sph " << AG << ' ' << BG << endl;
  H3DFloat radA = sphereA->getGlobalRadius();
  H3DFloat radB = sphereB->getGlobalRadius();
  H3DFloat collision_dist = radA + radB;
  Vec3f AG_BG = BG - AG;
  H3DFloat ctr_dist = AG_BG.length();
  if (ctr_dist == 0.0) {
    // A fudge to get rid of silly values if co-located.
    AG_BG.x += 0.0001f;
    ctr_dist = -0.0001f;
  }
  if (ctr_dist < collision_dist) {
    ret = true;
    penetration_dir = AG_BG;
    penetration_dir.normalizeSafe();
    penetration_mag = collision_dist - ctr_dist;
    global_contact_point = (AG+BG)/2.0f;
  }
  return ret;
}


// ----------------------------------------------------------
// Sphere - SphereHierarchy
void CollisionGroup::detectCollision( CollisionSphere *sphere,
    CollisionSphereHierarchy *hier ) {
  
///  cerr << "DetectCollision sph heir" << endl;
  // check intersection with sphere and hierarchy,
  // check next level of hierarchy 
  Vec3f penetration;
  H3DFloat penetration_mag;

///  cerr << "heir pos = " << hier->getGlobalPosition() << endl;
///  cerr << "sphere pos = " << sphere->getGlobalPosition() << endl;
///  Matrix4f m = hier->getGlobalToLocal();
///  for (int i = 0; i < 4; i++) {
///    for (int j = 0; j < 4; j++) {
///      cerr << m[i][j];
///    }
///    cerr << endl;
///  }
///  cerr << endl << endl;
  Vec3f contact_pt;
  if ( checkIntersection( sphere, hier, penetration, penetration_mag, contact_pt ) ) {
     for ( CollisionSphereHierarchy::MFCollidableGeometry::const_iterator 
        i = hier->collidables->begin(); 
        i != hier->collidables->end(); i++ ) {
      detectCollision( sphere, static_cast<CollisionGeometry*>(*i) ); // sphere, geo 
      if (collision_found) {
         break;
      }
    } 
  }
  
}
 
// ----------------------------------------------------------
// SphereHierarchy - Sphere
void CollisionGroup::detectCollision( CollisionSphereHierarchy *hier,
    CollisionSphere *sphere ) {
  detectCollision( sphere, hier );  
}
   

// ----------------------------------------------------------
void CollisionGroup::detectCollision( CollisionSphereHierarchy *hierA,
    CollisionSphereHierarchy *hierB ) {

  // if hierarchies intersect, check next level of hierarchy
  Vec3f penetration_dir;
  Vec3f contact_pt;
  H3DFloat penetration_mag;
  if ( checkIntersection( hierA, hierB, penetration_dir, penetration_mag, contact_pt ) ) {
///    cout << "Hierarchy-Hierarchy INTERSECTION" << endl;
///    cout << "  HierA: " << hierA->collidable_dynamic->collidable_name->getValue()
///        << " with radius: " << hierA->radius->getValue() << endl;  
///    cout << "  HierB: " << hierB->collidable_dynamic->collidable_name->getValue() 
///        << " with radius: " << hierB->radius->getValue() << endl;  
    for ( CollisionSphereHierarchy::MFCollidableGeometry::const_iterator 
        i = hierA->collidables->begin(); 
        i != hierA->collidables->end(); i++ ) {
      CollisionGeometry * geo = dynamic_cast<CollisionGeometry*>(*i);
      if (geo) {
         detectCollision( geo, hierB ); // sphere, sphere/hierarchy 
      }
      if (collision_found) {
         break;
      }
    } 
  }

}

// ----------------------------------------------------------
void CollisionGroup::doFirstCollideMessage(CollidableDynamic * coll_A, 
                                           CollidableDynamic * coll_B, const Vec3f & penetration_vec) {
    H3DFloat mA = coll_A->mass->getValue();
    H3DFloat mB = coll_B->mass->getValue();
    Vec3f penetration_prop_A = penetration_vec * 1.004 * (-mB/(mA + mB));
    Matrix3f scale_rotate_A = coll_A->getGlobalToLocal().getScaleRotationPart();
    Matrix3f scale_rotate_B = coll_B->getGlobalToLocal().getScaleRotationPart();
    Vec3f local_moveA = scale_rotate_A * penetration_prop_A;
    Vec3f penetration_prop_B = penetration_vec * 1.004 * (mA/(mA + mB));
    Vec3f local_moveB = scale_rotate_B * penetration_prop_B;

    vector<string> tmp;
    ostrstream strm0;
    strm0 << "Warning: Object inter-penetration at startup: " << '\0';
    tmp.push_back(strm0.str());
    ostrstream strm1;
    strm1 << "Move "
        ///<< sphereA->getDyn()->collidableName->getValue()
        << coll_A->getName()
        << " by "
        << local_moveA.x << ' ' << local_moveA.y << ' ' << local_moveA.z << " and" << '\0';
    tmp.push_back(strm1.str());
    ostrstream strm2;
    strm2 << "          " << coll_B->getName()
       << " by "
       << local_moveB.x << ' ' << local_moveB.y << ' ' << local_moveB.z << '\0';
    tmp.push_back(strm2.str());
    userMsg->setValue(tmp);
    cout << tmp[0] << endl << tmp[1] << endl << tmp[2] << endl;
}

// ----------------------------------------------------------
int CollisionGroup::lineSphereIntersection( const Vec3f &lineOrigin,
			    const Vec3f &lineGradient,
			    const Vec3f &sphereCentre,
			    H3DFloat sphereRadius,
			    Vec3f &soln1,
			    Vec3f &soln2 )
{
  H3DFloat a;
  H3DFloat b;
  H3DFloat c;
  H3DFloat delta;
  H3DFloat t;
  Vec3f lineSphere = lineOrigin - sphereCentre;

  H3DFloat lineGradient_sqrd = lineGradient * lineGradient;
  if ( lineGradient_sqrd < H3DUtil::Constants::f_epsilon ) {
    // This check prevents potential division by zero errors
    return 0;
  }

  a = lineGradient_sqrd;
  b = 2.0f * (lineGradient * lineSphere);
  c = lineSphere * lineSphere - sphereRadius * sphereRadius;
  delta = b*b - (4.0f*a*c);

  if ( delta < 0.0 ) {
    return 0;
  } else if ( delta == 0.0f ) {
    t = -b / (2*a);
    soln1 = lineOrigin + t * lineGradient;
    return 1;
  } else {
    t = (-b - sqrt(delta))/(2*a);
    soln1 = lineOrigin + t * lineGradient;
    t = (-b + sqrt(delta))/(2*a);
    soln2 = lineOrigin + t * lineGradient;
    return 2;
  }
}

