function V = volume(CON)

% Compute volume of the polytope represented by a linear constraint
% object
%
% Syntax:
%   "V = volume(con)"
%
% Description:
%   "volume(con)" returns the volume of the region represented by "con".  
%
% Examples:
%   Given,
%
%
%
%     "con ="
%
%     "[ 1.000000 0.000000 0.000000 ]x <= 4.000000"
%
%     "[ -1.000000 0.000000 0.000000 ]x <= -2.000000"
%
%     "[ 0.000000 1.000000 0.000000 ]x <= 3.000000"
%
%     "[ 0.000000 -1.000000 0.000000 ]x <= -1.000000"
%
%     "[ 0.000000 0.000000 1.000000 ]x <= 2.000000"
%
%     "[ 0.000000 0.000000 -1.000000 ]x <= 0.000000"
%
%
%
%   then,
%
%
%
%     "V = volume(con)"
%
%
%
%   results in
%
%
%
%     "V = 8"
%
%
%
% Note:
%   It is assumed that the polytope is full dimensional (n), so there are
%   no equality constraints.
%
% See Also:
%   linearcon

global GLOBAL_OPTIM_PAR

if length(CON.dE) == 1
  [CI,dI] = reduce_dimension(CON);
  disp('(n-1) polytope. Returning (n-1) volume.')
  V = compute_volume(1,[],CI,dI);
  return
end

CI = CON.CI; dI = CON.dI;
V = compute_volume(1,[],CI,dI);
return

function Vk = compute_volume(k,Xpre,CI,dI)

% Compute the constraints restricting x_1, ... ,x_k-1 to
% the values in Xpre

CE = []; dE = [];
n = size(CI,2);
for i = 1:k-1
  ei = [zeros(1,i-1) 1 zeros(1,n-i)]';
  CE(i,:) = ei';
  dE(i,:) = Xpre(i);
end
ek = [zeros(1,k-1) 1 zeros(1,n-k)]';

if (k == n)
  % 0-D volume is simply the length of the line
  xmax = linprog(-(NBDHP{k}.c)',CAR,dAR,[],[],[],[],[],GLOBAL_OPTIM_PAR);
  xmin = linprog((NBDHP{k}.c)',CAR,dAR,[],[],[],[],[],GLOBAL_OPTIM_PAR);
% xmin = lp((NBDHP{k}.c)',CAR,dAR);
% Xmax = lp(-ek,[CE; CI],[dE; dI],[],[],[],k-1);
  xkmin = Xmin(k);
  xkmax = Xmax(k);
  Vk = xkmax-xkmin;
elseif (k == n-1)
  % 1-D volume can only change linearly as a function of x_k
  % in this case the trapeziod rule gives the exact area
  Xk = get_break_points(CE,dE,CI,dI,k);
  Vcache = zeros(size(Xk));
  for i = 1:length(Xk)
    Vcache(i) = compute_volume(k+1,[Xpre Xk(i)],CI,dI);
  end
  Vk = 0;
  for i = 1:length(Xk)-1
    % Trapezoid rule
    Vk = Vk + 0.5*(Xk(i+1)-Xk(i))*(Vcache(i) + Vcache(i+1));
  end       
else
  % (n-k) dimensional volume is a polynomial in x_k of at most
  % degree (n-k)

  Xk = get_break_points(CE,dE,CI,dI,k);
  Vcache = zeros(size(Xk));
  for i = 1:length(Xk)
    Vcache(i) = compute_volume(k+1,[Xpre Xk(i)],CI,dI);
  end
  Vk = 0;
  % need to sample n-k-1 more points
  for i = 1:length(Xk)-1
    h = (Xk(i+1)-Xk(i))/(n-k);
    A = power_vector(Xk(i),n-k);
    b = Vcache(i);
    for j = 1:(n-k)-1
      xk = Xk(i)+j*h;
      A = [A; power_vector(xk,n-k)];
      b = [b; compute_volume(k+1,[Xpre xk],CI,dI)];
    end
    A = [A; power_vector(Xk(i+1),n-k)];
    b = [b; Vcache(i+1)];
    if rank(A) == n-k+1
      coeff = (A\b)';
      volume_coeff = polynomial_integrate(coeff);
      Vk = Vk + (polyval(volume_coeff,Xk(i+1)) - ...
                 polyval(volume_coeff,Xk(i)));
    else
      disp('error')
      Vk = Inf;
    end
  end
end

return

function Xk = get_break_points(CE,dE,CI,dI,k)

vtcs = compute_vertices(CE,dE,CI,dI);

% sort vertices according to the k-th coordinate
Xk = [];
for i = 1:length(vtcs)  
  vi = vtcs(i);
  xki = vi(k);
  j = 1; stop = 0;
  while ~stop & (j <= length(Xk))
    if (Xk(j) >= xki)
      stop = 1;
    else
      j = j + 1;
    end
  end
  if (j > length(Xk))
    Xk = [Xk xki];
  else
    Xk = [Xk(1,1:j-1) xki Xk(1,j:length(Xk))];
  end
end

% remove repeated coordinates
j = 1;
while (j <= length(Xk)-1)
  if abs(Xk(j)-Xk(j+1)) < 1e-6
    Xk = [Xk(1,1:j-1) Xk(1,j+1:length(Xk))];
  else
    j = j + 1;
  end
end

return

function vtcs = compute_vertices(CE,dE,CI,dI)

vtcs = vertices;
n_total = size(CI,2);
n_free = n_total-length(dE);

COMBO = nchoosek([1:length(dI)],n_free);
for i = 1:size(COMBO,1)
  C = CE; d = dE;
  for j = 1:length(COMBO(i,:))
    C = [C; CI(COMBO(i,j),:)];
    d = [d; dI(COMBO(i,j),:)];
  end
  if rank(C) == n_total
    vi = C\d;
    if feasible_point(linearcon([],[],CI,dI),vi)
      vtcs = vtcs | vi;
    end
  end
end
return

function X = power_vector(x,n)

X = [];
for i = 0:n
  X = [x^i X];
end
return

function coeff = polynomial_integrate(coeff)

coeff = [coeff 0];
n = length(coeff);
for i = 1:n-2
  coeff(i) = coeff(i)/(n-i);
end
return


function [CI,dI] = reduce_dimension(CON)

n = size(CON.CE,2);
V = null(CON.CE);
V = V(1:n-1,:);
T = [inv(V) zeros(n-1,1)];
v = vertices(CON);
v_reduced = vertices;
for k = 1:length(v)
  v_reduced = v_reduced | T*(v(k)-v(1));
end
CON_reduced = linearcon(polyhedron(v_reduced));
CI = CON_reduced.CI;
dI = CON_reduced.dI;