% Copyright 2016- Reza Iraji (rezairaji@gmail.com)
%    This file is implementation of PVA algorithm (YALMIP toolbox is required).

%    You can redistribute this file and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation; either version 3 of the License, or
%    (at your option) any later version.

%    This file 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.

%    You should have received a copy of the GNU General Public License
%    along with PVA; if not, write to the Free Software
%    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301  USA

%***************************************************************************
% * Title:          pva.m 
% * Author:         Reza Iraji
% * Created:        2016
% * Last modified:  06/25/2017
% *
% * Copyright (c) 2016- Reza Iraji
% * All Rights Reserved
%***************************************************************************

clear;
clear('yalmip');

tic;

%----------------------Data Points of Microwave----------------------------
% P{1} = [41 0 19 36 0 12];
% P{1}  = [36 0 12];         % 0 degree
% P{2}  = [35.45 -6.25 12];  % 10 degree
% P{3}  = [33.83 -12.31 12]; % 20 degree
% P{4}  = [31.18 -18 12];    % 30 degree
% P{5}  = [27.58 -23.14 12]; % 40 degree
% P{6}  = [23.14 -27.58 12]; % 50 degree
% P{7}  = [18 -31.18 12];    % 60 degree
% P{8}  = [12.31 -33.83 12]; % 70 degree
% P{9}  = [6.25 -35.45 12];  % 80 degree
% P{10} = [0 -36 12];        % 90 degree
%--------------------------------------------------------------------------

%-------------------------Data Points of Drawer----------------------------
% P{1}  = [0 0 0];
% P{2}  = [0 -2 0];
% P{3}  = [0 -4 0];
% P{4}  = [0 -6 0];
% P{5}  = [0 -8 0];
% P{6}  = [0 -10 0];
% P{7}  = [0 -12 0];
% P{8}  = [0 -14 0];
% P{9}  = [0 -16 0];
% P{10} = [0 -18 0];
%--------------------------------------------------------------------------

%------------------------Data Points of Puma 560 (1)-----------------------
P{1}  = [0 0 0 0.4318 0.0000 0.0025 0.4521 -0.1500 0.0025 0.4521 -0.1500 0.4343 0.4521 -0.1500 0.4343 0.4521 -0.1500 0.4343];
P{2}  = [0 0 0 0.4309 0.0000 0.0275 0.4512 -0.1500 0.0275 0.4512 -0.1500 0.4593 0.4512 -0.1500 0.4593 0.4512 -0.1500 0.4593];
P{3}  = [0 0 0 0.4226 0.0000 0.0886 0.4429 -0.1500 0.0886 0.4429 -0.1500 0.5204 0.4429 -0.1500 0.5204 0.4429 -0.1500 0.5204];
P{4}  = [0 0 0 0.3921 0.0000 0.1809 0.4124 -0.1500 0.1809 0.4124 -0.1500 0.6127 0.4124 -0.1500 0.6127 0.4124 -0.1500 0.6127];
P{5}  = [0 0 0 0.3269 0.0000 0.2821 0.3472 -0.1500 0.2821 0.3472 -0.1500 0.7139 0.3472 -0.1500 0.7139 0.3472 -0.1500 0.7139];
P{6}  = [0 0 0 0.2322 0.0000 0.3640 0.2525 -0.1500 0.3640 0.2525 -0.1500 0.7958 0.2525 -0.1500 0.7958 0.2525 -0.1500 0.7958];
P{7}  = [0 0 0 0.1319 0.0000 0.4111 0.1522 -0.1500 0.4111 0.1522 -0.1500 0.8429 0.1522 -0.1500 0.8429 0.1522 -0.1500 0.8429];
P{8}  = [0 0 0 0.0534 0.0000 0.4285 0.0737 -0.1500 0.4285 0.0737 -0.1500 0.8603 0.0737 -0.1500 0.8603 0.0737 -0.1500 0.8603];
P{9}  = [0 0 0 0.0109 0.0000 0.4317 0.0312 -0.1500 0.4317 0.0312 -0.1500 0.8635 0.0312 -0.1500 0.8635 0.0312 -0.1500 0.8635];
P{10} = [0 0 0 0.0001 0.0000 0.4318 0.0204 -0.1500 0.4318 0.0204 -0.1500 0.8636 0.0204 -0.1500 0.8636 0.0204 -0.1500 0.8636];
% P{1}  = [0.4318 0.0025 0.4521 0.0025 0.4521 0.4343 0.4521 0.4343 0.4521 0.4343];
% P{2}  = [0.4309 0.0275 0.4512 0.0275 0.4512 0.4593 0.4512 0.4593 0.4512 0.4593];
% P{3}  = [0.4226 0.0886 0.4429 0.0886 0.4429 0.5204 0.4429 0.5204 0.4429 0.5204];
% P{4}  = [0.3921 0.1809 0.4124 0.1809 0.4124 0.6127 0.4124 0.6127 0.4124 0.6127];
% P{5}  = [0.3269 0.2821 0.3472 0.2821 0.3472 0.7139 0.3472 0.7139 0.3472 0.7139];
% P{6}  = [0.2322 0.3640 0.2525 0.3640 0.2525 0.7958 0.2525 0.7958 0.2525 0.7958];
% P{7}  = [0.1319 0.4111 0.1522 0.4111 0.1522 0.8429 0.1522 0.8429 0.1522 0.8429];
% P{8}  = [0.0534 0.4285 0.0737 0.4285 0.0737 0.8603 0.0737 0.8603 0.0737 0.8603];
% P{9}  = [0.0109 0.4317 0.0312 0.4317 0.0312 0.8635 0.0312 0.8635 0.0312 0.8635];
% P{10} = [0.0001 0.4318 0.0204 0.4318 0.0204 0.8636 0.0204 0.8636 0.0204 0.8636];
%--------------------------------------------------------------------------

