function SieveAnalysis5(Granulometria)
% _____________________________________________________
% See Contents.m
%  With this routine you can get the sand sieve analysis. Routine
% displays the computation of the particle-size distribution in different
% windows. Add you can get the main parameters of statistics analysis:
% percentiles, mean, standard deviation, kurtosis, etc.
% Syntax:
%              >> Granulometria[ %your data];
%              >> SieveAnalysis5(Granulometria)
% Update: 1. I've received a couples of emails, in which  some
%                people had a lot of problems with how to give the inputs;
%                for this reason, I decided to delete the GUI. Now, the
%                user only has to do, it's to create a variable with the
%                name "Granulometria" and inside this variable, you need to
%                write your data. First column, the sieve mesh (in mm) and
%                the second, the weights (in g). For example:
%                Granulometria = [4.0 0.88; 2.0 1.08; 1.0 1.49; 0.50 3.58; 0.25 11.50;
%                                                0.125 21.50; 0.0625 9.81; 0.0313 2.70];
%                2. I deleted an enormous bugs.
%
% Last modification : 03/21/09
% Author: PhD(c) Gabriel Ruiz
% This routine is provided "as is" without warranty of any kind.
% Please, you don't attribute it.
% If you'll detect any mistake or bug, may you communicate to me,
% please?
% v1.1.5
% Copyright 2006.
% _____________________________________________________
     clc;

    screen = get(0, 'screensize');
    if screen(1,3) == 1152 && screen(1,4) == 864
                 screen(1,3) = screen(1,3) - 128; screen(1,4) = screen(1,4) - 96;
        elseif screen(1,3) == 1280 && screen(1,4) == 768
                 screen(1,3) = screen(1,3) - 256;
        elseif screen(1,3) == 1280 && screen(1,4) == 800
                 screen(1,3) = screen(1,3) - 256; screen(1,4) = screen(1,4) - 32;
        elseif screen(1,3) == 1280 && screen(1,4) == 960
                 screen(1,3) = screen(1,3) - 256; screen(1,4) = screen(1,4) - 192;
        elseif screen(1,3) == 1280 && screen(1,4) == 1024
                 screen(1,3) = screen(1,3) - 256; screen(1,4) = screen(1,4) - 256;
        elseif screen(1,3) == 1400 && screen(1,4) == 1050
                 screen(1,3) = screen(1,3) - 376; screen(1,4) = screen(1,4) - 282;
        elseif screen(1,3) == 1600 && screen(1,4) == 900
                 screen(1,3) = screen(1,3) - 576; screen(1,4) = screen(1,4) - 132;
        elseif screen(1,3) == 1600 && screen(1,4) == 1200
                 screen(1,3) = screen(1,3) - 576; screen(1,4) = screen(1,4) - 432;
    end
    vpan = screen(1,4)-35; hpan = screen(1,3);
    colores = [1 1 0.85 ]; colortexto = [0.50 0.25 0.25];
    WB = Granulometria(:,2);
    milimeters = Granulometria(:,1);
    r = length(milimeters);

        for i = 1 : r
                Worksheet_sieve_data{1,1}(i) = WB(i);
        end
        Sum_Col_1 = sum( Worksheet_sieve_data{1,1} );
        for i = 1: r
                Worksheet_sieve_data{1,2}(i) = ( Worksheet_sieve_data{1,1}(i) / Sum_Col_1) * 100;
                if i == 1
                        Worksheet_sieve_data{1,3}(i) = Worksheet_sieve_data{1,2}(i);
                        Worksheet_sieve_data{1,4}(i) = 100;
                else
                        Worksheet_sieve_data{1,3}(i) = Worksheet_sieve_data{1,2}(i) + Worksheet_sieve_data{1,3}(i-1);
                        Worksheet_sieve_data{1,4}(i) = Worksheet_sieve_data{1,4}(1) - Worksheet_sieve_data{1,3}(i);
                end
        end

        diamX = Worksheet_sieve_data{1,4} ;
        diamX = diamX';
        diamY = milimeters;
        D_5 = interp1(diamX, diamY, 5, 'pchip');
        D_10 = interp1(diamX, diamY, 10, 'pchip');
        D_16 = interp1(diamX, diamY, 16, 'pchip');
        D_25 = interp1(diamX, diamY, 25, 'pchip');
        D_30 = interp1(diamX, diamY, 30, 'pchip');
        D_50 = interp1(diamX, diamY, 50, 'pchip');
        D_60 = interp1(diamX, diamY, 60, 'pchip');
        D_75 = interp1(diamX, diamY, 75, 'pchip');
        D_84 = interp1(diamX, diamY, 84, 'pchip');
        D_95 = interp1(diamX, diamY, 95, 'pchip');

        resulchar.d_5 = num2str(D_5);
        resulchar.d_10 = num2str(D_10);
        resulchar.d_16 = num2str(D_16);
        resulchar.d_25 = num2str(D_25);
        resulchar.d_30 = num2str(D_30);
        resulchar.d_50 = num2str(D_50);
        resulchar.d_60 = num2str(D_60);
        resulchar.d_75 = num2str(D_75);
        resulchar.d_84 = num2str(D_84);
        resulchar.d_95 = num2str(D_95);

        D_5phi = -log(D_5) / log(2);      %Matlab has the function log2, but I prefer to use this form.
        D_10phi = -log(D_10) / log(2);
        D_16phi = -log(D_16) / log(2);
        D_25phi = -log(D_25) / log(2);
        D_30phi = -log(D_30) / log(2);
        D_50phi = -log(D_50) / log(2);
        D_60phi = -log(D_60) / log(2);
        D_75phi = -log(D_75) / log(2);
        D_84phi = -log(D_84) / log(2);
        D_95phi = -log(D_95) / log(2);

        Mean_grain_size = ( D_16phi + D_50phi + D_84phi ) / 3;
        Mean_grain_size_mm = 2 ^ -(Mean_grain_size);
        resulchar.MeanGS = num2str(Mean_grain_size_mm);

        if Mean_grain_size_mm <= 0.075
                    resulchar.ASTM = 'Fine Soil';
            elseif Mean_grain_size_mm >= 0.076 && Mean_grain_size_mm <= 0.425
                    resulchar.ASTM = 'Fine sand';
            elseif Mean_grain_size_mm >= 0.426 && Mean_grain_size_mm <= 2
                    resulchar.ASTM = 'Medium sand';
            elseif Mean_grain_size_mm >= 2.1 && Mean_grain_size_mm <= 4.75
                    resulchar.ASTM = 'Coarse sand';
            elseif Mean_grain_size_mm >= 4.76 && Mean_grain_size_mm <= 19
                    resulchar.ASTM = 'Fine gravel';
            elseif Mean_grain_size_mm >= 19.1 && Mean_grain_size_mm <= 75
                    resulchar.ASTM = 'Coarse gravel';
        end

        if Mean_grain_size_mm >= 0.0625 && Mean_grain_size_mm <= 0.125
                    resulchar.Wentworth = 'Very fine sand';
            elseif Mean_grain_size_mm >= 0.126 && Mean_grain_size_mm <= 0.250
                    resulchar.Wentworth = 'Fine sand';
            elseif Mean_grain_size_mm >= 0.251 && Mean_grain_size_mm <= 0.50
                    resulchar.Wentworth = 'Medium sand';
            elseif Mean_grain_size_mm >= 0.51 && Mean_grain_size_mm <= 1
                    resulchar.Wentworth = 'Coarse sand';
            elseif Mean_grain_size_mm >= 1.01 && Mean_grain_size_mm <= 2
                    resulchar.Wentworth = 'Very coarse sand';
            elseif Mean_grain_size_mm >= 2.01 && Mean_grain_size_mm <= 4.76
                    resulchar.Wentworth = 'Granule';
            elseif Mean_grain_size_mm >= 4.77 && Mean_grain_size_mm <= 8
                    resulchar.Wentworth = 'Small pebble';
            elseif Mean_grain_size_mm >= 8.01 && Mean_grain_size_mm <= 16
                    resulchar.Wentworth = 'Medium pebble';
            elseif Mean_grain_size_mm >= 16.01 && Mean_grain_size_mm <= 19.03
                    resulchar.Wentworth = 'Large pebble';
        end

        gravel = zeros(r,1);
        for i = 1 : r
            if milimeters(i,1) <= 75 && milimeters(i,1) >= 4.75
                gravel(i,1) = Worksheet_sieve_data{1,2}(i);
            end
        end
        Gravel = sum(gravel(:,1));
        resulchar.Grava = num2str(Gravel);

        sand = zeros(r,1);
        for i = 1 : r
            if milimeters(i,1) <= 4.74 && milimeters(i,1) >= 0.075
                    sand(i,1) = Worksheet_sieve_data{1,2}(i);
            end
        end
        Sand = sum(sand(:,1));
        resulchar.arena = num2str(Sand);

         fine = zeros(r,1);
        for i = 1 : r
            if milimeters(i,1) <= 0.074
                    fine(i,1) = Worksheet_sieve_data{1,2}(i);
            end
        end
        Fine = sum(fine(:,1));
        resulchar.fino =num2str(Fine);
        Muestrapor = [ Gravel Sand Fine ];

        Standard_Deviation = ( ( D_84phi - D_16phi ) / 4 ) + ( ( D_95phi - D_5phi ) / 6 );   % E.3.
        Standard_Deviation_mm = 2 ^ -(Standard_Deviation);
        resulchar.SDe = num2str(Standard_Deviation_mm);
        if Standard_Deviation < 0.34
                    resulchar.SD = 'Very well sorted';
            elseif Standard_Deviation >= 0.35 && Standard_Deviation <= 0.49
                    resulchar.SD = 'Well sorted';
            elseif Standard_Deviation >= 0.50 && Standard_Deviation <= 0.71
                    resulchar.SD = 'Moderately well sorted';
            elseif Standard_Deviation >= 0.72 && Standard_Deviation <= 0.99
                    resulchar.SD = 'Moderately sorted';
            elseif Standard_Deviation >= 1.00 && Standard_Deviation <= 1.99
                    resulchar.SD = 'Poorly sorted';
            elseif Standard_Deviation >= 2.00 && Standard_Deviation <= 3.99
                    resulchar.SD = 'Very poorly sorted';
            elseif Standard_Deviation >= 4.00
                    resulchar.SD = 'Extremely poorly sorted';
        end

        Skewness =  ( ( D_84phi + D_16phi - ( 2 * D_50phi ) ) / ( 2 * ( D_84phi - D_16phi ) ) ) + ...
                              ( ( D_95phi + D_5phi - ( 2 * D_50phi ) ) / ( 2 * ( D_95phi - D_5phi ) ) );
        Skewness_mm = 2 ^ -(Skewness);
        resulchar.S_ke = num2str(Skewness_mm);
        if Skewness < -0.29
                    resulchar.Sk = 'Very coarse skewed';
            elseif Skewness >= -0.30 && Skewness <= -0.09
                    resulchar.Sk = 'Coarse skewed';
            elseif Skewness >= -0.10 && Skewness <= 0.09
                    resulchar.Sk = 'Near symmetrical';
            elseif Skewness >= 0.10 && Skewness <= 0.29
                    resulchar.Sk = 'Fine skewed';
            elseif Skewness >= 0.30
                    resulchar.Sk = 'Very fine skewed';
        end

        Kurtosis = ( D_95phi - D_5phi ) / ( 2.44 * ( D_75phi - D_25phi ) );
        Kurtosis_mm = 2 ^ -(Kurtosis);
        resulchar.K_ur = num2str(Kurtosis_mm);
         if Kurtosis < 0.64
                    resulchar.kurt = 'Very platykurtic (flat)';
            elseif Kurtosis >= 0.65 && Kurtosis <= 0.89
                    resulchar.kurt = 'Platykurtic';
            elseif Kurtosis >= 0.90 && Kurtosis <= 1.10
                    resulchar.kurt = 'Mesokurtic ';
            elseif Kurtosis >= 1.11 && Kurtosis <= 1.49
                    resulchar.kurt = 'Leptokurtic (peaked)';
            elseif Kurtosis >= 1.50 && Kurtosis <= 2.99
                    resulchar.kurt = 'Very leptokurtic';
            elseif Kurtosis >= 3.00
                    resulchar.kurt = 'Extremely leptokurtic';
        end

        Cc_Dispersal = D_30 ^ 2 / ( D_10 * D_60 ) ;
        resulchar.CC = num2str(Cc_Dispersal);
        Cu_Hazen_uniformity = D_60 / D_10;
        resulchar.Cu =num2str(Cu_Hazen_uniformity);
        if Cu_Hazen_uniformity >= 6 && ( ( Cc_Dispersal >= 1 ) && ( Cc_Dispersal <= 3 ) )
                    resulchar.SUCS = 'SW';
        else
                    resulchar.SUCS = 'SP';
        end

        figure(3);
        set(figure(3), 'Units' , 'Pixels' , 'NumberTitle' , 'Off' , 'Resize' , 'on' , ...
                'Name' , '1.Sieve Analysis - Histogram' , 'Color' , [0.93 0.91 0.85] , ...
                'Position' , [  hpan-880   vpan-440   hpan-664   vpan-464 ] );
        histo_h =bar(milimeters,Worksheet_sieve_data{1,2}, 'group');
        colormap hsv;
        xlabel('Grain Size in mm' , 'Fontsize' , 7);
        ylabel('Percent Finer by Weight (%)' , 'Fontsize' , 7);
        title('Histogram' , 'Color' , 'r' , 'FontWeight' , 'Bold' , 'Fontsize' , 8);
        set(histo_h,  'FaceColor', 'r');
        set(gca,  'Xgrid' , 'on' , 'Ygrid' , 'on' , 'Xcolor' , [0 0 0.37] , 'Ycolor' , [0 0 0.37] , 'Box' , 'off' , ...
                   'FontWeight' , 'Bold' , 'FontSize' , 7 , 'FontAngle' , 'Oblique' , 'XMinorTick' , 'on' , 'YMinorTick' , 'on');

        figure(4);
        set(figure(4), 'Units' , 'Pixels' , 'NumberTitle' , 'Off' , 'Resize' , 'on' , 'Color' , [0.93 0.91 0.85] , ...
                'Position' , [  hpan-859   vpan-469  hpan-664  vpan-464 ] , ...
                'name' , '2.Sieve Analysis - Frecuency Curve' );
        FCruve_h = plot(milimeters, Worksheet_sieve_data{1,2}, '-', 'Color' , 'b' , 'Linewidth' , 1.5);
        xlabel('Grain Size in mm' , 'Fontsize' , 7);
        ylabel('Percent Finer by Weight (%)' , 'Fontsize' , 7);
        title('Frecuency Curve' , 'Color' , 'b' , 'FontWeight' , 'Bold' , 'Fontsize' , 8);
        set(gca,  'Xgrid' , 'on' , 'Ygrid' , 'on' , 'Xcolor' , [0 0 0.37] , 'Ycolor' , [0 0 0.37] , 'Box' , 'off' , ...
                   'FontWeight' , 'Bold' , 'FontSize' , 7 , 'FontAngle' , 'Oblique' , 'XMinorTick' , 'on' , 'YMinorTick' , 'on');

        figure(5);
        set(figure(5), 'Units' , 'Pixels' , 'NumberTitle' , 'Off' , 'Resize' , 'on' , 'Color' , [0.93 0.91 0.85] , ...
                'Position' , [hpan-838  vpan-499  hpan-664  vpan-464] , ...
                'name' , '3. Sieve Analysis - Cumulative Arithmetic Curve' );
        ACurve_h = plot(milimeters, Worksheet_sieve_data{1,3}, '-', 'Color' , [0 0.5 0] , 'Linewidth' , 1.5);
        xlabel('Grain Size in mm' , 'Fontsize' , 7);
        ylabel('Percent Finer by Weight (%)' , 'Fontsize' , 7);
        title('Frecuency Cumulative  Curve' , 'Color' , [0 0.5 0] , 'FontWeight' , 'Bold' , 'Fontsize' , 8);
        set(gca,  'Xgrid' , 'on' , 'Ygrid' , 'on' , 'Xcolor' , [0 0 0.37] , 'Ycolor' , [0 0 0.37] , 'Box' , 'off' , ...
                   'FontWeight' , 'Bold' , 'FontSize' , 7 , 'FontAngle' , 'Oblique' , 'XMinorTick' , 'on' , 'YMinorTick' , 'on');

        figure(6);
        set(figure(6), 'Units' , 'Pixels' , 'NumberTitle' , 'Off' , 'Resize' , 'on' , 'Color' , [0.93 0.91 0.85] , ...
                'Position' , [ hpan-816   vpan-529   hpan-664  vpan-464] , ...
                'name' , '4. Sieve Analysis - Cumulative Probability Arithmetic Curve' );
        CCurve_h = plot(milimeters, Worksheet_sieve_data{1,4}, '-','Color' , [1 0.5 0] , 'Linewidth' , 1.5);
        xlabel('Grain Size in mm', 'Fontsize' , 7);
        ylabel('Probability Finer by Weight (%)' , 'Fontsize' , 7);
        title('Cumulative Probability Curve ' , 'Color' , [1 0.5 0] , 'FontWeight' , 'Bold' , 'Fontsize' , 8);
        set(gca,  'Xgrid' , 'on' , 'Ygrid' , 'on' , 'Xcolor' , [0 0 0.37] , 'Ycolor' , [0 0 0.37] , 'Box' , 'off' , ...
                   'FontWeight' , 'Bold' , 'FontSize' , 7 , 'FontAngle' , 'Oblique' , 'XMinorTick' , 'on' , 'YMinorTick' , 'on');

        figure(7);
        set(figure(7), 'Units' , 'Pixels' , 'NumberTitle' , 'Off' , 'Resize' , 'on' , 'Color' , [0.93 0.91 0.85] , ...
                'Position' , [   hpan-778   vpan-587    hpan-348    vpan-464  ] , ...
                 'name' , '6. Sieve Analysis - Cumulative Probability Semi-Log Curve'  );
        CCurvesl_h = semilogx(milimeters, Worksheet_sieve_data{1,4}, '-o','Color' , [0 0 0.5] , ...
                                      'Linewidth' , 2.5, 'MarkerFaceColor' , [ 0 0 0.5 ] , 'MarkerSize' , 2, ...
                                      'MarkerEdgeColor' , [ 0 0.5 0.5] );
        axis tight;
        xlabel('Grain Size in mm', 'Fontsize' , 7);
        ylabel('Probability Finer by Weight (%)' , 'Fontsize' , 7);
        title('Cumulative Probability Curve ' , 'Color' , [0 0 0.5] , 'FontWeight' , 'Bold' , 'Fontsize' , 8);
        set(gca,  'Xgrid' , 'on' , 'Ygrid' , 'on' , 'Xcolor' , [0 0 0.37] , 'Ycolor' , [0 0 0.37] , 'Box' , 'off' , 'XDir' , 'normal' , ...
                   'FontWeight' , 'Bold' , 'FontSize' , 7 , 'FontAngle' , 'Oblique' , 'XMinorTick' , 'on' , 'YMinorTick' , 'on', ...
                   'Xlim' , [0.01 10]  , 'XTickLabel', { '0.01' ; '0.1' ; '1' ;  '10' } );
        dimark = [0.074 4.76];
        hold on;
        escalY = get(gca,'YLim');
        escalY = [ 0 escalY(1,2) ];
        lenmark = length(dimark);
        xmark = reshape([dimark;dimark;ones(1,lenmark)*nan;],lenmark*3,1);
        ymark = repmat([escalY nan],1,lenmark);
        plot(xmark,ymark, 'r', 'LineWidth', 2 );
        punto07 = patch(D_50 , 50 , [1 0 0] );
        punto08 = patch(0.01, 50 ,  [1 0 0] );
        punto09 = patch(D_50 , 0.01 , [1 0 0]);
         l4 = line('Xdata' , [D_50 D_50] , 'Ydata' , [0 50] , 'Color' ,  [0.75 0.75 0.75] , ...
                        'Linestyle' , '-' , 'Linewidth' , 2 ) ;
         l5 = line('Xdata' , [0.01 D_50] , 'Ydata' , [50 50] , 'Color' ,  [0.75 0.75 0.75] , ...
                        'Linestyle' , '-' , 'Linewidth' , 2 );
        set(punto07, 'marker' , 'o' , 'MarkerEdgeColor', 'r' , 'MarkerFaceColor', 'r' , ...
                 'MarkerSize' , 2);
        set(punto08, 'marker' , 'o' , 'MarkerEdgeColor', 'r' , 'MarkerFaceColor', 'r' , ...
                 'MarkerSize' , 2);
        set(punto09, 'marker' , 'o' , 'MarkerEdgeColor', 'r' , 'MarkerFaceColor', 'r', ...
                 'MarkerSize' , 2);
        td50 = text(D_50+.005 , 3, num2str(D_50)); set(td50, 'FontSize' , 8 , ...
                            'FontAngle' , 'Normal' , 'FontWeight' , 'Bold' );
        tfinos = text(0.011 , 95, 'Fine'); set(tfinos, 'FontSize' , 8 , 'FontWeight' , 'Bold');
        tsand = text(0.085 , 95, 'Sand'); set(tsand, 'FontSize' , 8 , 'FontWeight' , 'Bold' );
        tgrav = text(5 , 95, 'Gravel'); set(tgrav, 'FontSize' , 8 , 'FontWeight' , 'Bold' );

        figure(8);
        set(figure(8), 'Units' , 'Pixels' , 'NumberTitle' , 'Off' , 'Resize' , 'on' , 'Color' , [0.93 0.91 0.85] , ...
                'Position' , [   hpan-796   vpan-558    hpan-664  vpan-464  ] , ...
                'name' , '5. Sieve Analysis - Cumulative Probability Logarithmic Curve' );
        CCurvel_h = loglog(milimeters, Worksheet_sieve_data{1,4}, '-','Color' , [0.5 0 0.5] , 'Linewidth' , 1.5);
        xlabel('Grain Size in mm' ,'Fontsize' , 7);
        ylabel('Probability Finer by Weight (%)', 'Fontsize' , 7);
        title('Cumulative Probability Curve ' , 'Color' , [0.5 0 0.5] , 'FontWeight' , 'Bold' , 'Fontsize' , 8);
        set(gca,  'Xgrid' , 'on' , 'Ygrid' , 'on' , 'Xcolor' , [0 0 0.37] , 'Ycolor' , [0 0 0.37] , 'Box' , 'off' , ...
                   'FontWeight' , 'Bold' , 'FontSize' , 7 , 'FontAngle' , 'Oblique' , 'XMinorTick' , 'on' , 'YMinorTick' , 'on'); clc;

        figure(9);
        set(figure(9), 'Units' , 'Pixels' , 'NumberTitle' , 'Off' , 'Resize' , 'on' , 'Color' , [0.93 0.91 0.85] , ...
                'Position' , [  hpan-757   vpan-616    hpan-664  vpan-464  ] , ...
                'name' , '7. Sieve Analysis - % Percent of Sand'  );
        if Muestrapor(1,1) == 0 && Muestrapor(1,2) ~= 0 && Muestrapor(1,3) ~= 0
                    hotcake(1,1) =Muestrapor(1,2);
                    hotcake(1,2) =Muestrapor(1,3);
                    explode = [ 0 1 ];
                    piecg = pie(hotcake, explode);
                    hdlleg = legend('Sand', 'Fine', -1);
                    set(piecg(1,2), 'FontUnits', 'pixels', 'FontSize' , 10);
                    set(piecg(1,4), 'FontUnits', 'pixels', 'FontSize' , 10);
         elseif Muestrapor(1,1) ~= 0 && Muestrapor(1,2) ~= 0 && Muestrapor(1,3) == 0
                    hotcake(1,1) =Muestrapor(1,1);
                    hotcake(1,2) =Muestrapor(1,2);
                    explode = [ 0 1 ];
                    piecg = pie(hotcake, explode);
                    hdlleg = legend('Gravel' , 'Sand',  -1);
                    set(piecg(1,2), 'FontUnits', 'pixels', 'FontSize' , 10);
                    set(piecg(1,4), 'FontUnits', 'pixels', 'FontSize' , 10);
        elseif Muestrapor(1,1) ~= 0 && Muestrapor(1,2) ~= 0 && Muestrapor(1,3) ~= 0
                    explode = [ 0 1 0 ];
                    piecg = pie(Muestrapor, explode);
                    hdlleg = legend('Gravel' , 'Sand', 'Fine', -1);
                    set(piecg(1,2), 'FontUnits', 'pixels', 'FontSize' , 10);
                    set(piecg(1,4), 'FontUnits', 'pixels', 'FontSize' , 10);
                    set(piecg(1,6), 'FontUnits', 'pixels', 'FontSize' , 10);
        elseif Muestrapor(1,1) ~= 0 && Muestrapor(1,2) == 0 && Muestrapor(1,3) ~= 0
                    hotcake(1,1) = Muestrapor(1,1);
                    hotcake(1,2) = Muestrapor(1,3);
                    explode = [ 0 1 ];
                    piecg = pie(hotcake, explode);
                    hdlleg = legend('Gravel' , 'Fine',  -1);
                    set(piecg(1,2), 'FontUnits', 'pixels', 'FontSize' , 10);
                    set(piecg(1,4), 'FontUnits', 'pixels', 'FontSize' , 10);
        elseif Muestrapor(1,1) ~= 0 && Muestrapor(1,2) == 0 && Muestrapor(1,3) == 0
                    hotcake = Muestrapor(1,1) ;
                    piecg = pie(hotcake);
                    hdlleg = legend('Gravel' , -1);
                    set(piecg(1,2), 'FontUnits', 'pixels', 'FontSize' , 10);
        elseif Muestrapor(1,1) == 0 && Muestrapor(1,2) ~= 0  && Muestrapor(1,3) == 0
                    hotcake = Muestrapor(1,2) ;
                    piecg = pie(hotcake);
                    hdlleg = legend('Sand' , -1);
                    set(piecg(1,2), 'FontUnits', 'pixels', 'FontSize' , 10);
         elseif Muestrapor(1,1) == 0 && Muestrapor(1,2) == 0 && Muestrapor(1,3) ~= 0
                    hotcake = Muestrapor(1,3) ;
                    piecg = pie(hotcake);
                    hdlleg = legend('Fine' , -1);
                    set(piecg(1,2), 'FontUnits', 'pixels', 'FontSize' , 10);
        end
        set(hdlleg, 'Color' , [ 0.93 0.91 0.85 ], 'Box', 'on'); clc;

        result = figure(11);
        set(result, 'Units' , 'Pixels' , 'NumberTitle' , 'Off' , 'Resize' , 'on' , 'Color' , [0.93 0.91 0.85] , ...
                'Position' , [   10+hpan/3+hpan-686  35   hpan-690 (vpan/3)*2-50] , ...
                'name' , '8. Sieve Analysis - Results' );
         P4 = 12;
        uicontrol(result, 'Style' , 'Text' , 'String', 'The main results of sieve analysis were:' , 'Unit' , 'Pixels' , 'Position' , [0 418 300 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'HorizontalAlignment', 'left' );
        uicontrol(result, 'Style' , 'Text' , 'String', ['D_5 (mm) = ' , resulchar.d_5  ] , 'Unit' , 'Pixels' , 'Position' , [0 405 200 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['D_10 (mm) = ' , resulchar.d_10  ] , 'Unit' , 'Pixels' , 'Position' , [0 392 200 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['D_16 (mm) = ' , resulchar.d_16  ] , 'Unit' , 'Pixels' , 'Position' , [0 379 200 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['D_30 (mm) = ' , resulchar.d_30  ] , 'Unit' , 'Pixels' , 'Position' , [0 366 200 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['D_50 (mm) = ' , resulchar.d_50  ] , 'Unit' , 'Pixels' , 'Position' , [0 353 200 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'ForegroundColor' , [0.5 0 0 ] , 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['D_60 (mm) = ' , resulchar.d_60  ] , 'Unit' , 'Pixels' , 'Position' , [0 340 200 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['D_75 (mm) = ' , resulchar.d_75  ] , 'Unit' , 'Pixels' , 'Position' , [0 327 200 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['D_84 (mm) = ' , resulchar.d_84  ] , 'Unit' , 'Pixels' , 'Position' , [0 314 200 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85], 'FontWeight' , 'Bold', 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['D_95 (mm) = ' , resulchar.d_95  ] , 'Unit' , 'Pixels' , 'Position' , [0 301 200 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['Mean Grain Size  (mm) = ' , resulchar.MeanGS  ] , 'Unit' , 'Pixels' , 'Position' , [0 288 200 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'ForegroundColor' , [0.5 0 0 ] , 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['Standard Deviation ' , resulchar.SDe  ] , 'Unit' , 'Pixels' , 'Position' , [0 273 200 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['Skewness = ' , resulchar.S_ke  ] , 'Unit' , 'Pixels' , 'Position' , [0 260 200 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['Kurtosis = ' , resulchar.K_ur  ] , 'Unit' , 'Pixels' , 'Position' , [0 247 200 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['Coeficient of uniformity (Cc) = ' , resulchar.CC  ] , 'Unit' , 'Pixels' , 'Position' , [0 236 300 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['Coeficient of curvature (Cu) = ' , resulchar.Cu  ] , 'Unit' , 'Pixels' , 'Position' , [0 223 300 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['Standard Deviation Classification = ' , resulchar.SD  ] , 'Unit' , 'Pixels' , 'Position' , [0 210 300 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['Skewness Classification = ' , resulchar.Sk  ] , 'Unit' , 'Pixels' , 'Position' , [0 197 300 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['Kurtosis Classification = ' , resulchar.kurt  ] , 'Unit' , 'Pixels' , 'Position' , [0 184 300 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['ASTM Classification = ' , resulchar.ASTM  ] , 'Unit' , 'Pixels' , 'Position' , [0 169 300 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'ForegroundColor' , [0.5 0 0 ] , 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['% of Gravel = ' , resulchar.Grava  ] , 'Unit' , 'Pixels' , 'Position' , [0 158 300 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'ForegroundColor' , [0.5 0 0 ] , 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['% of Sand = ' , resulchar.arena  ] , 'Unit' , 'Pixels' , 'Position' , [0 145 300 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'ForegroundColor' , [0.5 0 0 ] , 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['% of Fine = ' , resulchar.fino  ] , 'Unit' , 'Pixels' , 'Position' , [0 132 300 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'ForegroundColor' , [0.5 0 0 ] , 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['Wentworth classification = ' , resulchar.Wentworth  ] , 'Unit' , 'Pixels' , 'Position' , [0 117 300 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'ForegroundColor' , [0.5 0 0 ] , 'HorizontalAlignment', 'left');
        uicontrol(result, 'Style' , 'Text' , 'String', ['USCS classification = ' , resulchar.SUCS  ] , 'Unit' , 'Pixels' , 'Position' , [0 103 300 P4] , ...
                                   'BackGroundColor' , [0.93 0.91 0.85] , 'FontWeight' , 'Bold', 'ForegroundColor' , [0.5 0 0 ] , 'HorizontalAlignment', 'left');


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function [lower, upper] = UpperBound3(S, g, df, B, f, Nr, NSim, NSSim, ...
    getpaths, payoff)
% method from Broadie for computing upper bounds, see Chapter 8

iVec = 1:NSSim;

v = g(:,end);   % start for backward induction
c = zeros(NSim,Nr-1);   % continuation value

% backward induction and regression from t_{Nr-1} up to t_1
for i = Nr-1:-1:1
        index = find(g(:,i) > 0); % all ITM paths
        s = S(index,i+1);         % values of S at given time point
        v = v * df(i+1);          % option value at t_i

        Acell = B(s);             % evaluate basis function in cell array B
        A = cell2mat(Acell{:,:}); % convert to matrix

        c(index,i) = A*f(:,i);                  % continuation value

        exercise = g(index,i) >= c(index,i);    % early exercise
        v(index(exercise)) = g(index(exercise),i);
end

lower = mean(v * df(1));    % final option value

% Computing the martingale numerically, martingale = pi

L = zeros(NSim,1); %erster Teil von delta_i vgl. Formel (6.6)
expectation = zeros(NSim,Nr); %zweiter Teil von delta_i vgl. Formel (6.6)
expectation(:,1) = lower * ones(NSim,1);

for i=1:1:Nr-1
    for j=1:NSim
        expectation(j,i+1) = subsimulation(S(j,i+1), df, B, NSSim, Nr-i,...
            f(:,i:end), iVec(1:NSSim), getpaths, payoff) * prod(df(1:i));
    end
end

pi = zeros(NSim,Nr+1); % stores the values of the constructed martingale

% first time step and last are different
i_exercise = g(:,1) >= c(:,1) & c(:,1) > 0; % exercise in this case

L(i_exercise) = g(i_exercise, 1) * prod(df(1:1));
L(~i_exercise) = expectation(~i_exercise,2);

pi(:,2) = pi(:,1) + L - expectation(:,1);

for i=2:1:Nr-1
    % exercise in this case if not already exercised
    i_exercise = g(:,i) >= c(:,i) & c(:,i) > 0 & ~i_exercise;

    L(i_exercise) = g(i_exercise, i) * prod(df(1:i));
    L(~i_exercise) = expectation(~i_exercise,i+1);

    pi(:,i+1) = pi(:,i) + L - expectation(:,i);
end
% finally exercise if in the money and not already exercised
i_exercise = g(:,Nr) > 0 & ~i_exercise;

pi(i_exercise,Nr+1) = pi(i_exercise,Nr) + g(i_exercise,Nr)* prod(df(1:Nr));
pi(~i_exercise,Nr+1) = pi(~i_exercise,Nr);

% upper bound using the martingale pi

maximum = zeros(NSim,1);

for j=1:1:NSim
    maximum(j) = max(g(j,:) - pi(j,2:end)); %vgl. Formel (6.5)
end
upper = mean(maximum);

end

function y = subsimulation(S0, df, B, NSim, Nr, beta, iVec, gp,payoff)
    S2 = gp(S0,NSim,Nr); S2 = S2(:,2:end);   % paths
    g2 = payoff(S2);                         % payoff

    exercise = Nr * ones(NSim,1);            % exercise per path
    for i=1:1:Nr-1
        i_nexercised = exercise == Nr;
        I_nexercise = iVec(i_nexercised);

        s = S2(i_nexercised,i);
        Acell = B(s);
        A = cell2mat(Acell{:,:});
        c = A * beta(:,i);

        i_exercise = g2(i_nexercised,i) >= c & g2(i_nexercised,i) > 0;
        exercise(I_nexercise(i_exercise)) = i;
    end
    summe=0;
    for j=1:1:NSim
        summe = summe + g2(j,exercise(j)) * prod(df(1:exercise(j)));
    end
    y = summe / NSim;                         % MC value from subsimulation
end


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtStkRadiation.m                             |
%|    #    |   VERSION    : 0.50                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 31.10.2018                                    |
%|  / 0 \  |   LAST MODIF : 25.11.2018                                    |
%| ( === ) |   SYNOPSIS   : Stoke radiation of a unit sphere using BEM    |
%|  `---'  |                with stokeslet G and stresslet T              |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Parameters
N  = 300;
n  = 100;
x0 = [0.1 0.2 0.3];

% Mesh unit sphere
mesh = mshSphere(N,1);

% Quadrature
gamma = dom(mesh,3);

% Finite element and unknowns
phi = fem(mesh,'P1');
unk = phi.unk;

% Radiation particles
mesh2 = mshSphere(n,2);
X     = mesh2.vtx;

% Graphical rep
figure
plot(mesh)
hold on
plotNrm(mesh)
plot(gamma)
plot(phi,'*r')
plot(msh(X))
alpha(0.5)
axis equal

% Initalization of block matrix for each coordinate (x,y,z)
G      = cell(3,3);
T      = cell(3,3);
mu     = cell(3,1);
lambda = cell(3,1);

% Loop for each coordinate
for i = 1:3
    for j = 1:3
        % Using r = x - y
        % Single layer : G = \int_gamma 1/(8pi) (\delta_ij/r + r_i*r_j/|r|^3)
        name   = ['[ij/r+rirj/r^3]',num2str(i),num2str(j)];
        green  = @(X,Y) 1/(8*pi) .* femGreenKernel(X,Y,name,[]);
        G{i,j} = integral(X,gamma,green,phi);

        % Double layer : T = \int_gamma -6/(8pi) (r_i*r_j*(r.n)/|r|^5)
        T{i,j} = 0;
        for k = 1:3
            name   = ['[rirjrk/r^5]',num2str(i),num2str(j),num2str(k)];
            green  = @(X,Y) -6/(8*pi) .* femGreenKernel(X,Y,name,[]);
            T{i,j} = T{i,j} + integral(X,gamma,green,ntimes(phi,k));
        end
    end

    % mu = [u] = u_int - u_ext = - Gi1 = - Gi1(x0,y)
    name  = ['[ij/r+rirj/r^3]',num2str(i),num2str(1)];
    mu{i} = -1/(8*pi) * femGreenKernel(x0,phi.unk,name,[]);

    % lambda = [sigma] = - Ti1, with nk = xk (because boundary is unit sphere)
    lambda{i} = 0;
    for k = 1:3
        name      = ['[rirjrk/r^5]',num2str(i),num2str(1),num2str(k)];
        lambda{i} = lambda{i} - (-6/(8*pi)) * femGreenKernel(x0,phi.unk,name,[]) .* unk(:,k);
    end
end

% Convert cells to full matrix
G      = cell2mat(G);
T      = cell2mat(T);
mu     = cell2mat(mu);
lambda = cell2mat(lambda);

% Stokes radiation in domain : ui(x) = - sum_j \int_gamma Gij(x,y) lambda_j dy ...
%              + sum_j \int_gamma Tij(x,y).n(y) mu_j dy
sol = -G*lambda + T*mu ;

% Analytic solution : Gi1
ref = cell(3,1);
for i = 1:3
    name   = ['[ij/r+rirj/r^3]',num2str(i),num2str(1)];
    ref{i} = 1/(8*pi) * femGreenKernel(X,x0,name,[]);
end
ref = cell2mat(ref);

% Relative error L2 and inf
norm(ref-sol)/norm(ref)
norm(ref-sol,'inf')/norm(ref,'inf')

% Graphical representation of solution
figure(2)
for i = 1:3
    subplot(1,3,i)
    ind = (i-1)*n + (1:n);
    plot(mesh2,sol(ind))
    axis equal
    colorbar
    title(['Component ',num2str(i)])
end

disp('~~> Michto gypsilab !')


function Derive_Equations()
%%%% Derive Equations - Five Link Biped Model %%%%
%
% This function derives the equations of motion, as well as some other useful
% equations (kinematics, contact forces, ...) for the five-link biped
% model.
%
% This version of the code includes a few more complicated features for
% dealing with difficult cost functions. In particular, it adds 10 slack
% variables to compute the abs(power) term in the cost function, and the
% primary control is the derivative of torque, rather than torque itself.
% This allows for regularization by the derivative of the input.
%
%
% Nomenclature:
%
% - There are five links, which will be numbered starting with "1" for the
% stance leg tibia, increasing as the links are father from the base joint,
% and ending with "5" for the swing leg tibia.
%   1 - stance leg tibia (lower leg)
%   2 - stance leg femur  (upper leg)
%   3 - torso
%   4 - swing leg femur
%   5 - swing leg tibia
%
% - This script uses absolute angles, which are represented with "q". All
% angles use positive convention, with the zero angle corresponding to a
% vertically aligned link configuration. [q] = [0] has the torso balanced
% upright, with both legs fully extended straight below it.
%
% - Derivatives with respect to time are notated by prepending a "d". For
% example the rate of change in an absolute angle is "dq" and angular
% acceleration would be "ddq"
%
% - Joint positions are given with "P", center of mass positions are "G"
%

clc; clear;
disp('Creating variables and derivatives...')

%%%% Absolute orientation (angle) of each link
q1 = sym('q1', 'real');
q2 = sym('q2','real');
q3 = sym('q3','real');
q4 = sym('q4','real');
q5 = sym('q5','real');

%%%% Absolute angular rate of each link
dq1 = sym('dq1','real');
dq2 = sym('dq2','real');
dq3 = sym('dq3','real');
dq4 = sym('dq4','real');
dq5 = sym('dq5','real');

%%%% Absolute angular acceleration of each linke
ddq1 = sym('ddq1','real');
ddq2 = sym('ddq2','real');
ddq3 = sym('ddq3','real');
ddq4 = sym('ddq4','real');
ddq5 = sym('ddq5','real');

%%%% Torques at each joint
u1 = sym('u1','real');  %Stance foot
u2 = sym('u2','real');   %Stance knee
u3 = sym('u3','real');   %Stance hip
u4 = sym('u4','real');   %Swing hip
u5 = sym('u5','real');   %Swing knee

%%%% Torques rate at each joint
du1 = sym('du1','real');  %Stance foot
du2 = sym('du2','real');   %Stance knee
du3 = sym('du3','real');   %Stance hip
du4 = sym('du4','real');   %Swing hip
du5 = sym('du5','real');   %Swing knee

%%%% Slack variables -- negative component of power
sn1 = sym('sn1','real');  %Stance foot
sn2 = sym('sn2','real');   %Stance knee
sn3 = sym('sn3','real');   %Stance hip
sn4 = sym('sn4','real');   %Swing hip
sn5 = sym('sn5','real');   %Swing knee

%%%% Slack variables -- positive component of power
sp1 = sym('sp1','real');  %Stance foot
sp2 = sym('sp2','real');   %Stance knee
sp3 = sym('sp3','real');   %Stance hip
sp4 = sym('sp4','real');   %Swing hip
sp5 = sym('sp5','real');   %Swing knee

%%%% Mass of each link
m1 = sym('m1','real');
m2 = sym('m2','real');
m3 = sym('m3','real');
m4 = sym('m4','real');
m5 = sym('m5','real');

%%%% Distance between parent joint and link center of mass
c1 = sym('c1','real');
c2 = sym('c2','real');
c3 = sym('c3','real');
c4 = sym('c4','real');
c5 = sym('c5','real');

%%%% Length of each link
l1 = sym('l1','real');
l2 = sym('l2','real');
l3 = sym('l3','real');
l4 = sym('l4','real');
l5 = sym('l5','real');

%%%% Moment of inertia of each link about its own center of mass
I1 = sym('I1','real');
I2 = sym('I2','real');
I3 = sym('I3','real');
I4 = sym('I4','real');
I5 = sym('I5','real');

g = sym('g','real'); % Gravity
Fx = sym('Fx','real');   %Horizontal contact force at stance foot
Fy = sym('Fy','real');   %Vertical contact force at stance foot
empty = sym('empty','real');   %Used for vectorization, user should pass a vector of zeros
t = sym('t','real');  %dummy continuous time

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                Set up coordinate system and unit vectors                %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

i = sym([1;0]);   %Horizontal axis
j = sym([0;1]);   %Vertical axis

e1 = cos(q1)*(j) + sin(q1)*(-i);  %unit vector from P0 -> P1, (contact point to stance knee)
e2 = cos(q2)*(j) + sin(q2)*(-i);  %unit vector from P1 -> P2, (stance knee to hip)
e3 = cos(q3)*(j) + sin(q3)*(-i);  %unit vector from P2 -> P3, (hip to shoulders);
e4 = -cos(q4)*(j) - sin(q4)*(-i);  %unit vector from P2 -> P4, (hip to swing knee);
e5 = -cos(q5)*(j) - sin(q5)*(-i);  %unit vector from P4 -> P5, (swing knee to swing foot);

P0 = 0*i + 0*j;   %stance foot = Contact point = origin
P1 = P0 + l1*e1;  %stance knee
P2 = P1 + l2*e2;  %hip
P3 = P2 + l3*e3;  %shoulders
P4 = P2 + l4*e4;  %swing knee
P5 = P4 + l5*e5;  %swing foot

G1 = P1 - c1*e1;  % CoM stance leg tibia
G2 = P2 - c2*e2;  % CoM stance leg febur
G3 = P3 - c3*e3;  % CoM torso
G4 = P2 + c4*e4;  % CoM swing leg femur
G5 = P4 + c5*e5;  % CoM swing leg tibia
G = (m1*G1 + m2*G2 + m3*G3 + m4*G4 + m5*G5)/(m1+m2+m3+m4+m5);  %Center of mass for entire robot

%%%% Define a function for doing '2d' cross product: dot(a x b, k)
cross2d = @(a,b)(a(1)*b(2) - a(2)*b(1));

%%%% Weight of each link:
w1 = -m1*g*j;
w2 = -m2*g*j;
w3 = -m3*g*j;
w4 = -m4*g*j;
w5 = -m5*g*j;

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                             Derivatives                                 %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

q = [q1;q2;q3;q4;q5];
dq = [dq1;dq2;dq3;dq4;dq5];
ddq = [ddq1;ddq2;ddq3;ddq4;ddq5];
u = [u1;u2;u3;u4;u5];
du = [du1;du2;du3;du4;du5];
sn = [sn1;sn2;sn3;sn4;sn5];
sp = [sp1;sp2;sp3;sp4;sp5];
z = [t;q;dq;u;du;sn;sp];   % time-varying vector of inputs

% Neat trick to compute derivatives using the chain rule
derivative = @(in)( jacobian(in,[q;dq;u])*[dq;ddq;du] );

% Velocity of the swing foot (used for step constraints)
dP5 = derivative(P5);

% Compute derivatives for the CoM of each link:
dG1 = derivative(G1);  ddG1 = derivative(dG1);
dG2 = derivative(G2);  ddG2 = derivative(dG2);
dG3 = derivative(G3);  ddG3 = derivative(dG3);
dG4 = derivative(G4);  ddG4 = derivative(dG4);
dG5 = derivative(G5);  ddG5 = derivative(dG5);
dG = derivative(G);  ddG = derivative(dG);

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                             Calculations:                               %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

singleStanceDynamics();
objectiveFunctions();
heelStrikeDynamics();

mechanicalEnergy();
contactForces();
kinematics();

disp('Done!');

%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                      Single-Stance Dynamics                             %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

% I solve the dynamics here by carefully selecting angular momentum balance
% equations about each joint, working my way out the kinematic tree from
% the root.

    function singleStanceDynamics()
        disp('Deriving single stance dynamics...')

        %%%% AMB - entire system @ P0
        eqnTorque0 = ...
            cross2d(G1-P0,w1) + ...
            cross2d(G2-P0,w2) + ...
            cross2d(G3-P0,w3) + ...
            cross2d(G4-P0,w4) + ...
            cross2d(G5-P0,w5) + ...
            u1;

        eqnInertia0 = ...
            cross2d(G1-P0,m1*ddG1) + ddq1*I1 + ...
            cross2d(G2-P0,m2*ddG2) + ddq2*I2 + ...
            cross2d(G3-P0,m3*ddG3) + ddq3*I3 + ...
            cross2d(G4-P0,m4*ddG4) + ddq4*I4 + ...
            cross2d(G5-P0,m5*ddG5) + ddq5*I5;

        %%%% AMB - swing leg, torso, stance femer  @ stance knee
        eqnTorque1 = ...
            cross2d(G2-P1,w2) + ...
            cross2d(G3-P1,w3) + ...
            cross2d(G4-P1,w4) + ...
            cross2d(G5-P1,w5) + ...
            u2;

        eqnInertia1 = ...
            cross2d(G2-P1,m2*ddG2) + ddq2*I2  + ...
            cross2d(G3-P1,m3*ddG3) + ddq3*I3  + ...
            cross2d(G4-P1,m4*ddG4) + ddq4*I4  + ...
            cross2d(G5-P1,m5*ddG5) + ddq5*I5 ;

        %%%% AMB - swing leg, torso @ hip
        eqnTorque2 = ...
            cross2d(G3-P2,w3) + ...
            cross2d(G4-P2,w4) + ...
            cross2d(G5-P2,w5) + ...
            u3;

        eqnInertia2 = ...
            cross2d(G3-P2,m3*ddG3) + ddq3*I3  + ...
            cross2d(G4-P2,m4*ddG4) + ddq4*I4  + ...
            cross2d(G5-P2,m5*ddG5) + ddq5*I5 ;

        %%%% AMB - swing leg @ hip
        eqnTorque3 = ...
            cross2d(G4-P2,w4) + ...
            cross2d(G5-P2,w5) + ...
            u4;

        eqnInertia3 = ...
            cross2d(G4-P2,m4*ddG4) + ddq4*I4  + ...
            cross2d(G5-P2,m5*ddG5) + ddq5*I5 ;

        %%%% AMB - swing tibia % swing knee
        eqnTorque4 = ...
            cross2d(G5-P4,w5) + ...
            u5;

        eqnInertia4 = ...
            cross2d(G5-P4,m5*ddG5) + ddq5*I5 ;

        %%%% Collect and solve equations:
        eqns = [...
            eqnTorque0 - eqnInertia0;
            eqnTorque1 - eqnInertia1;
            eqnTorque2 - eqnInertia2;
            eqnTorque3 - eqnInertia3;
            eqnTorque4 - eqnInertia4];

        [MM, FF] = equationsToMatrix(eqns,ddq);  % ddq = MM\ff;

        %%%% Compute gradients:
        [m, mi, mz, mzi, mzd] = computeGradients(MM,z,empty);
        [f, fi, fz, fzi, fzd] = computeGradients(FF,z,empty);

        % Write function file:
        matlabFunction(m, mi, f, fi,...   %dynamics
            mz, mzi, mzd, fz, fzi, fzd,...  %gradients
            'file','autoGen_dynSs.m',...
            'vars',{...
            'q1','q2','q3','q4','q5',...
            'dq1','dq2','dq3','dq4','dq5',...
            'u1','u2','u3','u4','u5',...
            'm1','m2','m3','m4','m5',...
            'I1','I2','I3','I4','I5',...
            'l1','l2','l3','l4',...
            'c1','c2','c3','c4','c5',...
            'g','empty'});

    end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                        Objective Functions                              %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

    function objectiveFunctions()

        % Joint rates:
        v1 = dq1;   % joint rate 1
        v2 = dq2-dq1;   % joint rate 2
        v3 = dq3-dq2; % joint rate 3
        v4 = dq4-dq3;  % joint rate 4
        v5 = dq5-dq4;  % joint rate 5

        % Compute the power used by each joint
        pow1 = v1*u1;  %Power used by joint 1
        pow2 = v2*u2;  %Power used by joint 2
        pow3 = v3*u3;  %Power used by joint 3
        pow4 = v4*u4;  %Power used by joint 4
        pow5 = v5*u5;  %Power used by joint 5

        % Constraint on the slack variables:
        slackCst = [...
            pow1 - (sp1 - sn1);
            pow2 - (sp2 - sn2);
            pow3 - (sp3 - sn3);
            pow4 - (sp4 - sn4);
            pow5 - (sp5 - sn5)];

        % Gradients of the constraint on slack variables:
        [c, ~, cz, czi, ~] = computeGradients(slackCst,z,empty);

        matlabFunction(c,cz,czi,...
            'file','autoGen_cst_costOfTransport.m',...
            'vars',{...
            'dq1','dq2','dq3','dq4','dq5',...
            'u1','u2','u3','u4','u5',...
            'sn1','sn2','sn3','sn4','sn5',...
            'sp1','sp2','sp3','sp4','sp5','empty'});

        % abs(power) using slack variables:
        gammaNeg = sym('gammaNeg','real');  %Torque-squared smoothing parameter
        gammaPos = sym('gammaPos','real');  %Torque-squared smoothing parameter
        absPower = gammaNeg*(sn1 + sn2 + sn3 + sn4 + sn5) + ...
            gammaPos*(sp1 + sp2 + sp3 + sp4 + sp5);

        % Cost of Transport:
        weight = (m1+m2+m3+m4+m5)*g;
        stepLength = sym('stepLength','real');
        alpha = sym('alpha','real');  %Torque-squared smoothing parameter
        beta = sym('beta','real');  %Torque-rate squared smoothing
        F = absPower/(weight*stepLength) + ...
            alpha*(u1^2 + u2^2 + u3^2 + u4^2 + u5^2) + ...
            beta*(du1^2 + du2^2 + du3^2 + du4^2 + du5^2);
        [f, ~, fz, fzi, ~]  = computeGradients(F,z,empty);

        matlabFunction(f,fz,fzi,...
            'file','autoGen_obj_costOfTransport.m',...
            'vars',{...
            'm1','m2','m3','m4','m5',...
            'u1','u2','u3','u4','u5',...
            'du1','du2','du3','du4','du5',...
            'sn1','sn2','sn3','sn4','sn5', ...
            'sp1','sp2','sp3','sp4','sp5',...
            'g','stepLength','gammaNeg','gammaPos','alpha','beta','empty'});

        % Swing foot height:
        stepHeight = sym('stepHeight','real');
        yFoot = P5(2);
        xFoot = P5(1);
        yMin = stepHeight*(1 - (xFoot/stepLength)^2);
        yCst = yMin - yFoot;  %Must be negative
        [y, ~, yz, yzi, ~]  = computeGradients(yCst,z,empty);
        matlabFunction(y,yz,yzi,...
                    'file','autoGen_cst_swingFootHeight.m',...
            'vars',{...
            'q1','q2','q4','q5',...
            'l1','l2','l4','l5'...
            'stepLength','stepHeight'});
    end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                        Heel-Strike Dynamics                             %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

    function heelStrikeDynamics()
        disp('Deriving heel-strike dynamics...')

        %%%% Notes:
        % xF - heelStrike(xI) --> constraint --> 0
        % xF - collision(footSwap(xI));
        %

        % Angles before heel-strike:
        q1m = sym('q1m','real');
        q2m = sym('q2m','real');
        q3m = sym('q3m','real');
        q4m = sym('q4m','real');
        q5m = sym('q5m','real');
        qm = [q1m;q2m;q3m;q4m;q5m];

        % Angles after heel-strike
        q1p = sym('q1p','real');
        q2p = sym('q2p','real');
        q3p = sym('q3p','real');
        q4p = sym('q4p','real');
        q5p = sym('q5p','real');
        qp = [q1p;q2p;q3p;q4p;q5p];

        % Angular rates before heel-strike:
        dq1m = sym('dq1m','real');
        dq2m = sym('dq2m','real');
        dq3m = sym('dq3m','real');
        dq4m = sym('dq4m','real');
        dq5m = sym('dq5m','real');
        dqm = [dq1m;dq2m;dq3m;dq4m;dq5m];

        % Angular rates after heel-strike
        dq1p = sym('dq1p','real');
        dq2p = sym('dq2p','real');
        dq3p = sym('dq3p','real');
        dq4p = sym('dq4p','real');
        dq5p = sym('dq5p','real');
        dqp = [dq1p;dq2p;dq3p;dq4p;dq5p];

        % torque before heel-strike:
        u1m = sym('u1m','real');
        u2m = sym('u2m','real');
        u3m = sym('u3m','real');
        u4m = sym('u4m','real');
        u5m = sym('u5m','real');
        um = [u1m;u2m;u3m;u4m;u5m];

        % torque after heel-strike
        u1p = sym('u1p','real');
        u2p = sym('u2p','real');
        u3p = sym('u3p','real');
        u4p = sym('u4p','real');
        u5p = sym('u5p','real');
        up = [u1p;u2p;u3p;u4p;u5p];

        % Compute kinematics before heel-strike:
        inVars = {'q1','q2','q3','q4','q5','dq1','dq2','dq3','dq4','dq5'};
        outVarsM = {'q1m','q2m','q3m','q4m','q5m','dq1m','dq2m','dq3m','dq4m','dq5m'};
        %         P0m = subs(P0,inVars,outVarsM);
        P1m = subs(P1,inVars,outVarsM);
        P2m = subs(P2,inVars,outVarsM);
        %         P3m = subs(P3,inVars,outVarsM);
        P4m = subs(P4,inVars,outVarsM);
        P5m = subs(P5,inVars,outVarsM);
        dP5m = subs(dP5,inVars,outVarsM);
        G1m = subs(G1,inVars,outVarsM);
        G2m = subs(G2,inVars,outVarsM);
        G3m = subs(G3,inVars,outVarsM);
        G4m = subs(G4,inVars,outVarsM);
        G5m = subs(G5,inVars,outVarsM);
        dG1m = subs(dG1,inVars,outVarsM);
        dG2m = subs(dG2,inVars,outVarsM);
        dG3m = subs(dG3,inVars,outVarsM);
        dG4m = subs(dG4,inVars,outVarsM);
        dG5m = subs(dG5,inVars,outVarsM);

        % Compute kinematics after heel-strike:
        outVarsP = {'q1p','q2p','q3p','q4p','q5p','dq1p','dq2p','dq3p','dq4p','dq5p'};
        P0p = subs(P0,inVars,outVarsP);
        P1p = subs(P1,inVars,outVarsP);
        P2p = subs(P2,inVars,outVarsP);
        %         P3p = subs(P3,inVars,outVarsP);
        P4p = subs(P4,inVars,outVarsP);
        %         P5p = subs(P5,inVars,outVarsP);
        dP5p = subs(dP5,inVars,outVarsP);
        G1p = subs(G1,inVars,outVarsP);
        G2p = subs(G2,inVars,outVarsP);
        G3p = subs(G3,inVars,outVarsP);
        G4p = subs(G4,inVars,outVarsP);
        G5p = subs(G5,inVars,outVarsP);
        dG1p = subs(dG1,inVars,outVarsP);
        dG2p = subs(dG2,inVars,outVarsP);
        dG3p = subs(dG3,inVars,outVarsP);
        dG4p = subs(dG4,inVars,outVarsP);
        dG5p = subs(dG5,inVars,outVarsP);

        %%%% AMB - entire system @ New stance foot
        eqnHs0m = ...   %Before collision
            cross2d(G1m-P5m,m1*dG1m) + dq1m*I1 + ...
            cross2d(G2m-P5m,m2*dG2m) + dq2m*I2 + ...
            cross2d(G3m-P5m,m3*dG3m) + dq3m*I3 + ...
            cross2d(G4m-P5m,m4*dG4m) + dq4m*I4 + ...
            cross2d(G5m-P5m,m5*dG5m) + dq5m*I5;
        eqnHs0 = ...   %After collision
            cross2d(G1p-P0p,m1*dG1p) + dq1p*I1 + ...
            cross2d(G2p-P0p,m2*dG2p) + dq2p*I2 + ...
            cross2d(G3p-P0p,m3*dG3p) + dq3p*I3 + ...
            cross2d(G4p-P0p,m4*dG4p) + dq4p*I4 + ...
            cross2d(G5p-P0p,m5*dG5p) + dq5p*I5;

        %%%% AMB - new swing leg, torso, stance femer  @  stance knee
        eqnHs1m = ...   %Before collision
            cross2d(G1m-P4m,m1*dG1m) + dq1m*I1 + ...
            cross2d(G2m-P4m,m2*dG2m) + dq2m*I2 + ...
            cross2d(G3m-P4m,m3*dG3m) + dq3m*I3 + ...
            cross2d(G4m-P4m,m4*dG4m) + dq4m*I4;
        eqnHs1 = ...   %After collision
            cross2d(G2p-P1p,m2*dG2p) + dq2p*I2 + ...
            cross2d(G3p-P1p,m3*dG3p) + dq3p*I3 + ...
            cross2d(G4p-P1p,m4*dG4p) + dq4p*I4 + ...
            cross2d(G5p-P1p,m5*dG5p) + dq5p*I5;

        %%%% AMB - swing leg, torso  @ new hip
        eqnHs2m = ...   %Before collision
            cross2d(G3m-P2m,m3*dG3m) + dq3m*I3 + ...
            cross2d(G2m-P2m,m2*dG2m) + dq2m*I2 + ...
            cross2d(G1m-P2m,m1*dG1m) + dq1m*I1;
        eqnHs2 = ...   %After collision
            cross2d(G3p-P2p,m3*dG3p) + dq3p*I3 + ...
            cross2d(G4p-P2p,m4*dG4p) + dq4p*I4 + ...
            cross2d(G5p-P2p,m5*dG5p) + dq5p*I5;

        %%%% AMB - swing leg @ new hip
        eqnHs3m = ...   %Before collision
            cross2d(G1m-P2m,m1*dG1m) + dq1m*I1 + ...
            cross2d(G2m-P2m,m2*dG2m) + dq2m*I2;
        eqnHs3 = ...   %After collision
            cross2d(G4p-P2p,m4*dG4p) + dq4p*I4 + ...
            cross2d(G5p-P2p,m5*dG5p) + dq5p*I5;

        %%%% AMB - swing tibia @ new swing knee
        eqnHs4m = ...   %Before collision
            cross2d(G1m-P1m,m1*dG1m) + dq1m*I1;
        eqnHs4 = ...   %After collision
            cross2d(G5p-P4p,m5*dG5p) + dq5p*I5;

        %%%% Collect and solve equations:
        eqnHs = [...
            eqnHs0m - eqnHs0;
            eqnHs1m - eqnHs1;
            eqnHs2m - eqnHs2;
            eqnHs3m - eqnHs3;
            eqnHs4m - eqnHs4];
        [MM, FF] = equationsToMatrix(eqnHs,dqp);

        %%%% Compute gradients:
        tp = sym('tp','real');   %Initial trajectory time
        tm = sym('tm','real');   %Final trajectory time
        zBnd = [tp;qp;dqp;up;tm;qm;dqm;um];
        [m, mi, mz, mzi, mzd] = computeGradients(MM,zBnd,empty);
        [f, fi, fz, fzi, fzd] = computeGradients(FF,zBnd,empty);

        % Heel-strike
        matlabFunction(m, mi, f, fi,...   %dynamics
            mz, mzi, mzd, fz, fzi, fzd,...  %gradients
            'file','autoGen_cst_heelStrike.m',...
            'vars',{...
            'q1p','q2p','q3p','q4p','q5p',...
            'q1m','q2m','q3m','q4m','q5m',...
            'dq1m','dq2m','dq3m','dq4m','dq5m',...
            'm1','m2','m3','m4','m5',...
            'I1','I2','I3','I4','I5',...
            'l1','l2','l3','l4','l5',...
            'c1','c2','c3','c4','c5','empty'});

        % Collision velocity of the swing foot:
        cst = [-dP5p(2); dP5m(2)];  %Swing foot velocity before and after collision (negative sign is intentional, since output is constrained to be negative);
        cstJac = jacobian(cst,zBnd);  %Gradient
        matlabFunction(cst, cstJac,...
            'file','autoGen_cst_footVel.m',...
            'vars',{...
            'q1p','q2p','q4p','q5p',...
            'q1m','q2m','q4m','q5m',...
            'dq1p','dq2p','dq4p','dq5p',...
            'dq1m','dq2m','dq4m','dq5m',...
            'l1','l2','l4','l5'});

        % Step length and height constraint:
        stepLength = sym('stepLength','real');
        ceq = [P5m(1)-stepLength; P5m(2)];
        ceqJac = jacobian(ceq,zBnd);  %Gradient
        matlabFunction(ceq, ceqJac,...
            'file','autoGen_cst_steplength.m',...
            'vars',{...
            'q1m','q2m','q4m','q5m',...
            'l1','l2','l4','l5','stepLength'});
    end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                         Mechanical Energy                               %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

    function mechanicalEnergy()
        disp('Deriving mechanical energy...')

        %%%% Energy:
        KineticEnergy = ...
            0.5*m1*dot(dG1,dG1) + 0.5*I1*dq1^2 + ...
            0.5*m2*dot(dG2,dG2) + 0.5*I2*dq2^2 + ...
            0.5*m3*dot(dG3,dG3) + 0.5*I3*dq3^2 + ...
            0.5*m4*dot(dG4,dG4) + 0.5*I4*dq4^2 + ...
            0.5*m5*dot(dG5,dG5) + 0.5*I5*dq5^2;
        PotentialEnergy = ...
            m1*g*G1(2) + ...
            m2*g*G2(2) + ...
            m3*g*G3(2) + ...
            m4*g*G4(2) + ...
            m5*g*G5(2);

        matlabFunction(KineticEnergy, PotentialEnergy,...
            'file','autoGen_energy.m',...
            'vars',{...
            'q1','q2','q3','q4','q5',...
            'dq1','dq2','dq3','dq4','dq5',...
            'm1','m2','m3','m4','m5',...
            'I1','I2','I3','I4','I5',...
            'l1','l2','l3','l4',...
            'c1','c2','c3','c4','c5',...
            'g'},...
            'outputs',{'KE','PE'});

    end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                          Contact Forces                                 %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

    function contactForces()

        %%%% Contact Forces:
        eqnForce5 = w1 + w2 + w3 + w4 + w5 + Fx*i + Fy*j;
        eqnInertia5 = (m1+m2+m3+m4+m5)*ddG;
        [AA,bb] = equationsToMatrix(eqnForce5-eqnInertia5,[Fx;Fy]);
        ContactForces = AA\bb;

        matlabFunction(ContactForces(1),ContactForces(2),...
            'file','autoGen_contactForce.m',...
            'vars',{...
            'q1','q2','q3','q4','q5',...
            'dq1','dq2','dq3','dq4','dq5',...
            'ddq1','ddq2','ddq3','ddq4','ddq5',...
            'm1','m2','m3','m4','m5',...
            'l1','l2','l3','l4',...
            'c1','c2','c3','c4','c5',...
            'g'},...
            'outputs',{'Fx','Fy'});

    end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                        Write Kinematics Files                           %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

    function kinematics()
        disp('Writing kinematics files...')

        P = [P1; P2; P3; P4; P5];
        Gvec = [G1; G2; G3; G4; G5];

        % Used for plotting and animation
        matlabFunction(P,Gvec,'file','autoGen_getPoints.m',...
            'vars',{...
            'q1','q2','q3','q4','q5',...
            'l1','l2','l3','l4','l5',...
            'c1','c2','c3','c4','c5'},...
            'outputs',{'P','Gvec'});

    end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                       Helper Functions                                  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [m, mi, mz, mzi, dim] = computeGradients(M,z,empty)
%
% This function computes the gradients of a matrix M with respect the the
% variables in z, and then returns both the matrix and its gradient as
% column vectors of their non-zero elements, along with the linear indicies
% to unpack them. It also simplifies m and mz.
%
% INPUTS:
%   M = [na, nb] = symbolic matrix
%   z = [nc, 1] = symbolic vector
%
% OUTPUTS:
%   m = [nd, 1] = symbolic vector of non-zero elements in M
%   mi = [nd, 1] = linear indicies to map m --> [na,nb] matrix
%   mz = [ne, 1] = symbolic vector of non-zero elements in Mz
%   mzi = [ne, 1] = linear indicies to map mz --> [na,nb,nc] array
%   dim = [3,1] = [na,nb,nc] = dimensions of 3d version of mz
%

[na, nb] = size(M);
nc = size(z,1);
M = simplify(M);

mz2 = jacobian(M(:),z);  %Compute jacobian of M, by first reshaping M to be a column vector
mz3 = reshape(mz2,na,nb,nc); %Expand back out to a three-dimensional array
mz3 = simplify(mz3);

% Extract non-zero elements to a column vector:
mi = find(M);
m = M(mi);
mzi = find(mz3);
mz = mz3(mzi); mz = mz(:);  %Collapse to a column vector
dim = [na,nb,nc];

% Pad any constant terms with "empty" to permit vectorization:
m = vectorizeHack(m, z, empty);
mz = vectorizeHack(mz, z, empty);

end

function x = vectorizeHack(x, z, empty)
%
% This function searches for any elements of x that are not dependent on
% any element of z. In this case, the automatically generated code will
% fail to vectorize properly. One solution is to add an array of zeros
% (empty) to the element.
%
% x = column vector of symbolic expressions
% z = column vector of symbolic variables
% z = symbolic variable, which the user will set equal to zero.
%

% Compute dependencies
g = jacobian(x,z);

% Check for rows of x with no dependence on z
[n,m] = size(g);
idxConst = true(n,1);
for i=1:n
    for j=1:m
        if ~isequal(sym(0),g(i,j))
            idxConst(i) = false;
            break;
        end
    end
end

% Add empty to those enteries
x(idxConst) = x(idxConst) + empty;

end


% SCAT_ENERGY Calculate scattering energy
%
% Usage
%    energy = scat_energy(S, U)
%
% Input
%    S (cell): The scattering transform.
%    U (cell): The wavelet modulus coefficients (optional).
%
% Output
%    energy (numeric): The energy of the scattering transform (the sum of the
%       squares of the coefficients). If both S and U are given, the energy of
%       both are computed and summed.

function energy = scat_energy(S, U)
	if nargin < 2
		U = [];
	end

	if ~isempty(U)
		energy = scat_energy(S) + scat_energy(U);
	else
		if iscell(S)
			energy = 0;
			for m = 1:numel(S)
				energy = energy + scat_energy(S{m});
			end
		else
			energy = 0;
			for p = 1:numel(S.signal)
				sig = S.signal{p};
				% TODO: fix so that multiple signals work!!
				%sz = size(sig);
				%sig = reshape(sig,[prod(sz(1:end-1)) sz(end)]);
				energy = energy + sum(abs(sig(:)).^2,1);
			end
		end
	end
end


%
% Usage:   [V [val_regularizer]]=mexProximalGraph(U,graph,param);
%
% Name: mexProximalGraph
%
% Description: mexProximalGraph computes a proximal operator. Depending
%         on the value of param.regul, it computes
%
%         Given an input matrix U=[u^1,\ldots,u^n], and a set of groups G,
%         it computes a matrix V=[v^1,\ldots,v^n] such that
%
%         if param.regul='graph'
%         for every column u of U, it computes a column v of V solving
%             argmin 0.5||u-v||_2^2 + lambda\sum_{g \in G} \eta_g||v_g||_inf
%
%         if param.regul='graph+ridge'
%         for every column u of U, it computes a column v of V solving
%             argmin 0.5||u-v||_2^2 + lambda\sum_{g \in G} \eta_g||v_g||_inf + lambda_2||v||_2^2
%
%
%         if param.regul='multi-task-graph'
%            V=argmin 0.5||U-V||_F^2 + lambda \sum_{i=1}^n\sum_{g \in G} \eta_g||v^i_g||_inf + ...
%                                                lambda_2 \sum_{g \in G} \eta_g max_{j in g}||V_j||_{inf}
%
%         it can also be used with any regularization addressed by mexProximalFlat
%
%         for all these regularizations, it is possible to enforce non-negativity constraints
%         with the option param.pos, and to prevent the last row of U to be regularized, with
%         the option param.intercept
%
% Inputs: U:  double p x n matrix   (input signals)
%               m is the signal size
%         graph: struct
%               with three fields, eta_g, groups, and groups_var
%
%               The first fields sets the weights for every group
%                  graph.eta_g            double N vector
%
%               The next field sets inclusion relations between groups
%               (but not between groups and variables):
%                  graph.groups           sparse (double or boolean) N x N matrix
%                  the (i,j) entry is non-zero if and only if i is different than j and
%                  gi is included in gj.
%
%               The next field sets inclusion relations between groups and variables
%                  graph.groups_var       sparse (double or boolean) p x N matrix
%                  the (i,j) entry is non-zero if and only if the variable i is included
%                  in gj, but not in any children of gj.
%
%               examples are given in test_ProximalGraph.m
%
%         param: struct
%               param.lambda  (regularization parameter)
%               param.regul (choice of regularization, see above)
%               param.lambda2  (optional, regularization parameter)
%               param.lambda3  (optional, regularization parameter)
%               param.verbose (optional, verbosity level, false by default)
%               param.intercept (optional, last row of U is not regularized,
%                 false by default)
%               param.pos (optional, adds positivity constraints on the
%                 coefficients, false by default)
%               param.numThreads (optional, number of threads for exploiting
%                 multi-core / multi-cpus. By default, it takes the value -1,
%                 which automatically selects all the available CPUs/cores).
%
% Output: V: double p x n matrix (output coefficients)
%         val_regularizer: double 1 x n vector (value of the regularization
%         term at the optimum).
%
% Author: Julien Mairal, 2010


function [Peg, Peh] = getPe(K, Ec, No, N, pulse);

[mew, sigma] = get_mean_var_G(K, N, Ec, pulse.m1);
Ig = sqrt(N*No/2 + mew);
Th = N*sqrt(Ec);       %Non- Coherent
Peg = qfunc(Th/Ig);
[mew, sigma] = get_mean_var_H(K, N, Ec, pulse.m1, pulse.m2, pulse.w1, pulse.w2);
I1 = sqrt(No*N/2 + mew);
I2 = sqrt(No*N/2 + (mew + sqrt(3) * sigma));
I3 = sqrt(No*N/2 + (mew - sqrt(3) * sigma));
Peh = 2/3*qfunc(Th/I1) + 1/6*qfunc(Th/I2) + 1/6*qfunc(Th/I3);

function [mew_MAI, sigma_MAI] = get_mean_var_H(K, N, P, m1, m2, w1, w2)
T =1;
mew_MAI = (N*P * (K-1)*m1);
sigma_MAI = (N*P) * ((K-1)*( (.375*w1 - m1^2) + ((N-1)/(N^2))*((1.5*w2) + (K-2)*(m2^2) )))^(.5);

function [mew_MAI, sigma_MAI] = get_mean_var_G(K, N, P, m1)
T =1;
mew_MAI = (N*P * (K-1)*m1);
sigma_MAI = 0;


%TR2Q	Convert homogeneous transform to a unit-quaternion
%
%	Q = tr2q(T)
%
%	Return a unit quaternion corresponding to the rotational part of the
%	homogeneous transform T.
%
%	See also Q2TR

%	Copyright (C) 1993 Peter Corke
function q = tr2q(t)
	q = zeros(1,4);
	q(1) = sqrt(trace(t))/2;
	kx = t(3,2) - t(2,3);	% Oz - Ay
	ky = t(1,3) - t(3,1);	% Ax - Nz
	kz = t(2,1) - t(1,2);	% Ny - Ox

	if (t(1,1) >= t(2,2)) & (t(1,1) >= t(3,3))
		kx1 = t(1,1) - t(2,2) - t(3,3) + 1;	% Nx - Oy - Az + 1
		ky1 = t(2,1) + t(1,2);			% Ny + Ox
		kz1 = t(3,1) + t(1,3);			% Nz + Ax
		add = (kx >= 0);
	elseif (t(2,2) >= t(3,3))
		kx1 = t(2,1) + t(1,2);			% Ny + Ox
		ky1 = t(2,2) - t(1,1) - t(3,3) + 1;	% Oy - Nx - Az + 1
		kz1 = t(3,2) + t(2,3);			% Oz + Ay
		add = (ky >= 0);
	else
		kx1 = t(3,1) + t(1,3);			% Nz + Ax
		ky1 = t(3,2) + t(2,3);			% Oz + Ay
		kz1 = t(3,3) - t(1,1) - t(2,2) + 1;	% Az - Nx - Oy + 1
		add = (kz >= 0);
	end

	if add
		kx = kx + kx1;
		ky = ky + ky1;
		kz = kz + kz1;
	else
		kx = kx - kx1;
		ky = ky - ky1;
		kz = kz - kz1;
	end
	nm = norm([kx ky kz]);
	if nm == 0,
		q = [1 0 0 0];
	else
		s = sqrt(1 - q(1)^2) / nm;
		q(2:4) = s*[kx ky kz];
	end


function [ ap, rcond, z, info ] = sppco ( ap, n )

%*****************************************************************************80
%
%% SPPCO factors a real symmetric positive definite matrix in packed form.
%
%  Discussion:
%
%    SPPCO also estimates the condition of the matrix.
%
%    If RCOND is not needed, SPPFA is slightly faster.
%
%    To solve A*X = B, follow SPPCO by SPPSL.
%
%    To compute inverse(A)*C, follow SPPCO by SPPSL.
%
%    To compute determinant(A), follow SPPCO by SPPDI.
%
%    To compute inverse(A), follow SPPCO by SPPDI.
%
%  Packed storage:
%
%    The following program segment will pack the upper triangle of
%    a symmetric matrix.
%
%      k = 0
%      do j = 1, n
%        do i = 1, j
%          k = k + 1
%          ap(k) = a(i,j)
%        end
%      end
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real AP(N*(N+1)/2), the packed form of a symmetric matrix A.
%    The columns of the upper triangle are stored sequentially in a
%    one-dimensional array.
%
%    Input, integer N, the order of the matrix.
%
%    Output, real AP(N*(N+1)/2), an upper riangular matrix R, stored
%    in packed form, so that A = R'*R.  If INFO /= 0, the factorization
%    is not complete.
%
%    Output, real RCOND, an estimate of the reciprocal condition
%    of A.  For the system A*X = B, relative perturbations in A and B of size
%    EPSILON may cause relative perturbations in X of size EPSILON/RCOND.
%    If RCOND is so small that the logical expression
%      1.0 + RCOND == 1.0D+00
%    is true, then A may be singular to working precision.  In particular,
%    RCOND is zero if exact singularity is detected or the estimate underflows.
%
%    Output, real Z(N), a work vector whose contents are usually
%    unimportant.  If A is singular to working precision, then Z is an
%    approximate null vector in the sense that
%      norm(A*Z) = RCOND * norm(A) * norm(Z).
%    If INFO /= 0, Z is unchanged.
%
%    Output, integer INFO, error flag.
%    0, for normal return.
%    K, signals an error condition.  The leading minor of order K is
%    not positive definite.
%

%
%  Find the norm of A.
%
  j1 = 1;
  for j = 1 : n
    z(j) = sasum ( j, ap(j1:j1+j-1), 1 );
    ij = j1;
    j1 = j1 + j;
    for i = 1 : j-1
      z(i) = z(i) + abs ( ap(ij) );
      ij = ij + 1;
    end
  end

  anorm = max ( z(1:n) );
%
%  Factor.
%
  [ ap, info ] = sppfa ( ap, n );

  if ( info ~= 0 )
    return
  end
%
%  RCOND = 1/(norm(A)*(estimate of norm(inverse(A)))).
%
%  Estimate = norm(Z)/norm(Y) where A * Z = Y and A * Y = E.
%
%  The components of E are chosen to cause maximum local
%  growth in the elements of W where R'*W = E.
%
%  The vectors are frequently rescaled to avoid overflow.
%
%  Solve R' * W = E.
%
  ek = 1.0;
  z(1:n) = 0.0;

  kk = 0;

  for k = 1 : n

    kk = kk + k;

    if ( z(k) ~= 0.0 )
      ek = -abs ( ek ) * r4_sign ( z(k) );
    end

    if ( ap(kk) < abs ( ek - z(k) ) )
      s = ap(kk) / abs ( ek - z(k) );
      z(1:n) = s * z(1:n);
      ek = s * ek;
    end

    wk = ek - z(k);
    wkm = -ek - z(k);
    s = abs ( wk );
    sm = abs ( wkm );
    wk = wk / ap(kk);
    wkm = wkm / ap(kk);
    kj = kk + k;

    if ( k + 1 <= n )

      for j = k + 1 : n
        sm = sm + abs ( z(j) + wkm * ap(kj) );
        z(j) = z(j) + wk * ap(kj);
        s = s + abs ( z(j) );
        kj = kj + j;
      end

      if ( s < sm )

        t = wkm - wk;
        wk = wkm;
        kj = kk + k;

        for j = k+1 : n
          z(j) = z(j) + t * ap(kj);
          kj = kj + j;
        end

      end

    end

    z(k) = wk;

  end

  z(1:n) = z(1:n) / sasum ( n, z(1:n), 1 );
%
%  Solve R * Y = W.
%
  for k = n : -1 : 1

    if ( ap(kk) < abs ( z(k) ) );
      s = ap(kk) / abs ( z(k) );
      z(1:n) = s * z(1:n);
    end

    z(k) = z(k) / ap(kk);
    kk = kk - k;
    t = -z(k);
    z(1:k-1) = saxpy ( k-1, t, ap(kk+1:kk+k-1), 1, z(1:k-1), 1 );

  end

  z(1:n) = z(1:n) / sasum ( n, z(1:n), 1 );

  ynorm = 1.0;
%
%  Solve R' * V = Y.
%
  for k = 1 : n

    z(k) = z(k) - sdot ( k-1, ap(kk+1:kk+k-1), 1, z(1:k-1), 1 );
    kk = kk + k;

    if ( ap(kk) < abs ( z(k) ) )
      s = ap(kk) / abs ( z(k) );
      z(1:n) = s * z(1:n);
      ynorm = s * ynorm;
    end

    z(k) = z(k) / ap(kk);

  end

  s = 1.0 / sasum ( n, z(1:n), 1 );
  z(1:n) = s * z(1:n);
  ynorm = s * ynorm;
%
%  Solve R * Z = V.
%
  for k = n : -1 : 1

    if ( ap(kk) < abs ( z(k) ) )
      s = ap(kk) / abs ( z(k) );
      z(1:n) = s * z(1:n);
      ynorm = s * ynorm;
    end

    z(k) = z(k) / ap(kk);
    kk = kk - k;
    t = -z(k);
    z(1:k-1) = saxpy ( k-1, t, ap(kk+1:kk+k-1), 1, z(1:k-1), 1 );

  end
%
%  Make ZNORM = 1.0.
%
  s = 1.0 / sasum ( n, z(1:n), 1 );
  z(1:n) = s * z(1:n);
  ynorm = s * ynorm;

  if ( anorm ~= 0.0 )
    rcond = ynorm / anorm;
  else
    rcond = 0.0;
  end

  return
end


function [ tri_num, tri_vert, tri_nabe ] = r8tris2 ( point_num, p )

%*****************************************************************************80
%
%% R8TRIS2 constructs a Delaunay triangulation of 2D vertices.
%
%  Discussion:
%
%    The routine constructs the Delaunay triangulation of a set of 2D vertices
%    using an incremental approach and diagonal edge swaps.  Vertices are
%    first sorted in lexicographically increasing (X,Y) order, and
%    then are inserted one at a time from outside the convex hull.
%
%  Modified:
%
%    07 February 2005
%
%  Author:
%
%    Original FORTRAN77 version by Barry Joe.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Barry Joe,
%    GEOMPACK - a software package for the generation of meshes
%    using geometric algorithms,
%    Advances in Engineering Software,
%    Volume 13, pages 325-331, 1991.
%
%  Parameters:
%
%    Input, integer POINT_NUM, the number of vertices.
%
%    Input, real P(2,POINT_NUM), the vertices.
%
%    Output, integer TRI_NUM, the number of triangles in the triangulation;
%    TRI_NUM is equal to 2*POINT_NUM - NB - 2, where NB is the number
%    of boundary vertices.
%
%    Output, integer TRI_VERT(3,TRI_NUM), the nodes that make up each triangle.
%    The elements are indices of P.  The vertices of the triangles are
%    in counter clockwise order.
%
%    Output, integer TRI_NABE(3,TRI_NUM), the triangle neighbor list.
%    Positive elements are indices of TIL; negative elements are used for links
%    of a counter clockwise linked list of boundary edges; LINK = -(3*I + J-1)
%    where I, J = triangle, edge index; TRI_NABE(J,I) refers to
%    the neighbor along edge from vertex J to J+1 (mod 3).
%
  tri_num = 0;
  tri_vert = [];
  tri_nabe = [];

  tol = 100.0 * r8_epsilon ( );
%
%  Sort the vertices by increasing (x,y).
%
  indx = r82vec_sort_heap_index_a ( point_num, p );

  p = r82vec_permute ( point_num, p, indx );
%
%  Make sure that the data points are "reasonably" distinct.
%
  m1 = 1;

  for i = 2 : point_num

    m = m1;
    m1 = i;

    k = 0;

    for j = 1 : 2

      cmax = max ( abs ( p(j,m) ), abs ( p(j,m1) ) );

      if ( tol * ( cmax + 1.0 ) < abs ( p(j,m) - p(j,m1) ) )
        k = j;
        break
      end

    end

    if ( k == 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R8TRIS2 - Fatal error!\n' );
      fprintf ( 1, '  Fails for point number I = %d\n', i );
      fprintf ( 1, '  M = %d\n', m );
      fprintf ( 1, '  M1 = %d\n', m1 );
      fprintf ( 1, '  X,Y(M)  = %f  %f\n', p(1,m), p(2,m) );
      fprintf ( 1, '  X,Y(M1) = %f  %f\n', p(1,m1), p(2,m1) );
      error ( 'R8TRIS2 - Fatal error!' )
      return
    end

  end
%
%  Starting from points M1 and M2, search for a third point M that
%  makes a "healthy" triangle (M1,M2,M)
%
  m1 = 1;
  m2 = 2;
  j = 3;

  while ( 1 )

    if ( point_num < j )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R8TRIS2 - Fatal error!\n' );
      error ( 'R8TRIS2 - Fatal error!' )
      return
    end

    m = j;

    lr = lrline ( p(1,m), p(2,m), p(1,m1), p(2,m1), p(1,m2), p(2,m2), 0.0 );

    if ( lr ~= 0 )
      break
    end

    j = j + 1;

  end
%
%  Set up the triangle information for (M1,M2,M), and for any other
%  triangles you created because points were collinear with M1, M2.
%
  tri_num = j - 2;

  if ( lr == -1 )

    tri_vert(1,1) = m1;
    tri_vert(2,1) = m2;
    tri_vert(3,1) = m;
    tri_nabe(3,1) = -3;

    for i = 2 : tri_num

      m1 = m2;
      m2 = i+1;
      tri_vert(1,i) = m1;
      tri_vert(2,i) = m2;
      tri_vert(3,i) = m;
      tri_nabe(1,i-1) = -3 * i;
      tri_nabe(2,i-1) = i;
      tri_nabe(3,i) = i - 1;

    end

    tri_nabe(1,tri_num) = -3 * tri_num - 1;
    tri_nabe(2,tri_num) = -5;
    ledg = 2;
    ltri = tri_num;

  else

    tri_vert(1,1) = m2;
    tri_vert(2,1) = m1;
    tri_vert(3,1) = m;
    tri_nabe(1,1) = -4;

    for i = 2 : tri_num
      m1 = m2;
      m2 = i+1;
      tri_vert(1,i) = m2;
      tri_vert(2,i) = m1;
      tri_vert(3,i) = m;
      tri_nabe(3,i-1) = i;
      tri_nabe(1,i) = -3 * i - 3;
      tri_nabe(2,i) = i - 1;
    end

    tri_nabe(3,tri_num) = -3 * tri_num;
    tri_nabe(2,1) = -3 * tri_num - 2;
    ledg = 2;
    ltri = 1;

  end
%
%  Insert the vertices one at a time from outside the convex hull,
%  determine visible boundary edges, and apply diagonal edge swaps until
%  Delaunay triangulation of vertices (so far) is obtained.
%
  top = 0;

  for i = j+1 : point_num

    m = i;
    m1 = tri_vert(ledg,ltri);

    if ( ledg <= 2 )
      m2 = tri_vert(ledg+1,ltri);
    else
      m2 = tri_vert(1,ltri);
    end

    lr = lrline ( p(1,m), p(2,m), p(1,m1), p(2,m1), p(1,m2), p(2,m2), 0.0 );

    if ( 0 < lr )
      rtri = ltri;
      redg = ledg;
      ltri = 0;
    else
      l = -tri_nabe(ledg,ltri);
      rtri = floor ( l / 3 );
      redg = mod(l,3) + 1;
    end

    [ ltri, ledg, rtri, redg ] = vbedg ( p(1,m), p(2,m), point_num, p, ...
      tri_num, tri_vert, tri_nabe, ltri, ledg, rtri, redg );

    n = tri_num + 1;
    l = -tri_nabe(ledg,ltri);

    while ( 1 )

      t = floor ( l / 3 );
      e = mod ( l, 3 ) + 1;
      l = -tri_nabe(e,t);
      m2 = tri_vert(e,t);

      if ( e <= 2 )
        m1 = tri_vert(e+1,t);
      else
        m1 = tri_vert(1,t);
      end

      tri_num = tri_num + 1;
      tri_nabe(e,t) = tri_num;
      tri_vert(1,tri_num) = m1;
      tri_vert(2,tri_num) = m2;
      tri_vert(3,tri_num) = m;
      tri_nabe(1,tri_num) = t;
      tri_nabe(2,tri_num) = tri_num - 1;
      tri_nabe(3,tri_num) = tri_num + 1;
      top = top + 1;

      if ( point_num < top )
        fprintf ( 1, '\n' );
        fprintf ( 1, 'R8TRIS2 - Fatal error!\n' );
        fprintf ( 1, '  Stack overflow.\n' );
        error ( 'R8TRIS2 - Fatal error!' )
      end

      work(top) = tri_num;

      if ( t == rtri & e == redg )
        break
      end

    end

    tri_nabe(ledg,ltri) = -3 * n - 1;
    tri_nabe(2,n) = -3 * tri_num - 2;
    tri_nabe(3,tri_num) = -l;
    ltri = n;
    ledg = 2;

    [ top, ltri, ledg, tri_vert, tri_nabe ] = swapec ( ...
      m, top, ltri, ledg, point_num, p, tri_num, tri_vert, tri_nabe, work );

  end
%
%  Now account for the sorting that we did.
%
  for i = 1 : 3
    for j = 1 : tri_num
      tri_vert(i,j) = indx ( tri_vert(i,j) );
    end
  end

  indx = perm_inverse ( point_num, indx );

  p = r82vec_permute ( point_num, p, indx );

  return
end


classdef ZXH_CF10 < PROBLEM
% <multi/many> <real> <large/none> <constrained>
% Constrained benchmark MOP proposed by Zhou, Xiang, and He

%------------------------------- Reference --------------------------------
% Y. Zhou, Y. Xiang, and X. He, Constrained multiobjective optimization:
% Test problem construction and performance evaluations, IEEE Transactions
% on Evolutionary Computation, 2021, 25(1): 172-186.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        k;  % Number of constrained variables
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            if isempty(obj.M); obj.M = 3; end
            if isempty(obj.D); obj.D = obj.M+10;  end
            obj.lower    = zeros(1,obj.D) + 1e-10;
            obj.upper    = ones(1,obj.D)  - 1e-10;
            obj.encoding = ones(1,obj.D);
            if obj.M <= 3
                obj.k = obj.M - 1;
            elseif obj.M > 3 && obj.M <= 8
                obj.k = floor(obj.M/2);
            else
                obj.k = 3;
            end
        end
        %% Evaluate multiple solutions
        function Population = Evaluation(obj,varargin)
            PopDec = varargin{1};
            OptX   = 0.2;
            [N,D]  = size(PopDec);
            M      = obj.M;
            % Step 1: Compute cumsum
            Sx = cumsum(PopDec(:,1:M).^2,2,'reverse');
            % Step 2: Compute theta
            THETA = 2/pi*atan(sqrt(Sx(:,2:end))./PopDec(:,1:M-1));
            % Step 3: Calculate Griewank function
            h = 5*(sum((PopDec(:,M+1:end)-OptX).^2,2)-prod(cos(10*pi*(PopDec(:,M+1:end)-OptX)./repmat(sqrt(1:(D-M)),N,1)),2)+1);
            % Step 4: Compute T_
            T = (1 - Sx(:,1)).^2 + h;
            % Step 5: Objectives (convex)
            G      = 1-[ones(N,1) cumprod(sin(pi/2*THETA),2)] .* [cos(pi/2*THETA) ones(N,1)];
            PopObj = G .* repmat((1+T),1,M);
            % Step 6: Constraints
            PopCon(:,1) = Sx(:,1) + h - 1;
            PopCon(:,2) = -(Sx(:,1) + h - 1/4);
            for i = 1 : obj.k
                PopCon(:,i+2) = min(THETA(:,i)-1/4,3/4-THETA(:,i));
            end
            Population = SOLUTION(varargin{1},PopObj,PopCon,varargin{2:end});
            obj.FE     = obj.FE + length(Population);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R = UniformPoint(N,obj.M);
            R = 1 - R./repmat(sqrt(sum(R.^2,2)),1,obj.M);
            T = zeros(size(R));
            for i = obj.M-1 : -1 : 1
                T(:,i) = atan((1-R(:,i+1))./(1-R(:,i))./cos(T(:,i+1)));
            end
            THETA = T(:,1:obj.k)*2/pi;
            Valid = all(THETA<=1/4|THETA>=3/4,2);
            R     = R(Valid,:);
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            if obj.M == 2
                R = obj.GetOptimum(100);
            elseif obj.M == 3
                a  = linspace(0,pi/2,30)';
                R  = {1-sin(a)*cos(a'),1-sin(a)*sin(a'),1-cos(a)*ones(size(a'))};
                T2 = atan((1-R{3})./(1-R{2}));
                T1 = asin((1-R{2})./cos(T2));
                THETA = cat(3,T1,T2)*2/pi;
                Valid = all(THETA<=1/4|THETA>=3/4,3);
                R{1}(~Valid) = nan;
            else
                R = [];
            end
        end
    end
end


% Fit a piece-wise linear regression model.
% Here is the model
%
%  X \
%  | |
%  Q |
%  | /
%  Y
%
% where all arcs point down.
% We condition everything on X, so X is a root node. Q is a softmax, and Y is a linear Gaussian.
% Q is hidden, X and Y are observed.

X = 1;
Q = 2;
Y = 3;
dag = zeros(3,3);
dag(X,[Q Y]) = 1;
dag(Q,Y) = 1;
ns = [1 2 1]; % make X and Y scalars, and have 2 experts
dnodes = [2];
onodes = [1 3];
bnet = mk_bnet(dag, ns, 'discrete', dnodes, 'observed', onodes);

IRLS_iter = 10;
clamped = 0;

bnet.CPD{1} = root_CPD(bnet, 1);

% start with good initial params
w = [-5 5];  % w(:,i) is the normal vector to the i'th decisions boundary
b = [0 0];  % b(i) is the offset (bias) to the i'th decisions boundary

mu = [0 0];
sigma = 1;
Sigma = repmat(sigma*eye(ns(Y)), [ns(Y) ns(Y) ns(Q)]);
W = [-1 1];
W2 = reshape(W, [ns(Y) ns(X) ns(Q)]);

bnet.CPD{2} = softmax_CPD(bnet, 2, w, b,  clamped, IRLS_iter);
bnet.CPD{3} = gaussian_CPD(bnet, 3, 'mean', mu, 'cov', Sigma, 'weights', W2);

engine = jtree_inf_engine(bnet);

evidence = cell(1,3);
evidence{X} = 0.68;

engine = enter_evidence(engine, evidence);

m = marginal_nodes(engine, Y);
m.mu


% qrtimax() -  perform Quartimax rotation of rows of a data matrix.
%
% Usage: >> [Q,B] = qrtimax(data);
%        >> [Q,B] = qrtimax(data,tol,'[no]reorder');
%
% Inputs:
%        data      - input matrix
%        tol       - the termination tolerance {default: 1e-4}
%        noreorder - rotate without negation/reordering
%
% Outputs:
%        B         - B=Q*A the Quartimax rotation of A
%        Q         - the orthogonal rotation matrix
%
% Author: Sigurd Enghoff, CNL / Salk Institute, 6/18/98

% Copyright (C) Sigurd Enghoff - CNL / Salk Institute, La Jolla 6/18/98
%
% 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 of the License, or
% (at your option) any later version.
%
% 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.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

% Reference: Jack O. Nehaus and Charles Wrigley (1954)
% The Quartimax Method: an analytic approach to orthogonal
% simple structure, Br J Stat Psychol, 7:81-91.

% 01-25-02 reformated help & license -ad

function [Q,B] = qrtimax(A,tol,reorder)

if nargin < 1
	help qrtimax
	return
end

DEFAULT_TOL = 1e-4;
MAX_ITERATIONS = 50;

if nargin < 3
	reorder = 1;
elseif isempty(reorder) | reorder == 0
	reorder = 1; % set default
else
	reorder = strcmp('reorder',reorder);
end

if nargin < 2
	eps1 = DEFAULT_TOL;
	eps2 = DEFAULT_TOL;
else
	eps1 = tol;
	eps2 = tol;
end

% Do unto 'Q' what is done to A

Q = eye(size(A,1));

% Compute the cross-products of the rows of the squared loadings,
% i.e. the cost function.
%
%  ---  ---
%  \    \     2    2
%  /    /    f    f
%  ---  ---   ij   ik
%   i   j<k
%
%  See reference, p. 85, line 3

B = tril((A.^2)*(A.^2)');
crit = [sum(sum(B)) - trace(B) , 0];

% Initialize variables

inoim = 0;
iflip = 1;
ict = 0;

% Main iterative loop: keep looping while no two consecutive trials
% satisfy tolerance constraint AND less than MAX_ITERATIONS trials
% AND one or more rotations were performed during last trial.

while inoim < 2 & ict < MAX_ITERATIONS & iflip,
	iflip = 0;

% Run through all combinations of j and k

	for j = 1:size(A,1)-1,
		for k = j+1:size(A,1),
%
%          ---                                   ---
%          \                2     2              \       2     2  2      2   2
% fnum = 4 /    [f   f    (f   - f  )]  , fden = /    [(f   - f  )  - 4 f   f  ]
%          ---    ki  kj    ki    kj             ---     ki    kj        ki  kj
%           k                                     k
%
%  See equation (5)

			u = A(j,:) .^ 2 - A(k,:) .^2;
			v = 2 * A(j,:) .* A(k,:);
			c = sum(u .^ 2 - v .^ 2);
			d = sum(u .* v);

			fden = c;
			fnum = 2 * d;

% Skip rotation if angle is too small

			if abs(fnum) > eps1 * abs(fden)
				iflip = 1;
				angl = atan2(fnum, fden);

% Set angle of rotation according to Table I

				if fnum > 0
					if fden > 0
						angl = .25 * angl;
					else
						angl = .25 * (pi - angl);
					end
				else
					if fden > 0
						angl = .25 * (2 * pi - angl);
					else
						angl = .25 * (pi + angl);
					end
				end

% Perform rotation

				tmp    =  cos(angl) * Q(j,:) + sin(angl) * Q(k,:);
				Q(k,:) = -sin(angl) * Q(j,:) + cos(angl) * Q(k,:);
				Q(j,:) = tmp;

				tmp    =  cos(angl) * A(j,:) + sin(angl) * A(k,:);
				A(k,:) = -sin(angl) * A(j,:) + cos(angl) * A(k,:);
				A(j,:) = tmp;
			end
		end
	end

% Compute cost function.

	B = tril((A.^2)*(A.^2)');
	crit = [sum(sum(B)) - trace(B) , crit(1)];

	inoim = inoim + 1;
	ict = ict + 1;

	fprintf('#%d - crit = %g\n',ict,(crit(1)-crit(2))/crit(1));

% Check relative change of cost function (termination criterion).

	if (crit(1) - crit(2)) / crit(1) > eps2
		inoim = 0;
	end
end

% Reorder and negate if required. Determine new row order based on
% row norms and reorder accordingly. Negate those rows in which the
% accumulated sum is negative.

if reorder
	fprintf('Reordering rows...');
	[fnorm index] = sort(sum(A'.^2));
	Q = Q .* ((2 * (sum(A') > 0) - 1)' * ones(1, size(Q,2)));
	A = A .* ((2 * (sum(A') > 0) - 1)' * ones(1, size(A,2)));
	Q = Q(fliplr(index),:);
	A = A(fliplr(index),:);
	fprintf('\n');
else
	fprintf('Not reordering rows.\n');
end

B=A;


% MHSWTRANS
%
%  MCMC Metropolis-Hastings transition function that
%  utilizes the Swendsen-Wang proposal distribution.
%  A - The current joint assignment.  This should be
%      updated to be the next assignment
%  G - The network
%  F - List of all factors
%  variant - a number (1 or 2) indicating the variant of Swendsen-Wang to use.  In variant 1,
%            all the q_{i,j}'s are equal
%
% Copyright (C) Daphne Koller, Stanford University, 2012

function A = MHSWTrans(A, G, F, variant)

%%%%%%%%%%%%%% Get Proposal %%%%%%%%%%%%%%
% Prune edges from q_list if the nodes don't have the same current value
q_list = G.q_list;
q_keep_indx = find(A(q_list(:, 1)) == A(q_list(:, 2)));
q_list = q_list(q_keep_indx, :);
% Select edges at random based on q_list
selected_edges_q_list_indx = find(q_list(:, 3) > rand(size(q_list,1), 1));
selected_edges = q_list(selected_edges_q_list_indx, 1:2);
% Compute connected components over selected edges
SelEdgeMat = sparse([selected_edges(:,1)'; selected_edges(:,2)'],...
                    [selected_edges(:,2)'; selected_edges(:,1)'],...
                    1, length(G.names), length(G.names));

[var2comp, cc_sizes] = scomponents(SelEdgeMat);
num_cc = length(cc_sizes);

% Select a connected component (the book calls this Y)
selected_cc = ceil(rand() * num_cc);
selected_vars = find(var2comp == selected_cc);
% Check that the dimensions are all the same and they have the same current assignment
assert(length(unique(G.card(selected_vars))) == 1);
assert(length(unique(A(selected_vars))) == 1);

% Pick a new label via sampling
old_value = A(selected_vars(1));
d = G.card(selected_vars(1));
LogR = zeros(1, d);
if variant == 1
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % YOUR CODE HERE
    % Specify the log of the distribution (LogR) from
    % which a new label for Y is selected for variant 1
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    LogR = ones(1,d).*log(1/d);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif variant == 2
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % YOUR CODE HERE
    % Specify the log of the distribution (LogR) from
    % which a new label for Y is selected for variant 2
    %
    % We suggest you read through the preceding code
    % before implementing this, one of the generated
    % data structures may be useful in implementing this section
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    LogR = BlockLogDistribution(selected_vars,G,F,A);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
else
    disp('WARNING: Unrecognized Swendsen-Wang Variant');
end

% Sample the new value from the distribution R
new_value = randsample(d, 1, true, exp(LogR));
A_prop = A;
A_prop(selected_vars) = new_value;

% Get the log-ratio of the probability of picking the connected component Y given A_prop over A
log_QY_ratio = 0.0;
for i = 1:size(G.q_list, 1)  % Iterate through *all* edges, not just the ones we selected earlier
    u = G.q_list(i, 1);
    v = G.q_list(i, 2);
    if length(intersect([u, v], selected_vars)) == 1  % the edge is from Y to outside-Y
        if A(u) == old_value && A(v) == old_value
            log_QY_ratio = log_QY_ratio - log(1 - G.q_list(i, 3));
        end
        if A_prop(u) == new_value && A_prop(v) == new_value
            log_QY_ratio = log_QY_ratio + log(1 - G.q_list(i, 3));
        end
    end
end

p_acceptance = 0.0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% YOUR CODE HERE
% Compute acceptance probability
%
% Read through the preceding code to understand
% how to find the previous and proposed assignments
% of variables, as well as some ratios used in computing
% the acceptance probabilitiy.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
lnum = LogProbOfJointAssignment(F,A_prop);
lden = LogProbOfJointAssignment(F,A);

p_acceptance = min(1,exp(lnum-lden+log_QY_ratio+LogR(old_value)-LogR(new_value)));
%p_acceptance = min(1,exp(log_QY_ratio));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Accept or reject proposal
if rand() < p_acceptance
    %disp('Accepted');
    A = A_prop;
end