#include "hpp/rbprm/utils/algorithms.h"

#include <hpp/pinocchio/collision-object.hh>

#include <hpp/util/debug.hh>

#include <pinocchio/fwd.hpp>

#include <pinocchio/multibody/geometry.hpp>

using namespace hpp;

namespace geom {

/// Computes the (unit) normal vector of a triangle based on the

/// global position of its vertices. The normal is subject to convention!

/// \param tri The global position of a triangles vertices

  // hppDout(notice,"normal, in geom :: "<<normal.transpose());

}

    const hpp::pinocchio::CollisionObjectConstPtr_t object,

    hpp::pinocchio::DeviceData& deviceData) {

  const BVHModelOBConst_Ptr_t model =

      static_pointer_cast<const hpp::fcl::BVHModel<hpp::fcl::OBBRSS> >(

  // todo avoid recopy, but if we keep the same ptr the geometry is changed

  }

}

}

  }

}

  }

}

  }

}

/// see https://en.wikipedia.org/wiki/Centroid#Centroid_of_polygon

  }

  // compute cz in the plan :

  // TODO

}

  }

}

  }

}

                                               // list (or the first)

      }

    }

}

  }

  // loop through all edges of the polygon

  }

}

}

  // If d is zero, there is no intersection

  // not supposed to happen

  // if (d == 0) throw;

  // Get the x and y

  // Check if the x and y coordinates are within both lines

  // not supposed to happen

  // if (x < min(x1, x2) || x > max(x1, x2) ||

  // x < min(x3, x4) || x > max(x3, x4)) return NULL;

  // if (y < min(y1, y2) || y > max(y1, y2) ||

  // y < min(y3, y4) || y > max(y3, y4)) return NULL;

  // Return the point of intersection

}

  // If d is zero, there is no intersection

  // not supposed to happen

  // if (d == 0) throw;

  // Get the x and y

  // Check if the x and y coordinates are within both lines

  // not supposed to happen

  // if (x < min(x1, x2) || x > max(x1, x2) ||

  // x < min(x3, x4) || x > max(x3, x4)) return NULL;

  // if (y < min(y1, y2) || y > max(y1, y2) ||

  // y < min(y3, y4) || y > max(y3, y4)) return NULL;

  // Return the point of intersection

  // find the correct z coordinate :

  double t;

  else {

    hppDout(notice, "in linesect there is no unique z value");

  }

}

                              CIT_Point clipBegin, CIT_Point clipEndHull) {

        else

      }

    }

  }

}

  double dirE, dirS;

      {

        {

        }

      {

      }

    }

  }

}

  double dirE, dirS;

      {

        {

        }

      {

      }

    }

  }

    hppDout(warning, "In computeIntersection3D : intersection is empty");

  }

  /*

  std::ostringstream ss;

  ss<<"[";

  for(size_t i = 0; i < outputList.size() ; ++i){

    ss<<"["<<outputList[i][0]<<","<<outputList[i][1]<<","<<outputList[i][2]<<"]";

    if(i< (outputList.size() -1))

      ss<<",";

  }

  ss<<"]";

  hppDout(notice,"intersection3D = "<<ss.str());

  */

}

}

}

  // hppDout(notice,"project point on plane, signed distance = "<<d);

  // hppDout(notice,"projected point from "<<point.transpose()<<" to

  // "<<proj.transpose());

}

                              CPointRef P0, Eigen::Ref<Point> res) {

  // hppDout(notice,"project point "<<point.transpose()<<" inside plan, with

  // normal "<<Pn.transpose()<<" and point in plan : "<<P0.transpose());

  // hppDout(notice,"number of points defining the plan : "<<plan.size());

    // hppDout(notice,"orthogonal projection is already inside the plan.");

  } else {

    // hppDout(notice,"orthogonal projection is not inside the plan, compute the

    // closest point inside the plan :");

    double d;

    // look for the intersection between the lines (proj_ortho -> center of the

    // intersection) and each edge of 'plan' return the intersection point which

    // lead to the smallest distance (original point -> intersection) plan is a

    // convexHull, the first point and the last points are equal so we don't

    // need specific case to handle the last point

      }

    }

    // hppDout(notice,"new proj with min distance : "<<d_min<<"   :

    // "<<res.transpose());

  }

}

                                  double t, fcl::Vec3f* distance,

                                  unsigned int* num_penetrating_points) {

    }

  }

}

                    const fcl::Vec3f& c, const fcl::Vec3f& p) {

}

                      BVHModelOBConst_Ptr_t model2,

                      fcl::CollisionResult result) {

  }  // for each contact point

  // hppDout(notice,"clipped point : ");

  // std::cout<<ss7.str()<<std::endl;

}

                           std::ostringstream* ss) {

                                     &num_penetrating_points);

  /*

  hppDout(notice,"Intersection between triangles : ");

  hppDout(notice,"[["<<tri[0][0]<<","<<tri[0][1]<<","<<tri[0][2]<<"],["<<tri[1][0]<<","<<tri[1][1]<<","<<tri[1][2]<<"],["<<tri[2][0]<<","<<tri[2][1]<<","<<tri[2][2]<<"],["<<tri[0][0]<<","<<tri[0][1]<<","<<tri[0][2]<<"]]");

  hppDout(notice,"[["<<tri2[0][0]<<","<<tri2[0][1]<<","<<tri2[0][2]<<"],["<<tri2[1][0]<<","<<tri2[1][1]<<","<<tri2[1][2]<<"],["<<tri2[2][0]<<","<<tri2[2][1]<<","<<tri2[2][2]<<"],["<<tri2[0][0]<<","<<tri2[0][1]<<","<<tri2[0][2]<<"]]");

  */

    hppDout(error,

            "triangle in the wrong side of the plane");  // shouldn't happen

  }

    distance =

                    // computation (one point of the triangle inside the plan)

  // distance have one and only one negative distance, we want to separate them

  double dpos[2];

    } else {

    }

  }

  // TODO case : intersection with vertice : only 1 intersection point

  // compute the first intersection point

  // compute the second intersection point

    // hppDout(notice,"first intersection :

    // "<<"["<<i1[0]<<","<<i1[1]<<","<<i1[2]<<"]");

  }

    // hppDout(notice,"second intersection :

    // "<<"["<<i2[0]<<","<<i2[1]<<","<<i2[2]<<"]");

  }

}

