/* Copyright (c) 1997-2016
   Ewgenij Gawrilow, Michael Joswig (Technische Universitaet Berlin, Germany)
http://www.polymake.org

This program is free software; you can redistribute it and/or modify it
under the terms of the GNU General Public License as published by the
Free Software Foundation; either version 2, or (at your option) any
later version: http://www.gnu.org/licenses/gpl.txt.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.
--------------------------------------------------------------------------------
*/

#include "polymake/client.h"
#include "polymake/polytope/separating_hyperplane.h"

namespace polymake { namespace polytope {

    //translate all entries of the matrix that do not have 0 as first coordinate (i.e. non-rays) by t
  template<typename Scalar>
        Matrix<Scalar> translate_non_rays(Matrix<Scalar> M, Vector<Scalar> t){
            Matrix<Scalar> tM(M.rows(),M.cols());
            typedef typename Entire< Rows< Matrix<Scalar> > >::iterator it;
            for (std::pair<it, it> r(entire(rows(M)), entire(rows(tM)));
     !r.first.at_end(); ++r.first, ++r.second){
                if((*(r.first))[0] == 0) *(r.second) = *(r.first);
                else *(r.second) = *(r.first)-t;
            }
            return tM;
        }

    template<typename Scalar>
        Vector<Scalar> separate_strong(perl::Object p1, perl::Object p2)
        {
            const Matrix<Scalar>
                V = p1.give("VERTICES | POINTS"),
                  W = p2.give("VERTICES | POINTS"),
                  L1 = p1.give("LINEALITY_SPACE | INPUT_LINEALITY"),
                  L2 = p2.give("LINEALITY_SPACE | INPUT_LINEALITY");

            int i=0; //index of the first non-ray in V
            for(auto r = entire(rows(V)); !r.at_end(); ++r, ++i)
               if((*r)[0] != 0) break;

            //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[i];
            t[0]=0;

            const Matrix<Scalar> ineqs( ( (translate_non_rays(V,t) / -translate_non_rays(W,t)) | //V is on the positive, W on the negative side of the hyperplane
                        ( -ones_vector<Scalar>(V.rows()+W.rows()))) //vz - eps >= 0 and wz + eps <= 0
                    /  unit_vector<Scalar>(V.cols()+1, V.cols()) //eps >=0
                    / (unit_vector<Scalar>(V.cols()+1, 0) - unit_vector<Scalar>(V.cols()+1, V.cols()))); //eps <= z_0 to ensure boundedness (the solver sets z_0 = 1 or z_0 = 0 always)

            Matrix<Scalar> eqs(0, V.cols());
            if(L1.rows()) eqs /= L1;
            if(L2.rows()) eqs /= L2;
            eqs = eqs | zero_vector<Scalar>();

            const Vector<Scalar> obj = unit_vector<Scalar>(V.cols()+1, V.cols());//maximize eps

            to_interface::solver<Scalar> S;
            Vector<Scalar> P, sep_hyp;

            Vector<Scalar> sol;
            //esception is caught in the wrapper
            std::pair<Scalar,Vector<Scalar>> sol_pair = S.solve_lp(ineqs, eqs, obj, true);
            if(sol_pair.first == 0) throw infeasible(); //the only separating plane is a weak one
            sol = sol_pair.second.slice(0, V.cols());

            //translate sol back to the original polytope position
            for(int i=1;i<sol.dim(); ++i){
                if(sol[i] != 0){
                    t[i] -= sol[0]/sol[i];
                    break;
                }
            }
            t[0]=1;
            sol[0] = -(sol * t - sol[0]);
            return sol;
        }

    //finds a weak separating plane for a full-dimensional polytope p1 from a polytope p2
    //t is an inner point of p1 that is not a vertex of p2
    //called from separate_weak
    template<typename Scalar>
        Vector<Scalar> separate_weak(perl::Object p1, perl::Object p2, Vector<Scalar> t)
        {
            const Matrix<Scalar>
                V = p1.give("VERTICES | POINTS"),
                  W = p2.give("VERTICES | POINTS"),
                  L1 = p1.give("LINEALITY_SPACE | INPUT_LINEALITY"),
                  L2 = p2.give("LINEALITY_SPACE | INPUT_LINEALITY");

            //the solution of the LP is supposed to define the separating hyperplane.
            //it thus must be constructed in a way that rules out the origin as a solution
            //as that does not define a plane. therefore the following two choices were made.

            //origin needs to be on the positive side of the separating plane and must not be a vertex of p1
            // in order for the LP to work. translate the whole thing s.t. the origin lies in the interior of p1
            t[0]=0;

            int i=0; //index of the first non-ray in V
            for(auto r = entire(rows(V)); !r.at_end(); ++r, ++i)
               if((*r)[0] != 0) break;


            //obj must not be a vertex of p1 or p2, so we choose an inner point of the translated polytope.
            const Vector<Scalar> obj = ones_vector<Scalar>(1) | ((V[i] - t)/2).slice(1);

            const Matrix<Scalar> ineqs( ( translate_non_rays(V,t) / -translate_non_rays(W,t)) //V is on the positive, W on the negative side of the hyperplane
                    / (2*ones_vector<Scalar>(1) | obj.slice(1))); //obj*z >= -1 to ensure boundedness

            Matrix<Scalar> eqs(0, V.cols());
            if(L1.rows()) eqs /= L1;
            if(L2.rows()) eqs /= L2;

            to_interface::solver<Scalar> S;
            Vector<Scalar> P, sep_hyp;

            Vector<Scalar> sol;
            //exception is caught in separate_weak
            sol = S.solve_lp(ineqs, eqs, obj, false).second; //minimize obj*z

            //translate sol back to the original polytope position
            for(int i=1;i<sol.dim(); ++i){
                if(sol[i] != 0){
                    t[i] -= sol[0]/sol[i];
                    break;
                }
            }
            t[0]=1;
            sol[0] = -(sol * t - sol[0]);
            return sol;
        }