%------------------------Data Points of Puma 560 (2)-----------------------
P{1}  = [0 0 0 0.4318 0.0019 -0.0029 0.4528 -0.1480 -0.0032 0.4581 -0.1480 0.4286 0.4581 -0.1480 0.4286 0.4581 -0.1480 0.4286];
P{2}  = [0 0 0 0.4301 0.0209 -0.0315 0.4575 -0.1280 -0.0342 0.5150 -0.1252 0.3938 0.5150 -0.1252 0.3938 0.5150 -0.1252 0.3938];
P{3}  = [0 0 0 0.4145 0.0660 -0.1013 0.4563 -0.0793 -0.1099 0.6357 -0.0507 0.2818 0.6357 -0.0507 0.2818 0.6357 -0.0507 0.2818];
P{4}  = [0 0 0 0.3594 0.1232 -0.2053 0.4198 -0.0147 -0.2213 0.7419 0.0957 0.0444 0.7419 0.0957 0.0444 0.7419 0.0957 0.0444];  
P{5}  = [0 0 0 0.2532 0.1531 -0.3145 0.3322 0.0255 -0.3347 0.7006 0.2483 -0.3022 0.7006 0.2483 -0.3022 0.7006 0.2483 -0.3022];
P{6}  = [0 0 0 0.1273 0.1225 -0.3940 0.2238 0.0072 -0.4115 0.4913 0.2646 -0.6321 0.4913 0.2646 -0.6321 0.4913 0.2646 -0.6321];
P{7}  = [0 0 0 0.0312 0.0448 -0.4283 0.1441 -0.0556 -0.4380 0.2610 0.1124 -0.8182 0.2610 0.1124 -0.8182 0.2610 0.1124 -0.8182];
P{8}  = [0 0 0 -0.0169 -0.0336 -0.4302 0.1082 -0.1190 -0.4322 0.1279 -0.0797 -0.8618 0.1279 -0.0797 -0.8618 0.1279 -0.0797 -0.8618];
P{9}  = [0 0 0 -0.0327 -0.0795 -0.4232 0.0984 -0.1552 -0.4210 0.0812 -0.1972 -0.8504 0.0812 -0.1972 -0.8504 0.0812 -0.1972 -0.8504];
P{10} = [0 0 0 -0.0355 -0.0913 -0.4205 0.0971 -0.1644 -0.4173 0.0724 -0.2277 -0.8438 0.0724 -0.2277 -0.8438 0.0724 -0.2277 -0.8438];
%--------------------------------------------------------------------------

spaceDim  = 18;
maxDegree = 4;
polyNo    = 17;
expNo     = 1;
range     = 1000;
pointsNo  = size(P, 2);

n = spaceDim + spaceDim * spaceDim;
x = sdpvar(1,n);

%-----------------------------Three-Leaved Clover--------------------------
% f(1) = (x(1)-1)^4 + 2*((x(1)-1)^2)*((x(2)-1)^2) + (x(2)-1)^4 - (x(1)-1)^3 + 3*(x(1)-1)*((x(2)-1)^2);
% f(2) = x(3);
%--------------------------------------------------------------------------

%----------------------------------Ampersand-------------------------------
% f(1) = ((x(2)-1)^2 - (x(1)-1)^2)*((x(1)-1) - 1)*(2*(x(1)-1) - 3) - 4*((x(1)-1)^2 + (x(2)-1)^2 - 2*(x(1)-1))^2;
% f(2) = x(3);
%--------------------------------------------------------------------------

%------------------------------Bicuspid------------------------------
% f(1) = ((x(1)-1)^2 - 1)*(x(1) - 2)^2 + ((x(2)-1)^2 - 1)^2;
% f(2) = x(3);
%--------------------------------------------------------------------------

%-------------------------------Bow----------------------------------
% f(1) = (x(1)-1)^4 - ((x(1)-1)^2)*(x(2)-1) + (x(2)-1)^3;
% f(2) = x(3);
%--------------------------------------------------------------------------

%------------------------------Elliptic------------------------------
% f(1) = (x(2)-1)^2 - (x(1)-1)^3 + x(1) - 2;
% f(2) = x(3);
%--------------------------------------------------------------------------

%-------------------------------Watt's---------------------------------
% f(1) = ((x(1)-1)^2 + (x(2)-1)^2)*((x(1)-1)^2 + (x(2)-1)^2 - 1)^2 + 4*((x(2)-1)^2)*((x(1)-1)^2 + (x(2)-1)^2 - 1);
% f(2) = x(3);
%--------------------------------------------------------------------------

%----------------------------------Torus-----------------------------------
% f(1) = ((x(1)-1)^2 + (x(2)-1)^2 + (x(3)-1)^2 + 4 - 1)^2 - 4*4*((x(1)-1)^2 + (x(2)-1)^2);
%--------------------------------------------------------------------------

%--------------------------------Klein Bottle------------------------------
% f(1) = ((x(1)-1)^2 + (x(2)-1)^2 + (x(3)-1)^2 + 2*(x(2)-1) - 1)*(((x(1)-1)^2 + (x(2)-1)^2 + (x(3)-1)^2 - 2*(x(2)-1) - 1)^2 - 8*(x(3)-1)^2) + 16*(x(1)-1)*(x(3)-1)*((x(1)-1)^2 + (x(2)-1)^2 + (x(3)-1)^2 - 2*(x(2)-1) - 1);
%--------------------------------------------------------------------------

%----------------------------------Calyx-----------------------------------
% f(1) = (x(1)-1)^2+(x(2)-1)^2*(x(3)-1)^3 - (x(3)-1)^4;
%--------------------------------------------------------------------------

%----------------------------------Calypso---------------------------------
% f(1) = (x(1)-1)^2+(x(2)-1)^2*(x(3)-1) - (x(3)-1)^2;
%--------------------------------------------------------------------------

%---------------------------------Columpius--------------------------------
% f(1) = (x(1)-1)^3*(x(2)-1)+(x(1)-1)*(x(3)-1)^3+(x(2)-1)^3*(x(3)-1)+(x(3)-1)^3+7*(x(3)-1)^2+5*(x(3)-1);
%--------------------------------------------------------------------------

%----------------------------------Daisy-----------------------------------
% f(1) = ((x(1)-1)^2 - (x(2)-1)^3)^2 - ((x(3)-1)^2 - (x(2)-1)^2)^3;
%--------------------------------------------------------------------------

%----------------------------------Dingdong--------------------------------
% f(1) = (x(1)-1)^2 + (x(2)-1)^2 + (x(3)-1)^3 - (x(3)-1)^2;
%--------------------------------------------------------------------------

%-----------------------------------Distel---------------------------------
% f(1) = (x(1)-1)^2 + (x(2)-1)^2 + (x(3)-1)^2 + 1000*((x(1)-1)^2 + (x(2)-1)^2)*((x(1)-1)^2 + (x(3)-1)^2)*((x(2)-1)^2 + (x(3)-1)^2) - 1;
%--------------------------------------------------------------------------

%-----------------------------------Durchblick-----------------------------
% f(1) = (x(1)-1)^3*(x(2)-1) + (x(1)-1)*(x(3)-1)^3 + (x(2)-1)^3*(x(3)-1) + (x(3)-1)^3 + 5*(x(3)-1);
%--------------------------------------------------------------------------

%-----------------------------------Eistüte--------------------------------
% f(1) = ((x(1)-1)^2 + (x(2)-1)^2)^3 - 4 * (x(1)-1)^2 * (x(2)-1)^2 * ((x(3)-1)^2 + 1);
%--------------------------------------------------------------------------

%-------------------------------------Eve----------------------------------
% f(1) = 5*(x(1)-1)^2 + 2*(x(1)-1)*(x(3)-1)^2 + 5*(x(2)-1)^6 + 15*(x(2)-1)^4 + 5*(x(3)-1)^2 - 15*(x(2)-1)^5 - 5*(x(2)-1)^3;
%--------------------------------------------------------------------------

%-----------------------------------Flirt----------------------------------
% f(1) = (x(1)-1)^2-(x(1)-1)^3+(x(2)-1)^2+(x(2)-1)^4+(x(3)-1)^3-10*(x(3)-1)^4;
%--------------------------------------------------------------------------

%-----------------------------------Geisha---------------------------------
% f(1) = (x(1)-1)^2*(x(2)-1)*(x(3)-1) + (x(1)-1)^2*(x(3)-1)^2 - (x(2)-1)^3*(x(3)-1) - (x(2)-1)^3;
%--------------------------------------------------------------------------

%-----------------------------------Harlekin-------------------------------
% f(1) = (x(1)-1)^3*(x(3)-1) + 10*(x(1)-1)^2*(x(2)-1) + (x(1)-1)*(x(2)-1)^2 + (x(2)-1)*(x(3)-1)^2 - (x(3)-1)^3;
%--------------------------------------------------------------------------

%-----------------------------------Helix----------------------------------
% f(1) = 6*(x(1)-1)^2 - 2*(x(1)-1)^4 - (x(2)-1)^2*(x(3)-1)^2;
%--------------------------------------------------------------------------

%-----------------------------------Herz-----------------------------------
% f(1) = (x(2)-1)^2+(x(3)-1)^3-(x(3)-1)^4-(x(1)-1)^2*(x(3)-1)^2;
%--------------------------------------------------------------------------

%-----------------------------Himmel und Hölle-----------------------------
% f(1) = (x(1)-1)^2-(x(2)-1)^2*(x(3)-1)^2;
%--------------------------------------------------------------------------

%----------------------------------Kolibri---------------------------------
% f(1) = (x(1)-1)^3 + (x(1)-1)^2*(x(3)-1)^2 - (x(2)-1)^2;
%--------------------------------------------------------------------------

%----------------------------------Leopold---------------------------------
% f(1) = (x(1)-1)^2*(x(2)-1)^2*(x(3)-1)^2 + 3*(x(1)-1)^2 + 3*(x(2)-1)^2 + (x(3)-1)^2 - 1;
%--------------------------------------------------------------------------

%----------------------------------Octdong---------------------------------
% f(1) = (x(1)-1)^2 + (x(2)-1)^2 + (x(3)-1)^4 - (x(3)-1)^2;
%--------------------------------------------------------------------------

%------------------------------------Plop----------------------------------
% f(1) = (x(1)-1)^2 + ((x(3)-1)+(x(2)-1)^2)^3;
%--------------------------------------------------------------------------

%---------------------------------Seepferdchen-----------------------------
% f(1) = ((x(1)-1)^2 - (x(2)-1)^3)^2 - ((x(1)-1) + (x(2)-1)^2)*(x(3)-1)^3;
%--------------------------------------------------------------------------

%------------------------------------Sofa----------------------------------
% f(1) = (x(1)-1)^2 + (x(2)-1)^3 + (x(3)-1)^5;
%--------------------------------------------------------------------------

%------------------------------2D sphere in 3D-----------------------------
% f(1) = (x(1)-1)^2 + (x(2)-1)^2 + (x(3)-1)^2 - 4;
%--------------------------------------------------------------------------

%------------------------------3D sphere in 4D-----------------------------
% f(1) = (x(1)-1)^2 + (x(2)-1)^2 + (x(3)-1)^2 + (x(4)-1)^2 - 4;
%--------------------------------------------------------------------------

%------------------------------4D sphere in 5D-----------------------------
% f(1) = (x(1)-1)^2 + (x(2)-1)^2 + (x(3)-1)^2 + (x(4)-1)^2 + (x(5)-1)^2 - 4;
%--------------------------------------------------------------------------