/*

T_Point intersectPolygonePlane(BVHModelOBConst_Ptr_t polygone,

BVHModelOBConst_Ptr_t model2, fcl::Vec3f n , double t, fcl::CollisionResult

result, bool useT, double epsilon){ T_Point res,triRes,sortedRes;

std::ostringstream ss; ss<<"[";

  for(size_t c = 0 ; c < result.numContacts() ; ++c){

  //  hppDout(info,"normal = "<<result.getContact(c).normal);

    if(result.getContact(c).normal.equal(-n,epsilon)){ // only compute

intersection for contact with the plane

      // need the -n because .normal are oriented from o1 to o2

      int i = result.getContact(c).b1;  // triangle index

      int j = result.getContact(c).b2;

      fcl::Vec3f tri[3] =

{polygone->vertices[polygone->tri_indices[i][0]],polygone->vertices[polygone->tri_indices[i][1]],polygone->vertices[polygone->tri_indices[i][2]]};

      fcl::Vec3f tri2[3] =

{model2->vertices[model2->tri_indices[j][0]],model2->vertices[model2->tri_indices[j][1]],model2->vertices[model2->tri_indices[j][2]]};

      fcl::Vec3f n2=0;

      fcl::FCL_REAL t2=0;

      fcl::Intersect::buildTrianglePlane(tri2[0],tri2[1],tri2[2], &n2, &t2);

    //  hppDout(info,"n = "<<n2);

     // hppDout(info,"t = "<<t2);

      if(n2.equal(n,epsilon) && ((!useT) ||((t2 + EPSILON >= t ) && (t2-EPSILON

<= t )))){ triRes = intersectTriangles(tri,tri2);

        res.insert(res.end(),triRes.begin(),triRes.end());

        triRes = intersectTriangles(tri2,tri);

        res.insert(res.end(),triRes.begin(),triRes.end());

      }

    }

  } // for each contact point

  if(res.empty()){

    hppDout(notice,"~ Intersection between polygon and plane is empty");

    std::cout<<"~ Intersection between polygon and plane is empty"<<std::endl;

    return res;

  }

  sortedRes = convexHull(res.begin(),res.end());

  hppDout(notice,"clipped point : ");

  for(size_t i = 0; i < sortedRes.size() ; ++i){

    ss<<"["<<sortedRes[i][0]<<","<<sortedRes[i][1]<<","<<sortedRes[i][2]<<"]";

    if(i< (sortedRes.size() -1))

      ss<<",";

  }

  ss<<"]";

  std::cout<<"intersection : "<<std::endl;

  std::cout<<ss.str()<<std::endl;

  hppDout(notice,"area = "<<area(sortedRes.begin(),sortedRes.end()));

  return sortedRes;

}

*/

// cf http://geomalgorithms.com/a05-_intersect-1.html

                              Point p0) {

    } else

  }

  // there ONE intersection point :

}

                               BVHModelOBConst_Ptr_t plane,

                               Eigen::Ref<Point> Pn) {

  // compute plane equation (normal, point inside the plan)

       i++) {  // FIXME : can test 2 times the same line (in both triangles),

               // avoid this ?

    // hppDout(info,"triangle : "<<i);

      // hppDout(info,"couple : "<<j);

          polygone

    }

  }

  // ordonate the point in the vector (clockwise) first point and last point are

  // the same

  /* hppDout(notice,"clipped point : ");

   std::ostringstream ss;

   ss<<"[";

   for(size_t i = 0; i < sortedRes.size() ; ++i){

     ss<<"["<<sortedRes[i][0]<<","<<sortedRes[i][1]<<","<<sortedRes[i][2]<<"]";

     if(i< (sortedRes.size() -1))

       ss<<",";

   }

   ss<<"]";

   hppDout(notice,"intersection = "<<ss.str());

*/

  // std::cout<<"intersection : "<<std::endl;

  // std::cout<<ss.str()<<std::endl;

}

  }

}

                         Eigen::Ref<Point> P0) {

  triPlane.p1 =

      plane

                                                 // triangle, is it an issue ?

}  // namespace geom