    //the separating plane is found using an LP whose solution is the normal vector.
    //to_solver forces the first coodinate of the solution to be 1, so the separating plane
    //cannot pass through the origin (as that would mean first coordinate 0).
    //to avoid that, we translate the polytopes such that the origin lies in the interior
    //of the polytope that is supposed to lie on the positive side of the plane.
    //in case both polytopes are low dimensional, we have to take extra care (see below)
    template<typename Scalar>
        Vector<Scalar> separate_weak(perl::Object p1, perl::Object p2)
        {
            const Matrix<Scalar>
                V = p1.give("VERTICES | POINTS"),
                  W = p2.give("VERTICES | POINTS"),
                  L1 = p1.give("LINEALITY_SPACE | INPUT_LINEALITY"),
                  L2 = p2.give("LINEALITY_SPACE | INPUT_LINEALITY");

            Matrix<Scalar> ker1 = null_space(V/L1);

            if(!ker1.rows()){ //p1 is fulldim
                Vector<Scalar> inn = p1.give("REL_INT_POINT");
                return separate_weak<Scalar>(p1,p2,inn);
            }
            else{
                Matrix<Scalar> ker2 = null_space(W/L2);

                if(!ker2.rows()){ //p2 is fulldim
                    Vector<Scalar> inn = p2.give("REL_INT_POINT");
                    return -separate_weak<Scalar>(p2,p1,inn); //p2 is fulldim => swap
                }
                else{ //both are lowdim

                    Vector<Scalar> sol;

                    try{
                        //we need to find an inner point of p1 that is not a vertex of p2.
                        //the loop terminates after a maximum of 2 restarts as there is a maximum of
                        //2 vertices of p2 on the line from inn to V[i]

                        if(V.rows()==1) throw infeasible(); //p1 is a point

                        int i=0; //index of the first non-ray in V
                        for(auto r = entire(rows(V)); !r.at_end(); ++r, ++i)
                            if((*r)[0] != 0) break;

                        Vector<Scalar> inn = p1.give("REL_INT_POINT");
                        for(auto r = entire(rows(W)); !r.at_end(); ){
                            if(*r == inn){
                                inn = ones_vector<Scalar>(1) | (inn + (V[i]-inn)/2).slice(1);
                                r = entire(rows(W));
                            }
                            else ++r;
                        }

                        sol = separate_weak<Scalar>(p1,p2,inn);
                    }catch(infeasible){

                        try{
                            if(W.rows()==1) throw infeasible(); //p2 is a point

                            int i=0; //index of the first non-ray in V
                            for(auto r = entire(rows(W)); !r.at_end(); ++r, ++i)
                                if((*r)[0] != 0) break;

                            //maybe the inner point lies on the separating plane!
                            //try the same for p1 and p2 swapped:
                            Vector<Scalar> inn = p2.give("REL_INT_POINT");
                            for (auto r = entire(rows(V)); !r.at_end(); ) {
                                if (*r == inn) {
                                    inn = ones_vector<Scalar>(1) | (inn + (W[i]-inn)/2).slice(1);
                                    r = entire(rows(V));
                                }
                                else ++r;
                            }
                            sol= -separate_weak<Scalar>(p2,p1,inn);
                        } catch (infeasible) {
                            // there is only two options left:
                            // either both inner points lie on the separating plane, so p1 and p2 lie in a common hyperplane,
                            // or they are inseparable.
                            if ((W*T(ker1)).non_zero()) throw infeasible();//no common plane
                            sol = ker1[0];
                        }
                    }
                    return sol;
                }
            }
        }

//separate two polytopes
    template<typename Scalar>
        Vector<Scalar> separating_hyperplane(perl::Object p1, perl::Object p2, perl::OptionSet options)
        {
            bool strong = options["strong"];
            Vector<Scalar> sol;
            try {
                if (strong)
                   sol = separate_strong<Scalar>(p1,p2);
                else
                   sol = separate_weak<Scalar>(p1,p2);
            }
            catch (infeasible) {
                throw std::runtime_error("separating_hyperplane: the given polytopes cannot be separated");
            }
            return sol;
        }

