#include "polymake/client.h"
#include "polymake/Vector.h"
#include "polymake/Matrix.h"
#include "polymake/linalg.h"
#include "polymake/polytope/to_interface.h"

namespace polymake { namespace polytope {

template<typename Scalar>
void is_vertex_sub(const Vector<Scalar>& q, const Matrix<Scalar>& points, bool& answer, Vector<Scalar>& sep_hyp)
{
   /*construction of LP according to cdd redundancy check for points, see
     http://www.ifor.math.ethz.ch/~fukuda/polyfaq/node22.html#polytope:Vredundancy
    */
   Matrix<Scalar> extension(zero_vector<Scalar>(points.rows())/ones_vector<Scalar>(points.rows()));
   Matrix<Scalar> ineqs(T(extension)|-points.minor(All,range(1,points.cols()-1)));//z^tp_i-z_0<=0; CAUTION: p_i is affine part of i-th point! 
   Vector<Scalar> prev_unbound(ones_vector<Scalar>(2)|-q.slice(1)); //z^tq-z_0<=1, prevents unboundedness
   ineqs/=prev_unbound;
   Matrix<Scalar> aff_hull=null_space(points/q);
   Matrix<Scalar> extension2(aff_hull.rows(),2);
   Matrix<Scalar> aff_hull_minor(aff_hull.rows(),aff_hull.cols()-1);
   if ( aff_hull.cols() > 1 ) {
	aff_hull_minor = aff_hull.minor(All, range(1,aff_hull.cols()-1));
   }
   Matrix<Scalar> eqs(extension2|-aff_hull_minor);
   Vector<Scalar> obj(zero_vector<Scalar>(1)|-ones_vector<Scalar>(1)|q.slice(1)); //z^tq-z_0

   to_interface::solver<Scalar> S;
   /// @retval first: objective value, second: solution
   typedef std::pair<Scalar, Vector<Scalar> > lp_solution;
   lp_solution sol=S.solve_lp(ineqs, eqs, obj, true);
   Scalar obj_val=sol.first;
   Vector<Scalar> res(sol.second);
   //H: z^tx=z_0, i.e., z_0-z^tx=0
   sep_hyp=-res.slice(1); //sep_hyp*point should be >= 0
   sep_hyp[0]=res[1]; 
   if(sol.first > 0){ //q non-red. <=> obj_val > 0
      answer=true;
   } else {
      answer=false;
   }
}

template<typename Scalar>
bool is_vertex(const Vector<Scalar>& q, const Matrix<Scalar>& points)
{
   bool answer;
   Vector<Scalar> sep_hyp(points.cols());
   is_vertex_sub(q, points, answer, sep_hyp);
   return answer;
}

template<typename Scalar>
perl::ListReturn separating_hyperplane(const Vector<Scalar>& q, const Matrix<Scalar>& points)
{
   bool answer;
   Vector<Scalar> sep_hyp(points.cols());
   is_vertex_sub(q, points, answer, sep_hyp);

   perl::ListReturn result;
   result << answer
          << sep_hyp;
   return result;
}

template<typename Scalar>
Vector<Scalar> separating_hyperplane_poly(perl::Object p1, perl::Object p2)
{
   Matrix<Scalar> V = p1.give("VERTICES | POINTS");
   Matrix<Scalar> W = p2.give("VERTICES | POINTS");
   Matrix<Scalar> L1 = p1.give("LINEALITY_SPACE | INPUT_LINEALITY");
   Matrix<Scalar> L2 = p2.give("LINEALITY_SPACE | INPUT_LINEALITY");
   
   if ( L1.rows() + L2.rows() > 0)
      throw std::runtime_error("separating_hyperplane: could not handle linealities");
      
   //origin needs to be on the positive side of the separating plane in order for the LP to work.
   //translate the whole thing s.t. v_0 is the origin
   Vector<Scalar> t = V[0];
   t[0]=0;

   const Matrix<Scalar> ineqs( ( ((V-repeat_row(t,V.rows())) / -(W-repeat_row(t,W.rows()))) | //V is on the positive, W on the negative side of the hyperplane
               ( -ones_vector<Scalar>(V.rows()) | zero_vector<Scalar>(W.rows()) ) ) //vz - z_d >= 0 and wz <= 0
           /  unit_vector<Scalar>(V.cols()+1, V.cols()) //z_d >=0
           / (unit_vector<Scalar>(V.cols()+1, 0) - unit_vector<Scalar>(V.cols()+1, V.cols()))); //z_d <= v_0[0] to ensure boundedness
   Matrix<Scalar> eqs(0,V.cols()+1);
   Vector<Scalar> obj = unit_vector<Scalar>(V.cols()+1, V.cols());

   to_interface::solver<Scalar> S;
   Vector<Scalar> P;
   Vector<Scalar> sep_hyp;
   /// @retval first: objective value, second: solution
   typedef std::pair<Scalar, Vector<Scalar> > lp_solution;

   try {
      lp_solution sol=S.solve_lp(ineqs, eqs, obj, true);
      Scalar obj_val=sol.first;
      Vector<Scalar> res(sol.second);
      sep_hyp = res.slice(0,V.cols());
   }
   catch (infeasible) {
      throw std::runtime_error("separating_hyperplane: the given polytopes cannot be separated");
   }
   /* not relevant
   catch (unbounded) {
      throw std::runtime_error("separating_hyperplane: the given polytopes cannot be separated");
      }*/
   //translate sol back to the original polytope position
   for(int i=1;i<sep_hyp.dim(); ++i){
       if(sep_hyp[i] != 0){
           t[i] -= sep_hyp[0]/sep_hyp[i];
           break;
       }
   }
   t[0]=1;
   sep_hyp[0] = -(sep_hyp * t - sep_hyp[0]*t[0]);

   return sep_hyp;
}
      

UserFunctionTemplate4perl("# @category Geometry"
                          "# Checks whether there exists a hyperplane seperating the  given point //q//"
                          "# from the //points// via solving a suitable LP (compare cdd redundancy check)."
                          "# If so, //q// would be a new vertex of the polytope P generated by //points// and //q//"
                          "# that is not a vertex in the convex hull of //points// alone. To get the seperating"
                          "# hyperplane, use __seperating_hyperplane__"
                          "# Works without knowing the facets of P!"
                          "# @param Vector q the vertex (candidate) which is to be separated from //points//"
                          "# @param Matrix points the points from which //q// is to be separated"
                          "# @return Bool 'true' if //q// is a vertex"
                          "# @example  > $q = cube(2)->VERTICES->row(0);"
                          "# > $points = cube(2)->VERTICES->minor(sequence(1,3),All);"
                          "# > print is_vertex($q,$points);"
                          "# | 1",
                          "is_vertex<Scalar>(Vector<type_upgrade<Scalar>>, Matrix<type_upgrade<Scalar>>)");

UserFunctionTemplate4perl("# @category Optimization"
                          "# Computes (the normal vector of) a hyperplane which separates a given point //q//"
                          "# from //points// via solving a suitable LP. The scalar product of the normal vector"
                          "# of the separating hyperplane and a point in //points// is greater or equal than 0"
                          "# (same behavior as for facets!)."
                          "# If //q// is not a vertex of P=conv(//points//,//q//),"
                          "# the function returns a zero vector and sets //answer// to 'false'."
                          "# Works without knowing the facets of P!"
                          "# @param Vector q the vertex (candidate) which is to be separated from //points//"
                          "# @param Matrix points the points from which //q// is to be separated"
                          "# @return List (Bool answer, Vector sep_hyp)"
                          "# @example The following stores the result in the List @r and then prints the answer and"
                          "# a description of the hyperplane separating the zeroth vertex of the square from the others."                         "# > $q = cube(2)->VERTICES->row(0);"
                          "# > $points = cube(2)->VERTICES->minor(sequence(1,3),All);"
                          "# > @r = separating_hyperplane($q,$points);"
                          "# > print $r[0];"
                          "# | 1"
                          "# > print $r[1];"
                          "# | 0 1/2 1/2",
                          "separating_hyperplane<Scalar>(Vector<type_upgrade<Scalar>>, Matrix<type_upgrade<Scalar>>)");

FunctionTemplate4perl("separating_hyperplane_poly<Scalar>(Polytope<type_upgrade<Scalar>>, Polytope<type_upgrade<Scalar>>)");

InsertEmbeddedRule("# @category Optimization"
                          "# Computes (the normal vector of) a hyperplane which separates two given polytopes"
                          "# //p1// and //p2// if possible."
                          "# @param Polytope p1 the first polytope"
                          "# @param Polytope p2 the second polytope"
                          "# @return Vector a hyperplane separating //p1// from //p2//\n"
                          "user_function separating_hyperplane<Scalar>(Polytope<type_upgrade<Scalar>>, Polytope<type_upgrade<Scalar>>) {\n"
                          "separating_hyperplane_poly(@_); }\n");
} }




// Local Variables:
// mode:C++
// c-basic-offset:3
// indent-tabs-mode:nil
// End:
