# Copyright (C) 2014 Quang-Cuong Pham <cuong.pham@normalesup.org>
# Algorithm to convert between the span and face representations of polyhedral convex cones and matrices
# Based on S. Hirai's thesis : "Analysis and planning of manipulation using the theory of polyhedral convex cones" (1991) 

from numpy import *
import random
random.seed()
import cvxopt
import cvxopt.solvers
import itertools
cvxopt.solvers.options['show_progress'] = False

TINY = 1e-10

# Check whether all elements of a list have the same sign
def ElementsHaveSameSign(v):
    if abs(v[0])<=TINY:
        return False
    for i in range(1,len(v)):
        if abs(v[i])<=TINY or v[i]*v[0]<=0:
            return False
    return True

# Check whether all elements of the list are negative
def CheckNegative(x):
    for xi in x:
        if xi > TINY:
            return False
    return True

# Regularize the equality constraint A*x = b in a QP/LP problem
# Return False if b is not in the range of A
def RegularizeConstraints(A,b):
    U, S, V = linalg.svd(A)    
    d = sum([abs(x)>TINY for x in S])
    Snew = diag(S[:d])
    Vnew = V[:d,:]
    bnew = dot(linalg.inv(U),b)
    if max(abs(bnew[d:]))<=TINY:
        return True, dot(Snew,Vnew), bnew[:d]
    else:
        return False, None, None

# Build a matrix whose rows are vectors in the list
def BuildMatrix(vlist):
    n = len(vlist)
    d = len(vlist[0])
    A = zeros((n,d))
    for i in range(n):
        A[i,:] = array(vlist[i])
    return A

# Check whether vector v can be written as a convex sum of a list of vectors
def IsConvexSum(v,vlist,returnprimalgap=False):
    n = len(vlist)
    if n == 0:
        return False
    d = len(v)
    qp_P = cvxopt.matrix(eye(n))
    qp_q = cvxopt.matrix(zeros(n))
    qp_G = cvxopt.matrix(-eye(n))
    qp_h = cvxopt.matrix(zeros(n))
    A = transpose(BuildMatrix(vlist))
    b = array(v)
    if linalg.matrix_rank(A) < A.shape[0]:
        res, A, b = RegularizeConstraints(A,b)
        if not res:
            return False
    qp_A = cvxopt.matrix(A)
    qp_b = cvxopt.matrix(b)
    try:
        sol = cvxopt.solvers.qp(qp_P, qp_q, qp_G, qp_h, qp_A, qp_b)
    except Exception as inst:
        print inst
        return False
    if returnprimalgap:
        if sol['status'] != 'optimal':
            return sol['primal infeasibility']
        else:
            return 0
    else:
        if sol['status'] != 'optimal':
            return False
        else:
            return True

# Reduce a list of vectors (either a span or face representation)
# A vector is eliminated if it can be written as the convex sum of the other vectors
# Return the number of vectors eliminated
def Reduce(vlist):
    nreduce = 0
    currentpos = 0
    while currentpos < len(vlist):
        print "Reduce,", currentpos+1, "/", len(vlist)
        v = vlist.pop(currentpos)
        if not IsConvexSum(v,vlist):
            vlist.insert(currentpos,v)
            currentpos += 1
        else:
            nreduce += 1
    return nreduce

# Prune vectors from a list of vectors (either a span or face representation)
# The vector closest to its projection on the cone formed by the remaining vectors is pruned at each step

def Proj(x,vlist):
    PT = BuildMatrix(vlist)
    qp_P = cvxopt.matrix(dot(PT,PT.T))
    qp_q = cvxopt.matrix(-dot(x,PT.T))
    qp_G = cvxopt.matrix(-eye(len(qp_q)))
    qp_h = cvxopt.matrix(zeros(len(qp_q)))
    sol = cvxopt.solvers.qp(qp_P, qp_q, qp_G, qp_h)
    z = sol['x']
    z = array(z).reshape((len(z),))
    return dot(PT.T,z)

def Prune(vlist,nmax):
    while len(vlist)>nmax :
        gaplist = []
        for pos in range(len(vlist)):
            v = vlist.pop(pos)
            vunit = v/linalg.norm(v)
            vproj = Proj(vunit,vlist)
            gaplist.append(linalg.norm(vunit-vproj))
            vlist.insert(pos,v)
        vlist.pop(gaplist.index(min(gaplist)))

def RandomPrune(vlist,nmax):
    while len(vlist)>nmax :
        vlist.pop(random.randint(0,len(vlist))-1)



# The class of Polyhedral Convex Cones
class PCC:
    def __init__(self):
        self.face = []
        self.span = []

    # Set the span representation
    def SetSpan(self,vlist,doreduce = True):
        vlist2 = [array(a) for a in vlist]
        if doreduce:
            Reduce(vlist2)
        self.span = vlist2

    # Set the face representation
    def SetFace(self,vlist,doreduce = True):
        vlist2 = [array(u) for u in vlist]
        if doreduce:
            Reduce(vlist2)
        self.face = vlist2

    # Compute the dual cone
    def ComputeDualCone(self):
        dualcone = PCC()
        dualcone.SetSpan(self.face)
        dualcone.SetFace(self.span)
        return dualcone

    # Compute the face representation from the span representation
    def ComputeFaceRepresentation(self):
        dualcone = self.ComputeDualCone()
        dualcone.ComputeSpanRepresentation()
        self.face = [array(a) for a in dualcone.span]

    # Compute the span representation from the face representation
    def ComputeSpanRepresentation(self):
        self.span = []
        n = len(self.face[0])
        # First, compute A[void] by SVD
        vlist = [array(u) for u in self.face]
        M = BuildMatrix(vlist)
        U,S,V = linalg.svd(M)
        # Dimension of A[void]
        d = n - sum([abs(x)>TINY for x in S])
        print "dim A[void] = ", d
        # Insert the +/- e_i where e_i are the base vectors of A[void]
        vdim = len(self.face[0])
        for i in range(vdim-d,vdim):
            self.span.append(V[i,:])
            self.span.append(-V[i,:])
        # Now compute the f_i by enumerating all the subsets of {1,...,m}
        m = len(self.face)
        fullset = set(range(m))
        totaliter = pow(2,len(fullset))-2
        subsetswithgooddim = []
        iteri = 0
        for subsetsize in range(1,m):
            for I in itertools.combinations(fullset,subsetsize):
                iteri += 1
                print "Iter", iteri, "/", totaliter
                # Check whether I a superset of a "good" subset
                setI = set(I)
                issuperset = False
                for smallset in subsetswithgooddim:
                    if smallset <= setI:
                        print "Is a superset:", I, smallset
                        issuperset = True
                        break
                if issuperset:
                    continue
                # Check the dimension of L[I]
                M = BuildMatrix([vlist[i] for i in fullset - setI])
                U,S,V = linalg.svd(M)
                dI = n - sum([abs(x)>TINY for x in S])
                if dI != d+1:
                    continue
                # Insert the set I into the list of "good" subset
                subsetswithgooddim.append(setI)
                # Find a vector of A[i] within the basis of L[i]
                vlistI = [vlist[i] for i in I]
                found = False
                for k in range(V.shape[0]-dI,V.shape[0]):
                    basek = V[k,:]
                    if ElementsHaveSameSign([dot(x,basek) for x in vlistI]):
                        if dot(vlistI[0],basek) < 0:
                            f = basek
                        else:
                            f = -basek
                        found = True
                        break
                if found:   
                    if not IsConvexSum(f,self.span):
                        self.span.append(f/linalg.norm(f))
                        print "Found a new vector! Span size:", len(self.span)
                else:
                    continue

# Convert a face-form matrix into a span-form matrix
def ConvertFaceToSpan(M):
    cone = PCC()
    cone.SetFace([M[i,:] for i in range(M.shape[0])])
    cone.ComputeSpanRepresentation()
    return transpose(array(cone.span))

# Convert a span-form matrix into a face-form matrix
def ConvertSpanToFace(M):
    cone = PCC()
    cone.SetSpan([M[:,i] for i in range(M.shape[1])])
    cone.ComputeFaceRepresentation()
    return array(cone.face)