  template <typename Scalar>
      bool cone_contains_point(perl::Object p, const Vector<Scalar> & q, perl::OptionSet options)
      {
          //the LP constructed here is supposed to find a conical/convex combination of the rays/vertices
          //of cone/polytope p that equals q. if the in_interior flag is set, all coefficients are
          //required to be strictly positive.
          Matrix<Scalar> P = p.give("RAYS | INPUT_RAYS");

          Matrix<Scalar> eqs = -q | T(P) ; //sum z_i p_i = q
          //(if p_i are given in hom.coords, also sum z_i = 1 for all i whose p_i are not rays)

          Matrix<Scalar> E = p.give("LINEALITY_SPACE | INPUT_LINEALITY");
          if(E.rows()){
              //for polytopes, replace first row of E with 0, so we have sum z_i = 1 only for points
              if(p.isa("Polytope")) E = zero_vector<Scalar>() / E.minor(range(1,E.rows()-1),All);

              eqs = eqs | T(E);
          }
          eqs = eqs | zero_vector<Scalar>();

          int n = P.rows();
          Matrix<Scalar> ineqs = zero_vector<Scalar>(n) | unit_matrix<Scalar>(n) | zero_matrix<Scalar>(n, E.rows()) | -ones_vector<Scalar>(n); //z_i >= eps for rays

          int c = n + E.rows() + 2;
          ineqs = ineqs / unit_vector<Scalar>(c,c-1) //eps >= 0
              / (unit_vector<Scalar>(c,0) - unit_vector<Scalar>(c,c-1)); // eps <= 1

          Vector<Scalar> obj = unit_vector<Scalar>(c,c-1); //maximize eps
          to_interface::solver<Scalar> S;
          try {
              std::pair<Scalar,Vector<Scalar>> sol = S.solve_lp(ineqs, eqs, obj, true);
              bool in = options["in_interior"];
              if(sol.first == 0 && in) return false; //the only separating plane is a weak one

              return true;
          }
          catch (infeasible) {
              return false;
          }

      }

    //check for separability of a point and a set of points
    template<typename Scalar>
        bool separable(perl::Object p, const Vector<Scalar>& q, perl::OptionSet options)
        {
            bool strong = options["strong"];

            return !cone_contains_point<Scalar>(p, q, perl::OptionSet("in_interior", !strong));
        }

    FunctionTemplate4perl("cone_contains_point<Scalar> [ is_ordered_field_with_unlimited_precision(type_upgrade<Scalar, Rational>) ](Cone<Scalar>, Vector<Scalar>, {in_interior=>0})");

    UserFunctionTemplate4perl("# @category Geometry"
            "# Checks whether there exists a hyperplane separating a given point //q//"
            "# from a polytope/cone //P// by solving a suitable LP."
            "# If true, //q// is a vertex of the polytope defined by //q// and the vertices of //P//."
            "# To get the separating hyperplane, use __separating_hyperplane__."
            "# Works without knowing the facets of P!"
            "# @param Vector q the vertex (candidate) which is to be separated from //P//"
            "# @param Cone P the polytope/cone from which //q// is to be separated"
            "# @option Bool strong Test for strong separability. default: true"
            "# @return Bool 'true' if //q// is separable from //p//"
            "# @example  > $q = cube(2)->VERTICES->row(0);"
            "# > print separable(cube(2), $q, strong=>0);"
            "# | 1",
            "separable<Scalar>(Cone<type_upgrade<Scalar>>, Vector<type_upgrade<Scalar>>, {strong=>1})");

    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 throws an //infeasible// exception."
            "# 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 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);"
            "# > print separating_hyperplane($q,$points);"
            "# | 0 1/2 1/2",
            "separating_hyperplane<Scalar>(Vector<type_upgrade<Scalar>>, Matrix<type_upgrade<Scalar>>)");

    UserFunctionTemplate4perl("# @category Optimization"
            "# Computes (the normal vector of) a hyperplane which separates two given polytopes"
            "# //p1// and //p2// if possible. Works by solving a linear program, not by facet enumeration."
            "# @param Polytope p1 the first polytope, will be on the positive side of the separating hyperplane"
            "# @param Polytope p2 the second polytope"
            "# @option Bool strong If this is set to true, the resulting hyperplane will be strongly separating,"
            "#  i.e. it wont touch either of the polytopes. If such a plane does not exist, an exception"
            "#  will be thrown. default: true"
            "# @return Vector a hyperplane separating //p1// from //p2//\n",
            "separating_hyperplane<Scalar>(Polytope<type_upgrade<Scalar>>, Polytope<type_upgrade<Scalar>>, {strong=>1})");
} }




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