/////////////////////////////////////////////////////////////
//                                                         //
// Copyright (c) 2007-2017 by The University of Queensland //
// Centre for Geoscience Computing                         //
// http://earth.uq.edu.au/centre-geoscience-computing      //
//                                                         //
// Primary Business: Brisbane, Queensland, Australia       //
// Licensed under the Open Software License version 3.0    //
// http://www.apache.org/licenses/LICENSE-2.0              //
//                                                         //
/////////////////////////////////////////////////////////////

#include "MeshVolWithJointSet.h"

// --- Project includes ---
#include "geometry/Triangle3D.h"

//-- system includes --
#include <cstdlib>

using std::rand;

// --- STL includes ---
#include <utility>

// --- I/O includes
#include <iostream>

using std::make_pair;

/*!
  Constructor
*/
MeshVolWithJointSet::MeshVolWithJointSet()
{}

/*!
  Constructor

  \param tp the triangle set
*/
MeshVolWithJointSet::MeshVolWithJointSet(const TriPatchSet& tp)
{
  m_mesh=tp;
  m_MinPoint=m_mesh.getBoundingBox().first;
  m_MaxPoint=m_mesh.getBoundingBox().second;
  m_DistPoint=m_MinPoint-Vector3(1.0,1.0,1.0);
}

/*!
  returns a map of the closest mesh facets and their distances to a given point

  \param P the point
  \param nmax the maximum number of objects to be returned
*/ 
const map<double,const AGeometricObject*> MeshVolWithJointSet::getClosestObjects(const Vector3& P,int nmax) const
{
  map<double,const AGeometricObject*> res;

  for(vector<Triangle3D>::const_iterator iter=m_mesh.triangles_begin();
      iter!=m_mesh.triangles_end();
      iter++){
    double ndist=iter->getDist(P);
    res.insert(make_pair(ndist,&(*iter)));
  }

  for(vector<Triangle3D>::const_iterator iter=m_joints.begin();
      iter!=m_joints.end();
      iter++){
    double ndist=iter->getDist(P);
    res.insert(make_pair(ndist,&(*iter)));
  }

  return res;  
}

/*!
  Check if a point is inside the volume.  Works by checking if a 
  line between the point and a point outside crosses
  an odd number of mesh facets.

  \param P the point
*/
bool MeshVolWithJointSet::isIn(const Vector3& P) const
{
  int cross_count=0;
  for(vector<Triangle3D>::const_iterator iter=m_mesh.triangles_begin();
      iter!=m_mesh.triangles_end();
      iter++){
    if(iter->crosses(P,m_DistPoint)) cross_count++;
  }
  return (cross_count%2)==1;
}

/*!
  Check if a sphere is fully inside the volume. Works by testing if center of the 
  sphere is inside and sphere doesn't intersect a mesh facet.

  \param S the sphere
*/
bool MeshVolWithJointSet::isIn(const Sphere& S)
{
  double r=S.Radius();
  Vector3 p=S.Center();

  bool res=isIn(S.Center());
  vector<Triangle3D>::const_iterator iter=m_mesh.triangles_begin();
  while((iter!=m_mesh.triangles_end()) && res){
    res=(iter->getDist(S.Center())>S.Radius());
    iter++;
  }

  if(res){
    // check intersection with joints
    vector<Triangle3D>::iterator iter=m_joints.begin();
    double dist=2*r;
    while((iter!=m_joints.end()) && res){
      dist=iter->getDist(p);
      res=(dist>r);
      iter++;
    }
  }

  return res;
}

void MeshVolWithJointSet::addJoints(const TriPatchSet& t)
{
  for(vector<Triangle3D>::const_iterator iter=t.triangles_begin();
      iter!=t.triangles_end();
      iter++){
    m_joints.push_back(*iter);
  }
}
