/*****************************
File:      CollidableDynamic.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/CollidableDynamic.h"
#include <H3D/H3DHapticsDevice.h>
#include "H3DNetworkingUtils/CollisionSphereHierarchy.h"
#include <H3D/Coordinate.h>

using namespace H3D;
using namespace H3DNetworkingUtils;

const u_int CollidableDynamic::split = 3;

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

namespace CollidableDynamicInternals {   
  FIELDDB_ELEMENT( CollidableDynamic, collisionSpheres,    INPUT_OUTPUT ); 
  FIELDDB_ELEMENT( CollidableDynamic, collisionStiffness,  INPUT_OUTPUT ); 
  FIELDDB_ELEMENT( CollidableDynamic, collisionResolution,  INPUT_OUTPUT ); 
  FIELDDB_ELEMENT( CollidableDynamic, autoGenerateCollisionSpheres,  INPUT_OUTPUT ); 
  FIELDDB_ELEMENT( CollidableDynamic, debugCollisionSphereLayer,  INPUT_OUTPUT ); 
  FIELDDB_ELEMENT( CollidableDynamic, overlap,  INPUT_OUTPUT ); 
}


CollidableDynamic::CollidableDynamic(
                  Inst< AddChildren        > _addChildren,
                  Inst< RemoveChildren     > _removeChildren,
                  Inst< MFChild            > _children,
                  Inst< SFNode             > _metadata,
                  Inst< SFBound            > _bound,  
                  Inst< SFVec3f            > _bboxCenter,
                  Inst< SFVec3f            > _bboxSize,
                  Inst< SFTransformedBound > _transformedBound,
                  Inst< SFMatrix4f         > _matrix,
                  Inst< SFMatrix4f         > _accumulatedForward,
                  Inst< SFMatrix4f         > _accumulatedInverse,
                  Inst< SFVec3f            > _position,
                  Inst< SFRotation         > _orientation,
                  Inst< SFVelocity         > _velocity,
                  Inst< SFVec3f            > _momentum,
                  Inst< SFVec3f            > _force,
                  Inst< SFAngularVelocity  > _angularVelocity,
                  Inst< SFVec3f            > _angularMomentum,
                  Inst< SFSpin             > _spin,
                  Inst< SFVec3f            > _torque,
                  Inst< SFFloat            > _mass,
                  Inst< SFMatrix3f         > _inertiaTensor,
                  Inst< SFMotion           > _motion,
                  Inst<MFVec3f>         _contactForces,
                  Inst<MFVec3f>         _contactTorques,
                  Inst<SumMFVec3f>      _totalContactForce,
                  Inst<SumMFVec3f>      _totalContactTorque,
                  Inst<SumForces>       _externalForces,
                  Inst<SumTorques>      _externalTorques,
                  Inst<SFVec3f>         _freedom,
                  Inst<SFVec3f>         _angularFreedom,
                  Inst<SFFloat>         _linearDamping,
                  Inst<SFFloat>         _rotationalDamping,
                  Inst<SFFloat>         _linearSpringStiffness,
                  Inst<SFFloat>         _angularSpringStiffness,
                  Inst<SFVec3f>         _globalPosition,

                  Inst< MFCollidableGeometry > _collisionSpheres,
                  Inst< SFFloat >      _collisionStiffness,
                  Inst< SFFloat >      _collisionResolution) :
Dynamic (_addChildren, _removeChildren, _children, _metadata, 
           _bound, _bboxCenter, _bboxSize, _transformedBound, _matrix, 
           _accumulatedForward, _accumulatedInverse, _position, _orientation, _velocity, _momentum,
           _force, _angularVelocity, _angularMomentum, _spin, _torque, _mass, _inertiaTensor, _motion, _contactForces,
           _contactTorques, _totalContactForce, _totalContactTorque, _externalForces, _externalTorques,
           _freedom, _angularFreedom, _linearDamping, _rotationalDamping, _linearSpringStiffness, _angularSpringStiffness,
           _globalPosition),
collisionSpheres (_collisionSpheres),
collisionStiffness (_collisionStiffness),
collisionResolution (_collisionResolution),
debugCollisionSphereLayer               (new SFInt32),
autoGenerateCollisionSpheres (new SFBool),
overlap (new SFFloat) {

  type_name = "CollidableDynamic";
  database.initFields( this );

  collisionSpheres->setOwner(this);
  collisionStiffness->setValue(0.4f);
  collisionResolution->setValue(0.02f);
  debugCollisionSphereLayer->setValue(0);
  autoGenerateCollisionSpheres->setValue(false);
  overlap->setValue(0.15f);
}

// -------------------------------------------------
void CollidableDynamic::initialize() {
   Dynamic::initialize();

   if (autoGenerateCollisionSpheres->getValue()) {
      BoxBound * bndP = dynamic_cast<BoxBound*>(bound->getValue());
      if (bndP) {
         Vec3f sz = bndP->size->getValue();
         Vec3f pos = bndP->center->getValue();
         H3DFloat min = (sz.x < sz.y) && (sz.x != 0.0f)? sz.x : sz.y;
         min = (sz.z == 0.0f) || (min < sz.z) ? min : sz.z;
         if (collisionResolution->getValue() < 0.0f) {
           collisionResolution->setValue(min);
         }
     
         // Auto create the collidable spheres for object collision
         int layer = 1;
         buildSphereHierarchy(identity_trans, this, NULL, layer, 1.0f);
      }
   }
   NodeVector nodes;
   for ( MFCollidableGeometry::const_iterator i = collisionSpheres.get()->begin(); 
         i != collisionSpheres.get()->end(); i++ ) {
      nodes.push_back(*i);
   }
   addChildren->setValue(nodes); // This must be done last otherwise it messes up iterations.

}

// -------------------------------------------------
void CollidableDynamic::buildSphereHierarchy(const Matrix4f & trans, X3DGroupingNode * grpP, 
                                             CollisionSphere * parent_sphP, u_int layer,
                                             H3DFloat accumulated_scale) {
   // If there are several in the group, make a sphere to contain them all.
   CollisionSphere * new_containing_sphP = NULL;
   Matrix4f current_trans; // identitiy
   if (grpP->children->size() > 1) {
      // We need to create a sphere around all children and use this as the parfent for the childre.
      BoxBound * bndP = dynamic_cast<BoxBound*>(grpP->bound->getValue());
      if (bndP) {
         Vec3f sz = bndP->size->getValue();
         Vec3f center = bndP->center->getValue();
         H3DFloat min = sz.x < sz.y ? sz.x : sz.y;
         min = min < sz.z ? min : sz.z;
         H3DFloat sphere_dia = sz.length();
         new_containing_sphP = createSphere(trans, false, layer, sphere_dia, center);
         layer++;
      }
   } else {
      // We don't create a sphere as the only child will create an identical one.
      // We just accumulate the transform matrix
      current_trans = trans;
   }
   CollisionSphere * containing_sphereP = parent_sphP;
   if (new_containing_sphP) {
      containing_sphereP = new_containing_sphP;
   }
   for ( MFChild::const_iterator i = grpP->children->begin(); 
      i != grpP->children->end(); i++ ) {
     X3DChildNode *child = static_cast<X3DChildNode*>(*i);
     X3DShapeNode * shapeP = dynamic_cast<X3DShapeNode*>(child);
     if (shapeP) {
        createSpheres(current_trans, shapeP, containing_sphereP, layer, accumulated_scale);
     } else {
        MatrixTransform * transP = dynamic_cast<MatrixTransform*>(child);
        if (transP) {
           const Matrix4f & matr = transP->matrix->getValue();
           Vec3f extra_scale = matr.getScalePart();
           H3DFloat max_scale = extra_scale.x > extra_scale.y ? extra_scale.x : extra_scale.y;
           max_scale = max_scale > extra_scale.z ? max_scale : extra_scale.z;
           
           buildSphereHierarchy(transP->matrix->getValue() * current_trans, 
                                transP, containing_sphereP, layer, accumulated_scale * max_scale);
        } else {
           X3DGroupingNode * groupP = dynamic_cast<X3DGroupingNode*>(child);
           if (groupP) {
              buildSphereHierarchy(trans, groupP, containing_sphereP, layer, accumulated_scale);
           }
        }
     }
  }
  if (new_containing_sphP) {
     if (parent_sphP) {
        static_cast<CollisionSphereHierarchy*>(parent_sphP)->collidables->push_back(new_containing_sphP);
     } else {
         collisionSpheres->push_back(new_containing_sphP);
     }
  }
}

// -------------------------------------------------
void CollidableDynamic::createSpheres(const Matrix4f & trans, 
                                      X3DShapeNode * shapeP, 
                                      CollisionSphere * parent_sphP, 
                                      u_int layer, H3DFloat scale) {
   // Create a number of spheres to cover a particular geometry.
   X3DGeometryNode * geomP = shapeP->geometry->getValue();
   if (geomP) {
      HAPI::Vec3 c, s;
      geomP->getBound(c, s);
      Vec3f center((float)(c.x), (float)(c.y), (float)(c.z));
      Vec3f size((float)(s.x), (float)(s.y), (float)(s.z));
      if ((size.x >= size.y) && (size.y >= size.z)) {
         max_dim = 0;
         middle_dim = 1;
         min_dim = 2;
      } else if ((size.x >= size.z) && (size.z >= size.y)){
         max_dim = 0;
         middle_dim = 2;
         min_dim = 1;
      } else if ((size.y >= size.x) && (size.x >= size.z)){
         max_dim = 1;
         middle_dim = 0;
         min_dim = 2;
      } else if ((size.y >= size.z) && (size.z >= size.x)){
         max_dim = 1;
         middle_dim = 2;
         min_dim = 0;
      } else if ((size.z >= size.x) && (size.x >= size.y)){
         max_dim = 2;
         middle_dim = 0;
         min_dim = 1;
      } else if ((size.z >= size.y) && (size.y >= size.x)){
         max_dim = 2;
         middle_dim = 1;
         min_dim = 0;
      }

      createSpheresOverGeometry(trans, center, size, 
                                 parent_sphP, layer, 
                                 geomP, scale);
   }
}

// -------------------------------------------------
void CollidableDynamic::createSpheresOverGeometry(const Matrix4f & trans, 
                                                  const Vec3f & offset_in_geom, 
                                                  const Vec3f & size, 
                                                  CollisionSphere * parent_sphP,
                                                  u_int layer, X3DGeometryNode * geomP,
                                                  H3DFloat scale) {
   // Create a number of spheres to cover a particular geometry.
   // We get here if the geometry is not a X3DComposedGeometryNode. We just create our collision 
   // spheres from the bounding box.
   H3DFloat max_size = size[max_dim];
   H3DFloat sphere_dia = size.length();
   H3DFloat requested_res = collisionResolution->getValue() / scale;
   bool leaf_node(false);
   if (sphere_dia <= requested_res) {
      leaf_node = true;
   } else if (sphere_dia > requested_res) {
      H3DFloat next_size = sphere_dia / split;
      H3DFloat curr_diff = sphere_dia - requested_res;
      H3DFloat next_diff = requested_res - next_size;
      if ((next_diff > 0.0f) && (curr_diff < next_diff)) {
         leaf_node = true;
      }
   } 

   CollisionSphere * sphP = createSphere(trans, leaf_node, layer, sphere_dia, offset_in_geom);
   ///sphP->setPointCheckData(scale, offset_in_geom);

   X3DComposedGeometryNode * coord_setP = dynamic_cast<X3DComposedGeometryNode*>(geomP);
   if (coord_setP) {
      if (!containsPoints(sphP, coord_setP)) {
         // Throw it away - there is nothing inside
         if (debugCollisionSphereLayer->getValue() > 0) {
            cout << "                                  --->>> discard - no coords" << endl;
         }
         delete sphP;
         sphP = NULL;
      }
   }
   
   if (sphP) {
      if (!leaf_node) {
         Vec3f next_size;
         u_int num_along_length = split;
         next_size[max_dim] = size[max_dim] / num_along_length;
         u_int num_across_width = (u_int)(ceilf(size[middle_dim] / next_size[max_dim]));
         num_across_width = num_across_width == 0 ? 1 : num_across_width;
         next_size[middle_dim] = size[middle_dim] / num_across_width;
         u_int num_down_depth = (u_int)(ceilf(size[min_dim] / next_size[max_dim]));
         num_down_depth = num_down_depth == 0 ? 1 : num_down_depth;
         next_size[min_dim] = size[min_dim] / num_down_depth;

         Vec3f local_offset;
         H3DFloat length_step = size[max_dim] / num_along_length;
         H3DFloat width_step = size[middle_dim] / num_across_width;
         H3DFloat depth_step = size[min_dim] / num_down_depth;
         local_offset[max_dim] = -size[max_dim] / 2.0f + length_step / 2.0f;
         for (u_int i = 0; i < num_along_length; i++, local_offset[max_dim] += length_step) {
            local_offset[middle_dim] = -size[middle_dim] / 2.0f + width_step / 2.0f;
            for (u_int j = 0; j < num_across_width; j++, local_offset[middle_dim] += width_step) {
               local_offset[min_dim] = -size[min_dim] / 2.0f + depth_step / 2.0f;
               for (u_int k = 0; k < num_down_depth; k++, local_offset[min_dim] += depth_step) {
                  createSpheresOverGeometry(identity_trans, local_offset + offset_in_geom, 
                                             next_size, sphP, layer+1, geomP, scale);
               }
            }
         }
      }
      if (parent_sphP) {
         static_cast<CollisionSphereHierarchy*>(parent_sphP)->collidables->push_back(sphP);
      } else {
         collisionSpheres->push_back(sphP);
      }
   }
}

// -------------------------------------------------
CollisionSphere * CollidableDynamic::createSphere(const Matrix4f & trans, bool leaf_node, u_int layer, 
                                                   H3DFloat dia, const Vec3f & rel_pos) {
   CollisionSphere * sphP = NULL;
   if (leaf_node) {
      // Finished recursion
      sphP = new CollisionSphere;
   } else {
      sphP = new CollisionSphereHierarchy;
   }
   sphP->setLayer(layer);
   sphP->matrix->setValue(trans);
   sphP->setLocalOffset(rel_pos);
   sphP->radius->setValue(dia * 0.5f * (1 + overlap->getValue())); // Radius = 0.5*dia, then oversize by overlap (default 15%).
   if (debugCollisionSphereLayer->getValue() > 0) {
      cout << layer << ": ";
      if (layer == 1) {
         cout << "   ";
      } else if (layer == 2) {
         cout << "      ";
      } else if (layer == 3) {
         cout << "         ";
      } else if (layer == 4) {
         cout << "            ";
      } else if (layer == 5) {
         cout << "               ";
      } else if (layer == 6) {
         cout << "                  ";
      } 
      cout << "c = " << rel_pos << ", r = " << sphP->radius->getValue() << endl;
   }

   if (layer == debugCollisionSphereLayer->getValue()) {
      sphP->debug->setValue(true);
   }
   return sphP;
}

// -------------------------------------------------
bool CollidableDynamic::containsPoints(CollisionSphere * sphP, X3DComposedGeometryNode * coord_set_geomP) {
   Coordinate * coordP = dynamic_cast<Coordinate *>(coord_set_geomP->coord->getValue());
   if (coordP) {
      for ( MFVec3f::const_iterator i = coordP->point->begin(); 
         i != coordP->point->end(); i++ ) {
         Vec3f pt = *i;
         if (sphP->contains(pt)) {
            return true;
         }
      }
   }
   return false;
}

// -------------------------------------------
void CollidableDynamic::MFCollidableGeometry::onAdd(Node * n) {
   CollidableDynamic * dynP = static_cast<CollidableDynamic*>(getOwner());
   static_cast<CollisionGeometry*>(n)->setDyn(dynP);
   TypedMFNode< CollisionGeometry >::onAdd(n);
}

// -------------------------------------------
void CollidableDynamic::MFCollidableGeometry::onRemove(Node * n) {
   static_cast<CollidableDynamic*>(getOwner())->children->erase(n);
   TypedMFNode< CollisionGeometry >::onRemove(n);
}


