# Copyright (C) 2014 Quang-Cuong Pham <cuong.pham@normalesup.org>
# Test the PCC conversion algorithms : compute the wrench cone for a rectangular surface
# Based on Caron, Pham and Nakamura (2014)

from numpy import *
import cvxopt
import cvxopt.solvers
import PCC

# Test for rectangular area
# y : the vector [f1x,f1y,f1z,...,f4x,f4y,f4z]
# x : the vector [fx,fy,fz,taux,tauy,tauz]
mu = 0.3 # friction coefficient
X = 0.5 # rectangle width
Y = 0.2 # rectangle height

# Face representation for y
B = zeros((16,12))
for i in range(4):
    B[4*i,[3*i,3*i+1,3*i+2]] = [1,0,-mu]
    B[4*i+1,[3*i,3*i+1,3*i+2]] = [-1,0,-mu]
    B[4*i+2,[3*i,3*i+1,3*i+2]] = [0,1,-mu]
    B[4*i+3,[3*i,3*i+1,3*i+2]] = [0,-1,-mu]

# Relationship between x and y
A = zeros((6,12))
A[0,:] = [1,0,0,1,0,0,1,0,0,1,0,0]
A[1,:] = [0,1,0,0,1,0,0,1,0,0,1,0]
A[2,:] = [0,0,1,0,0,1,0,0,1,0,0,1]
A[3,:] = Y * array([0,0,1,0,0,-1,0,0,-1,0,0,1])
A[4,:] = -X * array([0,0,1,0,0,1,0,0,-1,0,0,-1])
A[5,:] = X * array([0,1,0,0,1,0,0,-1,0,0,-1,0]) - Y * array([1,0,0,-1,0,0,-1,0,0,1,0,0])

# Matrix conversions
BS = PCC.ConvertFaceToSpan(B)
ABS = dot(A,BS)
C = PCC.ConvertSpanToFace(ABS)

# Test the validity of C
ntot = 1000
nByneg = 0
nCxneg = 0
for i in range(ntot):
    y = random.rand(12) - 0.5*ones(12)
    y[2] += 2*random.rand()
    y[5] += 2*random.rand()
    y[8] += 2*random.rand()
    y[11] += 2*random.rand()
    By = dot(B,y)
    x = dot(A,y)
    Cx = dot(C,x)
    # Test 1 : (B*y <= 0) implies (C*x = C*A*y <= 0)
    if PCC.CheckNegative(By):
        nByneg += 1
        print "B*y negative"
        if not PCC.CheckNegative(Cx):
            print "**************************************************"
            print "C*x not negative!!!!"
            print Cx
            raw_input()
    # Test 2 : (C*x <= 0) implies (exists y, x = A*y and B*y <= 0)
    if PCC.CheckNegative(Cx):
        nCxneg += 1
        print "C*x negative"
        n = 12
        qp_P = cvxopt.matrix(eye(n))
        qp_q = cvxopt.matrix(zeros(n))
        qp_G = cvxopt.matrix(B)
        qp_h = cvxopt.matrix(zeros(B.shape[0]))
        qp_A = cvxopt.matrix(A)
        qp_b = cvxopt.matrix(x)
        sol = cvxopt.solvers.qp(qp_P, qp_q, qp_G, qp_h, qp_A, qp_b)
        y2 = array(sol['x']).reshape((n,))
        z = dot(B,y2)
        if max(z) > 0 or linalg.norm(dot(A,y)-x) > 1e-5:
            print "**************************************************"
            print "Could not find any y such that x = A*y and B*y <=0!!!!"
            print "max z", min(z)
            print "norm(B*y-x)", linalg.norm(dot(A,y2)-x)
            raw_input()

print "Number of B*y negative:", nByneg, "/", ntot
print "Number of C*x negative:", nCxneg, "/", ntot


T = array([
    # fx  fy              fz  taux tauy tauz
    [-1,   0,            -mu,    0,   0,   0],
    [+1,   0,            -mu,    0,   0,   0],
    [0,   -1,            -mu,    0,   0,   0],
    [0,   +1,            -mu,    0,   0,   0],
    [0,    0,             -Y,   -1,   0,   0],
    [0,    0,             -Y,   +1,   0,   0],
    [0,    0,             -X,    0,  -1,   0],
    [0,    0,             -X,    0,  +1,   0],
    [-Y,  -X,  -(X + Y) * mu,  +mu,  +mu,  -1],
    [-Y,  +X,  -(X + Y) * mu,  +mu,  -mu,  -1],
    [+Y,  -X,  -(X + Y) * mu,  -mu,  +mu,  -1],
    [+Y,  +X,  -(X + Y) * mu,  -mu,  -mu,  -1],
    [+Y,  +X,  -(X + Y) * mu,  +mu,  +mu,  +1],
    [+Y,  -X,  -(X + Y) * mu,  +mu,  -mu,  +1],
    [-Y,  +X,  -(X + Y) * mu,  -mu,  +mu,  +1],
    [-Y,  -X,  -(X + Y) * mu,  -mu,  -mu,  +1]])

Tlines = [T[i, :] for i in xrange(T.shape[0])]
Clines = [C[i, :] for i in xrange(C.shape[0])]
T_by_C = all([PCC.IsConvexSum(t, Clines) for t in Tlines])
C_by_T = all([PCC.IsConvexSum(c, Tlines) for c in Clines])

if T_by_C and C_by_T:
    print "Everything OK: T <=> C"
elif T_by_C:
    print "C => T but not (T => C)"
elif C_by_C:
    print "T => C but not (C => T)"