%------------------------------5D sphere in 6D-----------------------------
% f(1) = (x(1)-1)^2 + (x(2)-1)^2 + (x(3)-1)^2 + (x(4)-1)^2 + (x(5)-1)^2 + (x(6)-1)^2 - 4;
%--------------------------------------------------------------------------

%------------------------------6D sphere in 7D-----------------------------
% f(1) = (x(1)-1)^2 + (x(2)-1)^2 + (x(3)-1)^2 + (x(4)-1)^2 + (x(5)-1)^2 + (x(6)-1)^2 + (x(7)-1)^2 - 4;

%--------------------------------------------------------------------------
% f(1) = (x(1)-100)^2 + (x(2)-1)^2 + (x(3)-50)^2;
% f(1) = x(3)^2 - x(1)^3 - x(2)^3;
% f(1) = (x(1) - x(2)) * (2*x(1) - x(2) + 1) * (2*x(1) + x(2) - 4);
% f(1) = x(1) * x(2) * x(3);
% f(2) = x(2)^2;
% f(3) = x(1) + x(3);
%--------------------------------------------------------------------------

monomials = monolist(x(1:spaceDim), maxDegree);

B = [];
for i = 1:pointsNo
    assign(x(1:spaceDim), P{i});
    
    B = [B; value(monomials)'];
end
basis   = null(B);
coeffNo = size(basis, 1);
basisNo = size(basis, 2);

finalDim = 1000;
for exp = 1:expNo
    for i = 1:polyNo 
        lambda = -range + 2 * range * rand(1, basisNo);
    
        coeffs = zeros(coeffNo, 1);
        for j = 1:basisNo
            coeffs = coeffs + lambda(j) * basis(:,j);
        end
        
        savedCoeffs{i} = coeffs;
        
        f(i) = coeffs' * monomials;
    end

    A   = reshape(x(spaceDim+1:n), [spaceDim, spaceDim])';
    obj = trace(A);

    F = set([]);
    for i = 1:polyNo
        F = F + [f(i) >= 0];
        F = F + [-f(i) >= 0];
    end

    J           = jacobian([f(1:polyNo)]', x(1:spaceDim));
    constraints = A^2 - J' * J;

    for i = 1:spaceDim
        for j = 1:spaceDim
            F = F + [constraints(i, j) >= 0];
            F = F + [-constraints(i, j) >= 0];
        end
    end

    optimize(F,obj);

%     [sol,z,momentdata] = solvemoment(F,obj,[],3);
%     [sol,z] = solvemoment(F,obj);

%     value(obj)
%     fprintf('x1 = %f\n', value(x(1)));
%     fprintf('x2 = %f\n', value(x(2)));
%     fprintf('x3 = %f\n', value(x(3)));
%     fprintf('x4 = %f\n', value(x(4)));
%     fprintf('x5 = %f\n', value(x(5)));
%     fprintf('x6 = %f\n', value(x(6)));

    eval = replace(J, x(1:spaceDim), value(x(1:spaceDim)));
    varietyDim = spaceDim - rank(full(eval));
    
    toc
    
    fprintf('Variety dimension: %d\n', varietyDim);
    
    for i = 1:polyNo
        sdisplay(f(i));
    end
    
%     for i = 1:polyNo
%         if i == 1
% %             depict3(maxDegree, savedCoeffs{i}, [-2,38,-38,2,0,24], 'red'); % Microwave
%             depict3(maxDegree, savedCoeffs{i}, [-1,1,-18,0,-1,1], 'red'); % Drawer
%         elseif i == 2
% %             depict3(maxDegree, savedCoeffs{i}, [-2,38,-38,2,0,24], 'green'); % Microwave
%             depict3(maxDegree, savedCoeffs{i}, [-1,1,-18,0,-1,1], 'green'); % Drawer
%         end
%         
%         hold on;
%     end
    
%     x = set([]); y = set([]); z = set([]);
%     for i = 1:pointsNo        
%         x = [x P{i}(1)];
%         y = [y P{i}(2)];
%         z = [z P{i}(3)];
%     end
%     plot3(x, y, z, 'linewidth', 4);
%     view(28, 25);
%     axis off;

%     ip = interp3(x, y, z, 'spline');
    
    finalDim = min(finalDim, varietyDim);
end

%toc

fprintf('Minimum variety dimension: %d\n', finalDim);