function [ node_xyz, tet_node ] = tet_mesh_order4_example_set ( node_num, ...
  tet_num )

%*****************************************************************************80
%
%% TET_MESH_ORDER4_EXAMPLE_SET sets an example linear tet mesh.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 August 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, integer TET_NUM, the number of tetrahedrons.
%
%    Output, real NODE_XYZ(3,NODE_NUM), the node coordinates.
%
%    Output, integer TET_NODE(4,TET_NUM), the nodes
%    forming each tet.
%
  tet_node(1:4,1:tet_num) = [ ...
         1,   2,   4,  10; ...
         2,   4,   5,  10; ...
         2,   5,  10,  11; ...
         2,   3,   5,  11; ...
         4,   5,  10,  13; ...
         3,   5,   6,  11; ...
         5,  10,  11,  13; ...
         4,   5,   7,  13; ...
         5,   6,   8,  14; ...
         5,   7,   8,  13; ...
         6,   8,   9,  14; ...
        11,  13,  14,  19; ...
        12,  14,  15,  20; ...
         3,   6,  11,  12; ...
         5,   6,  11,  14; ...
         6,   9,  14,  15; ...
         6,  11,  12,  14; ...
         6,  12,  14,  15; ...
         7,   8,  13,  16; ...
         5,   8,  13,  14; ...
        10,  11,  13,  19; ...
         8,   9,  14,  17; ...
        11,  12,  14,  20; ...
         5,  11,  13,  14; ...
         8,  13,  14,  16; ...
         9,  14,  15,  17; ...
        13,  14,  16,  22; ...
         8,  14,  16,  17; ...
        14,  15,  17,  23; ...
        14,  16,  17,  22; ...
         9,  15,  17,  18; ...
        15,  17,  18,  23; ...
        14,  17,  22,  23; ...
        13,  14,  19,  22; ...
        11,  14,  19,  20; ...
        14,  15,  20,  23; ...
        15,  20,  21,  23; ...
        21,  23,  24,  29; ...
        20,  22,  23,  28; ...
        14,  19,  20,  22; ...
        15,  18,  23,  24; ...
        12,  15,  20,  21; ...
        15,  21,  23,  24; ...
        16,  17,  22,  25; ...
        19,  20,  22,  28; ...
        17,  18,  23,  26; ...
        20,  21,  23,  29; ...
        14,  20,  22,  23; ...
        17,  22,  23,  25; ...
        18,  23,  24,  26; ...
        22,  23,  25,  31; ...
        17,  23,  25,  26; ...
        23,  24,  26,  32; ...
        23,  25,  26,  31; ...
        18,  24,  26,  27; ...
        24,  26,  27,  32; ...
        23,  26,  31,  32; ...
        22,  23,  28,  31; ...
        20,  23,  28,  29; ...
        23,  24,  29,  32; ...
        24,  29,  30,  32; ...
        30,  32,  33,  38; ...
        29,  31,  32,  37; ...
        23,  28,  29,  31; ...
        24,  27,  32,  33; ...
        21,  24,  29,  30; ...
        24,  30,  32,  33; ...
        25,  26,  31,  34; ...
        28,  29,  31,  37; ...
        26,  27,  32,  35; ...
        29,  30,  32,  38; ...
        23,  29,  31,  32; ...
        26,  31,  32,  34; ...
        27,  32,  33,  35; ...
        31,  32,  34,  40; ...
        26,  32,  34,  35; ...
        32,  33,  35,  41; ...
        32,  34,  35,  40; ...
        27,  33,  35,  36; ...
        33,  35,  36,  41; ...
        32,  35,  40,  41; ...
        31,  32,  37,  40; ...
        29,  32,  37,  38; ...
        32,  33,  38,  41; ...
        33,  38,  39,  41; ...
        39,  41,  42,  47; ...
        38,  40,  41,  46; ...
        32,  37,  38,  40; ...
        33,  36,  41,  42; ...
        30,  33,  38,  39; ...
        33,  39,  41,  42; ...
        34,  35,  40,  43; ...
        37,  38,  40,  46; ...
        35,  36,  41,  44; ...
        38,  39,  41,  47; ...
        32,  38,  40,  41; ...
        35,  40,  41,  43; ...
        36,  41,  42,  44; ...
        40,  41,  43,  49; ...
        35,  41,  43,  44; ...
        41,  42,  44,  50; ...
        41,  43,  44,  49; ...
        36,  42,  44,  45; ...
        42,  44,  45,  50; ...
        41,  44,  49,  50; ...
        40,  41,  46,  49; ...
        38,  41,  46,  47; ...
        41,  42,  47,  50; ...
        42,  47,  48,  50; ...
        48,  50,  51,  56; ...
        47,  49,  50,  55; ...
        41,  46,  47,  49; ...
        42,  45,  50,  51; ...
        39,  42,  47,  48; ...
        42,  48,  50,  51; ...
        43,  44,  49,  52; ...
        46,  47,  49,  55; ...
        44,  45,  50,  53; ...
        47,  48,  50,  56; ...
        41,  47,  49,  50; ...
        44,  49,  50,  52; ...
        45,  50,  51,  53; ...
        49,  50,  52,  58; ...
        44,  50,  52,  53; ...
        50,  51,  53,  59; ...
        50,  52,  53,  58; ...
        45,  51,  53,  54; ...
        51,  53,  54,  59; ...
        50,  53,  58,  59; ...
        49,  50,  55,  58; ...
        47,  50,  55,  56; ...
        50,  51,  56,  59; ...
        51,  56,  57,  59; ...
        50,  55,  56,  58; ...
        51,  54,  59,  60; ...
        48,  51,  56,  57; ...
        51,  57,  59,  60; ...
        52,  53,  58,  61; ...
        53,  54,  59,  62; ...
        50,  56,  58,  59; ...
        53,  58,  59,  61; ...
        54,  59,  60,  62; ...
        53,  59,  61,  62; ...
        54,  60,  62,  63 ]';

  node_xyz(1:3,1:node_num) = [ ...
  0.0,  0.0,  0.0; ...
  0.0,  0.0,  0.5; ...
  0.0,  0.0,  1.0; ...
  0.0,  0.5,  0.0; ...
  0.0,  0.5,  0.5; ...
  0.0,  0.5,  1.0; ...
  0.0,  1.0,  0.0; ...
  0.0,  1.0,  0.5; ...
  0.0,  1.0,  1.0; ...
  0.5,  0.0,  0.0; ...
  0.5,  0.0,  0.5; ...
  0.5,  0.0,  1.0; ...
  0.5,  0.5,  0.0; ...
  0.5,  0.5,  0.5; ...
  0.5,  0.5,  1.0; ...
  0.5,  1.0,  0.0; ...
  0.5,  1.0,  0.5; ...
  0.5,  1.0,  1.0; ...
  1.0,  0.0,  0.0; ...
  1.0,  0.0,  0.5; ...
  1.0,  0.0,  1.0; ...
  1.0,  0.5,  0.0; ...
  1.0,  0.5,  0.5; ...
  1.0,  0.5,  1.0; ...
  1.0,  1.0,  0.0; ...
  1.0,  1.0,  0.5; ...
  1.0,  1.0,  1.0; ...
  1.5,  0.0,  0.0; ...
  1.5,  0.0,  0.5; ...
  1.5,  0.0,  1.0; ...
  1.5,  0.5,  0.0; ...
  1.5,  0.5,  0.5; ...
  1.5,  0.5,  1.0; ...
  1.5,  1.0,  0.0; ...
  1.5,  1.0,  0.5; ...
  1.5,  1.0,  1.0; ...
  2.0,  0.0,  0.0; ...
  2.0,  0.0,  0.5; ...
  2.0,  0.0,  1.0; ...
  2.0,  0.5,  0.0; ...
  2.0,  0.5,  0.5; ...
  2.0,  0.5,  1.0; ...
  2.0,  1.0,  0.0; ...
  2.0,  1.0,  0.5; ...
  2.0,  1.0,  1.0; ...
  2.5,  0.0,  0.0; ...
  2.5,  0.0,  0.5; ...
  2.5,  0.0,  1.0; ...
  2.5,  0.5,  0.0; ...
  2.5,  0.5,  0.5; ...
  2.5,  0.5,  1.0; ...
  2.5,  1.0,  0.0; ...
  2.5,  1.0,  0.5; ...
  2.5,  1.0,  1.0; ...
  3.0,  0.0,  0.0; ...
  3.0,  0.0,  0.5; ...
  3.0,  0.0,  1.0; ...
  3.0,  0.5,  0.0; ...
  3.0,  0.5,  0.5; ...
  3.0,  0.5,  1.0; ...
  3.0,  1.0,  0.0; ...
  3.0,  1.0,  0.5; ...
  3.0,  1.0,  1.0 ]';

  return
end


function y = abs( x )

%Disciplined convex/geometric programming information for ABS:
%   ABS(X) is convex and nonmonotonic in X. Therefore, when used in
%   DCPs, X must be affine. ABS(X) is not useful in DGPs, since all
%   log-convex and log-concave expressions are already positive.

%
% Determine the expression types
%

% 0 : convex, concave, invalid
% 1 : constant
% 2 : real affine
% 3 : complex affine
persistent remap
if isempty( remap ),
    remap_1 = cvx_remap( 'constant' );
    remap_2 = cvx_remap( 'real-affine' );
    remap_3 = cvx_remap( 'complex-affine' );
    remap_4 = cvx_remap( 'log-valid' );
    remap = remap_1 + ( 2 * remap_2 + 3 * remap_3 + 4 * remap_4 ) .* ~remap_1;
end
v = remap( cvx_classify( x ) );

%
% Process each type of expression one piece at a time
%

vu = sort( v(:) );
vu = vu([true;diff(vu)~=0]);
nv = length( vu );
sx = x.size_;
if nv ~= 1,
    y = cvx( sx, [] );
end
for k = 1 : nv,

    %
    % Select the category of expression to compute
    %

    vk = vu( k );
    if nv == 1,
        xt = x;
    else
        t = v == vk;
        xt = cvx_subsref( x, t );
    end

    %
    % Perform the computations
    %

    switch vk,
        case 0,
            % Invalid
            error( 'Disciplined convex programming error:\n    Illegal operation: abs( {%s} ).', cvx_class( xt ) );
        case 1,
            % Constant
            cvx_optval = cvx( builtin( 'abs', cvx_constant( xt ) ) );
        case 2,
            % Real affine
            st = size( xt );
            cvx_begin
                epigraph variable w( st )
                { xt, w } == lorentz( st, 0 );
            cvx_end
        case 3,
            % Complex affine
            st = size( xt );
            cvx_begin
                epigraph variable w( st )
                { xt, w } == complex_lorentz( st, 0 );
            cvx_end
        case 4,
            % log-affine, log-convex
            cvx_optval = xt;
        otherwise,
            error( 'Shouldn''t be here.' );
    end

    %
    % Store the results
    %

    if nv == 1,
        y = cvx_optval;
    else
        y = cvx_subsasgn( y, t, cvx_optval );
    end

end

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


%
% Original source
% http://www.mat.univie.ac.at/~neum/glopt/coconut/Benchmark/Library1/alkyl.mod
%
function test_alkyl

  addpath('../lib') ;

  auxdata = {} ;
  options.lb = [ 0, 0,   0,   0, 0, 0.85, 0.9,  3,  1.2, 1.45, 0.99, 0.99, 0.9, 0.99 ] ;
  options.ub = [ 2, 1.6, 1.2, 5, 2, 0.93, 0.95, 12, 4,   1.62, 1.01010101010101, 1.01010101010101, 1.11111111111111, 1.01010101010101 ] ;

  % The constraint functions are bounded to zero
  options.cl = zeros(1,7); %  constraints
  options.cu = zeros(1,7);

  % Set up the auxiliary data.
  options.auxdata = auxdata ;

  % Set the IPOPT options.
  options.ipopt.jac_d_constant   = 'no';
  options.ipopt.hessian_constant = 'no';
  options.ipopt.mu_strategy      = 'adaptive';
  options.ipopt.max_iter         = 400;
  options.ipopt.tol              = 1e-10;
  options.ipopt.linear_solver    = 'ma57';
  %options.ipopt.linear_solver    = 'mumps';
  %options.ipopt.linear_solver    = 'pardiso';

  % The callback functions.
  funcs.objective         = @objective;
  funcs.constraints       = @constraints;
  funcs.gradient          = @gradient;
  funcs.jacobian          = @jacobian;
  funcs.jacobianstructure = @jacobianstructure;
  if true
    funcs.hessian           = @hessian;
    funcs.hessianstructure  = @hessianstructure;
    options.ipopt.derivative_test = 'second-order';
  else
    options.ipopt.hessian_approximation      = 'limited-memory';
    options.ipopt.limited_memory_update_type = 'bfgs' ; % {bfgs}, sr1 = 6; % {6}
    options.ipopt.derivative_test            = 'first-order';
  end

  % Run IPOPT.
  x0 = [ 1.745,   ...
         1.2,     ...
         1.1,     ...
         3.048,   ...
         1.974,   ...
         0.893,   ...
         0.928,   ...
         8,       ...
         3.6,     ...
         1.45,    ...
         1,       ...
         1,       ...
         1,       ...
         1 ] ;

  tic
  [x, info] = ipopt_auxdata(x0,funcs,options);
  elapsed = toc ;

  info;

  x

end

%%
% map the indices with the corresponding index in the spase matrix
function f = objective( x, auxdata )
  x2  = x(1) ;
  x3  = x(2) ;
  x4  = x(3) ;
  x5  = x(4) ;
  x6  = x(5) ;
  x7  = x(6) ;
  x8  = x(7) ;
  x9  = x(8) ;
  x10 = x(9) ;
  x11 = x(10) ;
  x12 = x(11) ;
  x13 = x(12) ;
  x14 = x(13) ;
  x15 = x(14) ;
  f   = - 6.3*x5*x8 + 5.04*x2 + 0.35*x3 + x4 + 3.36*x6 ;
end

%%
% map the indices with the corresponding index in the spase matrix
function g = gradient(x,auxdata)
  x2  = x(1) ;
  x3  = x(2) ;
  x4  = x(3) ;
  x5  = x(4) ;
  x6  = x(5) ;
  x7  = x(6) ;
  x8  = x(7) ;
  x9  = x(8) ;
  x10 = x(9) ;
  x11 = x(10) ;
  x12 = x(11) ;
  x13 = x(12) ;
  x14 = x(13) ;
  x15 = x(14) ;
  g   = [ 5.04, 0.35, 1, -6.3*x8, 3.36, 0, -6.3*x5, 0, 0, 0, 0, 0, 0, 0 ] ;
end

function c = constraints(x,auxdata)
  x2  = x(1) ;
  x3  = x(2) ;
  x4  = x(3) ;
  x5  = x(4) ;
  x6  = x(5) ;
  x7  = x(6) ;
  x8  = x(7) ;
  x9  = x(8) ;
  x10 = x(9) ;
  x11 = x(10) ;
  x12 = x(11) ;
  x13 = x(12) ;
  x14 = x(13) ;
  x15 = x(14) ;
  e2  =  - 0.819672131147541*x2 + x5 - 0.819672131147541*x6 ;
  e3  = 0.98*x4 - x7*(0.01*x5*x10 + x4) ;
  e4  =  - x2*x9 + 10*x3 + x6 ;
  e5  = x5*x12 - x2*(1.12 + 0.13167*x9 - 0.0067*x9*x9) ;
  e6  = x8*x13 - 0.01*(1.098*x9 - 0.038*x9*x9) - 0.325*x7 - 0.57425;
  e7  = x10*x14 + 22.2*x11 - 35.82;
  e8  = x11*x15 - 3*x8 + 1.33 ;
  c   = [e2;e3;e4;e5;e6;e7;e8] ;
end

function jac = jacobian(x,auxdata)
  x2  = x(1) ;
  x3  = x(2) ;
  x4  = x(3) ;
  x5  = x(4) ;
  x6  = x(5) ;
  x7  = x(6) ;
  x8  = x(7) ;
  x9  = x(8) ;
  x10 = x(9) ;
  x11 = x(10) ;
  x12 = x(11) ;
  x13 = x(12) ;
  x14 = x(13) ;
  x15 = x(14) ;

  jac = sparse(7,14) ;

  jac(1,1) = -0.819672131147541 ;
  jac(1,4) = 1 ;
  jac(1,5) = -0.819672131147541 ;

  jac(2,3) = 0.98 - x7 ;
  jac(2,4) = -x7*(0.01*x10);
  jac(2,6) = -(0.01*x5*x10 + x4);
  jac(2,9) = - x7*(0.01*x5);

  jac(3,1) = -x9 ;
  jac(3,2) = 10 ;
  jac(3,5) = 1 ;
  jac(3,8) = -x2 ;

  jac(4,1)  = -(1.12 + 0.13167*x9 - 0.0067*x9*x9) ;
  jac(4,4)  = x12 ;
  jac(4,8)  = -x2*(0.13167 - 2*0.0067*x9) ;
  jac(4,11) = x5 ;

  jac(5,6)  = -0.325;
  jac(5,7)  = x13 ;
  jac(5,8)  = - 0.01*(1.098 - 2*0.038*x9) ;
  jac(5,12) = x8 ;

  jac(6,9)  = x14;
  jac(6,10) = 22.2;
  jac(6,13) = x10;

  jac(7,7)  = -3 ;
  jac(7,10) = x15 ;
  jac(7,14) = x11 ;

end

function jac = jacobianstructure(auxdata)
  jac = sparse(7,14) ;

  jac(1,1) = 1 ;
  jac(1,4) = 1 ;
  jac(1,5) = 1 ;

  jac(2,3) = 1 ;
  jac(2,4) = 1 ;
  jac(2,6) = 1 ;
  jac(2,9) = 1 ;

  jac(3,1) = 1 ;
  jac(3,2) = 1 ;
  jac(3,5) = 1 ;
  jac(3,8) = 1 ;

  jac(4,1)  = 1 ;
  jac(4,4)  = 1 ;
  jac(4,8)  = 1 ;
  jac(4,11) = 1 ;

  jac(5,6)  = 1 ;
  jac(5,7)  = 1 ;
  jac(5,8)  = 1 ;
  jac(5,12) = 1 ;

  jac(6,9)  = 1 ;
  jac(6,10) = 1 ;
  jac(6,13) = 1 ;

  jac(7,7)  = 1 ;
  jac(7,10) = 1 ;
  jac(7,14) = 1 ;
end

function H = hessian(x, sigma, L, auxdata)

  x2  = x(1) ;
  x3  = x(2) ;
  x4  = x(3) ;
  x5  = x(4) ;
  x6  = x(5) ;
  x7  = x(6) ;
  x8  = x(7) ;
  x9  = x(8) ;
  x10 = x(9) ;
  x11 = x(10) ;
  x12 = x(11) ;
  x13 = x(12) ;
  x14 = x(13) ;
  x15 = x(14) ;

  Hf = sparse(14,14) ;
  Hf(4,7) = -6.3 ;
  Hf(7,4) = -6.3 ;

  H2 = sparse(14,14) ;
  H2(3,6) = -1 ;
  H2(6,3) = -1;
  H2(4,6) = -(0.01*x10) ;
  H2(6,4) = -(0.01*x10);
  H2(4,9) = -x7*0.01 ;
  H2(9,4) = -x7*0.01 ;
  H2(6,9) = -(0.01*x5);
  H2(9,6) = -(0.01*x5);

  H3 = sparse(14,14) ;
  H3(1,8)  = -1 ;
  H3(8,1)  = -1 ;

  H4 = sparse(14,14) ;
  H4(1,8)  = -(0.13167 - 2*0.0067*x9) ;
  H4(8,1)  = -(0.13167 - 2*0.0067*x9) ;
  H4(4,11) = 1 ;
  H4(11,4) = 1 ;
  H4(8,8)  = x2*2*0.0067 ;

  H5 = sparse(14,14) ;
  H5(7,12) = 1 ;
  H5(12,7) = 1 ;
  H5(8,8)  = 0.01*(2*0.038) ;

  H6 = sparse(14,14) ;
  H6(9,13) = 1;
  H6(13,9) = 1;

  H7 = sparse(14,14) ;
  H7(10,14) = 1 ;
  H7(14,10) = 1 ;

  H  = tril(sparse(sigma * Hf + L(2)*H2 + L(3)*H3 + L(4)*H4 + L(5)*H5 + L(6)*H6 + L(7)*H7)) ;
end

function H = hessianstructure(auxdata)

  Hf = sparse(14,14) ;
  Hf(4,7) = 1;
  Hf(7,4) = 1;

  H2 = sparse(14,14) ;
  H2(3,6) = 1;
  H2(6,3) = 1;
  H2(4,6) = 1;
  H2(6,4) = 1;
  H2(4,9) = 1;
  H2(9,4) = 1;
  H2(6,9) = 1;
  H2(9,6) = 1;

  H3 = sparse(14,14) ;
  H3(1,8) = 1;
  H3(8,1) = 1;

  H4 = sparse(14,14) ;
  H4(1,8)  = 1;
  H4(8,1)  = 1;
  H4(4,11) = 1;
  H4(11,4) = 1;
  H4(8,8)  = 1;

  H5 = sparse(14,14) ;
  H5(7,12) = 1;
  H5(12,7) = 1;
  H5(8,8)  = 1;

  H6 = sparse(14,14) ;
  H6(9,13) = 1;
  H6(13,9) = 1;

  H7 = sparse(14,14) ;
  H7(10,14) = 1 ;
  H7(14,10) = 1 ;

  H = tril(sparse(Hf+H2+H3+H4+H5+H6+H7)) ;
end


close all
clear

%------------------------------------------------------------------------------------------
nb = 1000;%Total number of initial states
nmpcd=zeros(1,nb);%Store computing time for one time-step for NMPC-DCBF
impcd=zeros(1,nb);%Store computing time for one time-step for iMPC-DCBF
nmpcinf=zeros(1,1);%Store infeasible rate for one time-step for NMPC-DCBF
impcinf=zeros(1,1);%Store infeasible rate for one time-step for iMPC-DCBF
i=24;%i is number of horozon
ts = 1;%Total time steps
load(gamma1_0p4gamma2_0p4\InitialStateData');%Load the generated 1000 initial states
[a, b, c, d]=compare(i,ts,nb,InitialMat);%Involves NMPC-DCBF and iMPC-DCBF
nmpcd(1,:)=a;
impcd(1,:)=b;
nmpcinf(1,1)=c;
impcinf(1,1)=d;
nmpcplot1=nmpcd(1,:);
nmpcplot1(nmpcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
impcplot1=impcd(1,:);
impcplot1(impcplot1==-1)=[];%Eliminate infeasible one time-step trajectories

save('timecom24','nmpcplot1','impcplot1');
save('feasibility24','nmpcinf','impcinf');

distnmpc = fitdist(nmpcplot1','Normal');
distimpc = fitdist(impcplot1','Normal');
mu1=distnmpc.mu;%Get mean of sample of computing time for NMPC-DCBF
mu2=distimpc.mu;%Get mean of sample of computing time for iMPC-DCBF
sigma1=distnmpc.sigma;%Get variance of sample of computing time for NMPC-DCBF
sigma2=distimpc.sigma;%Get variance of sample of computing time for iMPC-DCBF

%Initialize atmosphere parameters
function [tnmpc, timpc, ratiotnmpc, ratiotimpc]=compare(N11,ttsim,samplen,InitialMat)
tnmpc=[];
timpc=[];
for i=1:samplen
N1=N11;
tsim=ttsim;
xini1=InitialMat(1,i);
yini1=InitialMat(2,i);
thetaini1=InitialMat(3,i);
vini1=InitialMat(4,i);
x01=[xini1;yini1;thetaini1;vini1];
x02=[xini1 yini1 thetaini1 vini1];
t1 = nmpcdcbf(x01, N1, tsim);
t2 = impcdcbf(x02, N1, tsim);
tindex=[N11 i];
disp(tindex);
tnmpc=[tnmpc t1];%Computing time for NMPC-DCBF
timpc=[timpc t2];%Computing time for iMPC-DCBF
end
nnmpc1 = length(tnmpc);
nimpc1 = length(timpc);
tnmpcs = tnmpc;
timpcs = timpc;
tnmpcs(tnmpcs==-1)=[];
timpcs(timpcs==-1)=[];
nnmpc2 = length(tnmpcs);
nimpc2 = length(timpcs);
ratiotnmpc = (nnmpc1-nnmpc2)/nnmpc1;%Infeasible rate for NMPC-DCBF
ratiotimpc = (nimpc1-nimpc2)/nimpc1;%Infeasible rate for iMPC-DCBF
end

function t1 = nmpcdcbf(x00, N1, ttsim)
%% General Flags
run_nmpc_dcbf_one = true;
run_nmpc_dcbf_multiple = true;

%% Setup and Parameters
x0 = x00;
dt = 0.1;
time_total = ttsim *dt;
P = zeros(4,4);%Weight matrix P
P(1,1) = 10; P(2,2) = 10; P(3,3) = 10;P(4,4) = 10;
Q = zeros(4,4);%Weight matrix Q
Q(1,1) = 10; Q(2,2) = 10; Q(3,3) = 10;Q(4,4) = 10;
R = zeros(4,4);%Weight matrix R
R(1,1) = 1; R(2,2) = 1;R(3,3) = 1000;R(4,4) = 1000;
%Variables range as below
xmin = [-10; -10; -10;-10];
xmax = [10; 10; 10;10];
umin = [-7; -5;-inf;-inf];
umax = [7; 5;inf;inf];

%% Discrete-time unicycle model
system.dt = dt;
system.A = [1 0 0 0;
    0 1 0 0;
    0 0 1 0;
    0 0 0 1];
system.B = [x0(4,1)*cos(x0(3,1))*dt;
    x0(4,1)*sin(x0(3,1))*dt;
    0;
    0];
system.C =[0 0 0 0;
    0 0 0 0;
    1*dt 0 0 0;
    0 1*dt 0 0];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;

%% NMPC-DCBF parameters
params_nmpc_dcbf.Q = Q;
params_nmpc_dcbf.R = R;
params_nmpc_dcbf.P = P;
params_nmpc_dcbf.gamma1 = 0.4;
params_nmpc_dcbf.gamma2 = 0.4;

%% Obstacle
obs.pos1 = [0; 0];
obs.r1 = 1;

%% Simulate NMPC-DCBF with other N
params_nmpc_dcbf.N = N1;
if run_nmpc_dcbf_one
    fprintf('Run NMPC-DCBF-24\n');
    controller_nmpc_dcbf_multiple24 = NMPCDCBF2(x0, system, params_nmpc_dcbf);%Highest order mcbf=2
    controller_nmpc_dcbf_multiple24.obs = obs;
    controller_nmpc_dcbf_multiple24.sim(time_total);
end
t1=controller_nmpc_dcbf_multiple24.tt;
end

function t2=impcdcbf(x00, N1, ttsim)
t2=0;
xo = 0;
yo = 0;
r = 1;
N = N1;%Number of horizon
K1 = 1000;%Maximum iteration times, jmax
K = 1000;%Maximum iteration times, jmax
dt = 0.1;
gamma1 = 4;
gamma2 = 4;
nsim = ttsim;

tic;

% Constraints

umin = [-7; -5; -inf; -inf;];
umax = [7; 5; Inf; Inf];
xmin = [-10; -10; -10; -10];
xmax = [ 10;  10;  10;  10];

% Objective function
Q = diag([10 10 10 10]);
QN = Q;
R = 1 * eye(4);
R(3,3) = 1000;
R(4,4) = 1000;
% Initial and reference states
x0 = x00;
xr = [3; 0.01; 0; 0];
ur = [0; 0; 1; 1];

% Dynamic system initialization
BB = [0 0 0 0;0 0 0 0;dt 0 0 0;0 dt 0 0];

% Convex MPC frame
[nx, nu] = size(BB);
% Initialize states set (x00,x01,,,x0N）
x_0 = [];
x0 = x0';
u_0 = zeros(nu, N);
u0 = zeros(nu, 1);% we may need to change this for a better warm-up start
impc = zeros(nx, nsim + 1);
impc(:, 1) = x0;
x0new = x0;
for i=1 : (N+1)
    x_0 = [x_0 x0new];
    x0new = [dt*x0new(4)*cos(x0new(3))+x0new(1);dt*x0new(4)*sin(x0new(3))+x0new(2);dt*u0(1)+x0new(3);dt*u0(2)+x0new(4)];
end

abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);%First order CBF constraints
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);%Second order CBF constraints
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);

% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];

% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();

% Check solver status
if ~strcmp(res.info.status, 'solved')
%    error('OSQP did not solve the problem!')
        t2=-1;
        return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);

for j = 1 : (K1-1)%Iterations for the first time-step
    storagex = ctrlx;%store the state variables and inputs in order to compare them with the next iteration
    storageu = ctrlu;
    x_0 = trans(x0, ctrlx, nx, N+1);
    u_0 = transu(ctrlu, nu, N);
    abc = tangentline(x_0, r);
    Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
    lcbf = -abc(3, :);
    lcbf(1) = [];
    lcbf = lcbf';
    ucbf = inf * ones(N, 1);
    A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
    lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
    ucbf2 = inf * ones(N-1, 1);
    % Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
    % - quadratic objective
    P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
    % - linear objective
    q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
    % - linear dynamics
    Ax = getax(x_0, dt, N, nx);
    Bu = kron([sparse(1, N); speye(N)], BB);
    Cx = getcx(x_0, u_0, dt, N, nx);
    Aeq = [Ax, Bu];
    leq = [-x0; -Cx];
    ueq = leq;
    % - input and state constraints
    Aineq = speye((N+1)*nx + N*nu);
    lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
    uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

    % - OSQP constraints
    A = [Aeq; Aineq; Acbf; A2cbf];
    l = [leq; lineq; lcbf; lcbf2];
    u = [ueq; uineq; ucbf; ucbf2];
    % Create an OSQP object
    prob = osqp;
    % Setup workspace
    prob.setup(P, q, A, l, u, 'warm_start', true);
    % Solve
    res = prob.solve();

    % Check solver status
    if ~strcmp(res.info.status, 'solved')
%        error('OSQP did not solve the problem!')
        t2=-1;
        return
    end
    ctrlx = res.x(1:(N+1)*nx);
    ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
    testx = (storagex - ctrlx)'*(storagex - ctrlx);
    testu = (storageu - ctrlu)'*(storageu - ctrlu);
    test = (testx)/(storagex'*storagex);
    if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
      break
    end
end % Move for the first step
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
t2=t2+toc;
return

x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, 2) = x0;
storagex = ctrlx;
storageu = ctrlu;

for i = 1 : (nsim-1)%Iterations for the left time steps

   for j = 1 : K
    abc = tangentline(x_0, r);
    Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
    lcbf = -abc(3, :);
    lcbf(1) = [];
    lcbf = lcbf';
    ucbf = inf * ones(N, 1);
    A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
    lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
    ucbf2 = inf * ones(N-1, 1);
    % Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
    % - quadratic objective
    P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
    % - linear objective
    q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
    % - linear dynamics
    Ax = getax(x_0, dt, N, nx);
    Bu = kron([sparse(1, N); speye(N)], BB);
    Cx = getcx(x_0, u_0, dt, N, nx);
    Aeq = [Ax, Bu];
    leq = [-x0; -Cx];
    ueq = leq;
    % - input and state constraints
    Aineq = speye((N+1)*nx + N*nu);
    lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
    uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

    % - OSQP constraints
    A = [Aeq; Aineq; Acbf; A2cbf];
    l = [leq; lineq; lcbf; lcbf2];
    u = [ueq; uineq; ucbf; ucbf2];
    % Create an OSQP object
    prob = osqp;
    % Setup workspace
    prob.setup(P, q, A, l, u, 'warm_start', true);
    % Solve
    res = prob.solve();

    % Check solver status
    if ~strcmp(res.info.status, 'solved')
%        error('OSQP did not solve the problem!')
        t2=-1;
        return
    end
    ctrlx = res.x(1:(N+1)*nx);
    ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
    x0 = ctrlx(1:nx);
    testx0 = [testx0 x0];
    x_0 = trans(x0, ctrlx, nx, N+1);
    u_0 = transu(ctrlu, nu, N);
    testx = (storagex - ctrlx)'*(storagex - ctrlx);
    testu = (storageu - ctrlu)'*(storageu - ctrlu);
    test = (testx)/(storagex'*storagex);
    if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
      break
    end
    storagex = ctrlx;
    storageu = ctrlu;
   end
   ctrl = ctrlu(1:nu);
   x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
   ctrlu = rewrite(ctrlu, nu, N);
   x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
   u_0 = transu(ctrlu, nu, N);
   impc(:, i+2) = x0;
end
end

% Linerize the CBF constraints (get a, b, c for lines)
function abc = tangentline(xy, r)% x and y from initialize states set, abc are coeeficients for linear equation a*x+b*y+c=0
[xx, ~] = size(xy);%xx=2,yy=N+1
xy(xx,:) = []; % this part should be changed for other case
xy((xx-1),:) = []; % this part should be changed other case
[xx, yy] = size(xy);%xx=2,yy=N+1
xyjiao = zeros(xx, yy);%intersection points
for i = 1 : xx
    for j = 1 : yy
        xyjiao(i, j) = r * xy(i, j) * (1 / (xy(:, j)' * xy(:, j)))^(0.5);%calculate coordinates of intersection points
    end
end
cc = -r^2 * ones(1, yy);
abc = [xyjiao; cc];
end

% Get CBF constraints matrix
function Acbf = cbfmat(abc, nx, nu, dt, gamma, x0)
[~, yy] = size(abc);
Acbfx = zeros((yy-1), yy*nx);
Acbfu = zeros((yy-1), (yy-1)*nu);
for i = 1 : (yy-1)
    Acbfx(i, (i*nx)+1) = abc(1, (i+1));
    Acbfx(i, (i*nx)+2) = abc(2, (i+1));
end
for i = 1 : (yy-1)
    Acbfu(i, ((i-1)*nu+3)) = - (1 - dt * gamma)^(i) * (abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
Acbf = [Acbfx Acbfu];
end

% Transfer vector x into matrix
function res = trans(x0, vector, nxx, nyy)%nxx=nx,nyy=N+1
 res = zeros(nxx, nyy);
 res(:,1) = x0;
 for i = 1 : (nyy -1)
     res(:,i+1)= vector(((i)*nxx+1):(i+1)*nxx);
 end
end

% Transfer vector u into matrix
function resu = transu(vector, nxx, nyy)%nxx=nu,nyy=N
 resu = zeros(nxx, nyy);
 for i = 1 : (nyy)
     resu(:,i)= vector(((i-1)*nxx+1):(i)*nxx);
 end
end

% Rewrite u vector
function reu = rewrite(vector, nu, N)
append = vector((N-1)*nu+1:N*nu);
vector(1:nu) = [];
reu = [vector;append];
end

% Get new x_0
function x_0 = newinit(x0, ctrlu, nx, nu, N, dt)
 x_0 = zeros(nx, N+1);
 x_0(:, 1) = x0;
 for i=1 : N
    u0 = ctrlu((i-1)*nu+1:i*nu);
    x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*u0(1)+x0(3);dt*u0(2)+x0(4)];
    x_0(:, i + 1) = x0;
 end
end

% Get AA matrix
function AA = getaa(x0, dt)
 AA = [1 0 -x0(4)*sin(x0(3))*dt cos(x0(3))*dt;0 1 x0(4)*cos(x0(3))*dt sin(x0(3))*dt;0 0 1 0;0 0 0 1];
end
% Get CC matrix
function CC = getcc(x0, x1, u0, dt)
 CC = [x0(4)*sin(x0(3))*x0(3)*dt-x0(4)*cos(x0(3))*dt+x1(1)-x0(1);-x0(4)*cos(x0(3))*x0(3)*dt-x0(4)*sin(x0(3))*dt+x1(2)-x0(2);-u0(1)*dt+x1(3)-x0(3);-u0(2)*dt+x1(4)-x0(4)];
end
% Get Ax matrix
function Ax = getax(x_0, dt, N, nx)
x0 = x_0(:,1);
AA = getaa(x0, dt);
Ax = kron(speye(N+1), -speye(nx)) + kron(sparse(diag(ones(N, 1), -1)), AA);
for i = 1 : (N-1)
   x0 = x_0(:,i+1);
   AA = getaa(x0, dt);
   Ax(nx*(i+1)+1:nx*(i+1)+nx,nx*i+1:nx*i+nx) = AA;
end
end
% Get Cx matrix
function Cx = getcx(x_0, u_0, dt, N, nx)
Cx = zeros(N*nx, 1);
for i = 1 : N
    u0 = u_0(:,i);
    x0 = x_0(:,i);
    x1 = x_0(:,i+1);
    CC = getcc(x0, x1, u0, dt);
    Cx((i-1)*nx+1:(i-1)*nx+nx) = CC;
end
end
% Get A2cbf
function A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0)
[~, yy] = size(abc);
Acbfx2 = zeros((yy-2), yy*nx);
Acbfx22 = zeros((yy-2), yy*nx);
Acbfu2 = zeros((yy-2), (yy-1)*nu);
for i = 1 : (yy-2)
    Acbfx2(i, (i*nx)+1) = (gamma1-1/dt)*abc(1, (i+1));
    Acbfx2(i, (i*nx)+2) = (gamma1-1/dt)*abc(2, (i+1));
    Acbfx2(i, ((i+1)*nx)+1) = (1/dt)*abc(1, (i+2));
    Acbfx2(i, ((i+1)*nx)+2) = (1/dt)*abc(2, (i+2));
end
for i = 1 : (yy-2)
    Acbfx22(i, (1*nx)+1) = -(1-dt*gamma2)^(i)/dt*abc(1, (1+1));
    Acbfx22(i, (1*nx)+2) = -(1-dt*gamma2)^(i)/dt*abc(2, (1+1));
end
Acbfx2 = Acbfx2 + Acbfx22;
for i = 1 : (yy-2)
    Acbfu2(i, ((i-1)*nu+4)) = - (1 - dt * gamma2)^(i)*(gamma1-1/dt)*(abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
A2cbf = [Acbfx2 Acbfu2];
end
% Get lcbf2
function lcbf2 = getlcbf2(abc, dt, gamma1, gamma2)
[~, yy] = size(abc);
lcbf2 = zeros((yy-2),1);
for i = 1 : (yy-2)
    lcbf2(i, 1) = -abc(3, (i+2))/dt-(gamma1-1/dt)*abc(3, (i+1))+(1 - dt * gamma2)^(i)*abc(3, (1+1))/dt;
end
end


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nick.battista@unc.edu
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "Hill+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us to add a specific muscle model, please let Nick (nick.battista@unc.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: updates the spring attributes!
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function springs_info = update_Springs(dt,current_time,xLag,yLag,springs_info)

%springs_info: col 1: starting spring pt (by lag. discretization)
%              col 2: ending spring pt. (by lag. discretization)
%              col 3: spring stiffness
%              col 4: spring resting lengths

t = current_time;   %CURRENT TIME IN SIM.
ds1 = 3.906250e-03; %PHASE 1 Lag. Spacing
ds2 = 3.342919e-03; %PHASE 2 Lag. Spacing

Nbody = 106;                    %Number of springs before bell connections
RL = give_Resting_Lengths();    %Resting Lengths for Phase 1 (col 1) and Phase 2 (col 2)

% CHANGE RESTING LENGTH BTWN SIDES OF JELLYFISH BELL
for i=1:length( springs_info(:,4) )

    if i <= Nbody
        springs_info(i,4) = ds1 + 5*t*(ds2-ds1);
    else
        ii = i-Nbody;
        springs_info(i,4) = RL(ii,1) + 5*t*( RL(ii,2) - RL(ii,1) );
        %springs_info(107+(i-1),4) = distVec(i)*abs( cos(4*pi*current_time) );
        %springs_info(107+(i-1),4) = distVec(i) * ( 1 - 0.75*( sin(4*pi*current_time) ) );
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: give Resting Lengths (hardcoded!)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function RL = give_Resting_Lengths()

RL = [0.150000000000000   0.075000000000000
   0.149926741000240   0.074973170433545
   0.149706855857594   0.074892624017213
   0.149340020675537   0.074758187724185
   0.148825697410294   0.074569727429823
   0.148163136564709   0.074326763924451
   0.147351380996092   0.074028781527991
   0.146388643571571   0.073675421427274
   0.145274017365354   0.073265752667258
   0.144005152820154   0.072798927606369
   0.142580953464829   0.072274183901744
   0.140998544348621   0.071690309088121
   0.139255611062067   0.071045912762170
   0.137348541189626   0.070339929225114
   0.135274492318472   0.069570469632096
   0.133030516962324   0.068735403317456
   0.130612168717920   0.067832977122889
   0.128014600466564   0.066860709378195
   0.125234180745507   0.065816586316769
   0.122265614832230   0.064697078425658
   0.119103315657513   0.063499067862592
   0.115741451424401   0.062218444447808
   0.112173999448214   0.060850772863983
   0.108392818785359   0.059391311235482
   0.104393528844001   0.057835033327000
   0.100165653754868   0.056174749467196
   0.095702659718569   0.054402747848968
   0.090998237021989   0.052510837712120
   0.086041701055792   0.050487212358191
   0.080827140474999   0.048320369497431
   0.075349280754860   0.045994841644143
   0.069598549422225   0.043493120994599
   0.063573706279126   0.040793235757662
   0.057274941348722   0.037865974506005
   0.050704050274530   0.034676918247010
   0.043870119404512   0.031179908592683
   0.036787309077097   0.027316537892343
   0.029477909226729   0.023009349350403
   0.021975429586313   0.018167707789027
   0.014318966133393   0.012694598507574
   0.006558552552061   0.006558010776142];


function g = tensorKernGradient(kern, x, varargin)

% TENSORKERNGRADIENT Gradient of TENSOR kernel's parameters.
% FORMAT
% DESC computes the gradient of functions with respect to the
% tensor product
% kernel's parameters. As well as the kernel structure and the
% input positions, the user provides a matrix PARTIAL which gives
% the partial derivatives of the function with respect to the
% relevant elements of the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG x : the input locations for which the gradients are being
% computed.
% ARG partial : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The argument takes
% the form of a square matrix of dimension  numData, where numData is
% the number of rows in X.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters. The ordering of the vector should match
% that provided by the function kernExtractParam.
%
% FORMAT
% DESC computes the derivatives as above, but input locations are
% now provided in two matrices associated with rows and columns of
% the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG x1 : the input locations associated with the rows of the
% kernel matrix.
% ARG x2 : the input locations associated with the columns of the
% kernel matrix.
% ARG partial : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The matrix should
% have the same number of rows as X1 and the same number of columns
% as X2 has rows.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters.
%
% SEEALSO tensorKernParamInit, kernGradient, tensorKernDiagGradient, kernGradX
%
% COPYRIGHT : Neil D. Lawrence, 2006

% KERN

% Last of varargin is covGrad.
  g = zeros(1, kern.nParams);
  startVal = 1;
  endVal = 0;
  twoXin = 0;
  if length(varargin) > 1
    twoXin = 1;
    x2 = varargin{1};
    covGrad = varargin{end};
  else
    covGrad = varargin{end};
  end

  for i = 1:length(kern.comp)
    tempKern = tensorKernSlash(kern, i);
    if twoXin
      tempCovGrad = covGrad.*kernCompute(tempKern, x, x2);
    else
      tempCovGrad = covGrad.*kernCompute(tempKern, x);
  end
  endVal = endVal + kern.comp{i}.nParams;
  if ~isempty(kern.comp{i}.index)
    % only part of the data is involved in the kernel.
    if ~twoXin
      g(1, startVal:endVal)  = kernGradient(kern.comp{i}, ...
                                            x(:, kern.comp{i}.index), ...
                                            tempCovGrad);
    else
      g(1, startVal:endVal) = kernGradient(kern.comp{i}, ...
                                           x(:, kern.comp{i}.index), ...
                                           x2(:, kern.comp{i}.index), ...
                                           tempCovGrad);
    end
  else
    if ~twoXin
      % all the data is involved with the kernel.
      g(1, startVal:endVal)  = kernGradient(kern.comp{i}, x, ...
                                            tempCovGrad);
    else
      g(1, startVal:endVal) = kernGradient(kern.comp{i}, x, x2, ...
                                           tempCovGrad);
    end
  end
  startVal = endVal + 1;
end
g = g*kern.paramGroups;


% sim_press_shaped.m
% Robin Simpson and Jamie Near, 2014.
%
% USAGE:
% out = sim_press_shaped(n,sw,Bfield,linewidth,sys,tau1,tau2,RF,tp,dx,dy,Gx,Gy,flipAngle,centreFreq)
%
% DESCRIPTION:
% This function simulates the PRESS experiment.  The excitation is
% simulated as an instantaneous rotation, and the refocusing pulses are
% simulated as shaped rotations.
%
% It employs coherence selection to only include desired coherence orders
% of the spin system being simulated. For the PRESS experiment, the desired
% coherence after the excitation pulse is -1, +1 after the first refocusing
% pulse and -1 after the final refocusing pulse.
%
% Finally, this code simulates the spectrum at a given point in space (x,y),
% given the values of the slice selection gradients (Gx, and Gy).  The pulse
% waveform is assumed to be the same for both refocusing pulses.  In order
% to fully simulate the MEGA-PRESS experiment, you have to run this
% simulation many times at various points in space (x,y), and then add
% together the resulting spectra.
%
% INPUTS:
% n         = number of points in fid/spectrum
% sw        = desired spectral width in [Hz]
% Bfield    = main magnetic field strength in [T]
% linewidth = linewidth in [Hz]
% sys       = spin system definition structure
% tau1      = echo time 1 in [ms].
% tau2      = echo time 2 in [ms].
% RF        = RF pulse definition structure for refoc pulses (obtain using 'io_loadRFwaveform.m')
% tp        = RF pulse duration in [ms]
% dx        = position offset in x-direction (corresponding to first refocusing pulse) [cm]
% dy        = position offset in y-direction (corresponding to second refocusing pulse) [cm]
% Gx        = gradient strength for first selective refocusing pulse [G/cm]
% Gy        = gradient strength for second selective refocusing pulse [G/cm]
% flipAngle = flip angle of refocusing pulses [degrees] (Optional.  Default = 180 deg)
% centreFreq= centre frequency of the spectrum in [ppm] (Optional.  Default = 2.3)
%
% OUTPUTS:
% out       = simulated spectrum, in FID-A structure format, using PRESS
%             sequence.

function out = sim_press_shaped(n,sw,Bfield,linewidth,sys,tau1,tau2,RF,tp,dx,dy,Gx,Gy,flipAngle,centreFreq)

if nargin<15
    centreFreq=2.3;
    if nargin<14
        flipAngle=180;
    end
end

if tau1<tp/1000
    error('ERROR:  Echo-time 1 cannot be less than duration of refocusing pulse! ABORTING!!');
end
if tau2<tp/1000
    error('ERROR:  Echo-time 2 cannot be less than duration of refocusing pulse! ABORTING!!');
end

%Set water to centre
for k=1:length(sys)
    sys(k).shifts=sys(k).shifts-centreFreq;
end

%Calculate Hamiltonian matrices and starting density matrix.
[H,d]=sim_Hamiltonian(sys,Bfield);

%Calculate new delays by subtracting the pulse duration from tau1 and tau2;
delays=zeros(2);
delays(1)=tau1-tp;
delays(2)=tau2-tp;
if sum(delays<0)
    error(['ERROR! The following taus are too short: ' num2str(find(delays<0)) '.']);
end

%BEGIN PULSE SEQUENCE************
d=sim_excite(d,H,'x');                                    %EXCITE
d=sim_COF(H,d,-1);                       %Keep only -1 coherences
d=sim_evolve(d,H,delays(1)/2000);                         %Evolve by delays(1)/2
d=sim_shapedRF(d,H,RF,tp,flipAngle,90,dx,Gx);             %1st shaped 180 degree refocusing pulse
d=sim_COF(H,d,1);                        %Keep only +1 coherences
d=sim_evolve(d,H,(delays(1)+delays(2))/2000);             %Evolve by (delays(1)+delays(2))/2
d=sim_shapedRF(d,H,RF,tp,flipAngle,90,dy,Gy);             %2nd shaped 180 degree refocusing pulse
d=sim_COF(H,d,-1);                       %Keep only -1 coherences
d=sim_evolve(d,H,delays(2)/2000);                         %Evolve by delays(2)/2
[out,dout]=sim_readout(d,H,n,sw,linewidth,90);            %Readout along y (90 degree phase);
%END PULSE SEQUENCE**************

%Correct the ppm scale:
out.ppm=out.ppm-(4.65-centreFreq);

%Fill in structure header fields:
out.seq='press';
out.te=tau1+tau2;
out.sim='shaped';

%Additional fields for compatibility with FID-A processing tools.
out.sz=size(out.specs);
out.date=date;
out.dims.t=1;
out.dims.coils=0;
out.dims.averages=0;
out.dims.subSpecs=0;
out.dims.extras=0;
out.averages=1;
out.rawAverages=1;
out.subspecs=1;
out.rawSubspecs=1;
out.flags.writtentostruct=1;
out.flags.gotparams=1;
out.flags.leftshifted=0;
out.flags.filtered=0;
out.flags.zeropadded=0;
out.flags.freqcorrected=0;
out.flags.phasecorrected=0;
out.flags.averaged=1;
out.flags.addedrcvrs=1;
out.flags.subtracted=1;
out.flags.writtentotext=0;
out.flags.downsampled=0;
out.flags.isFourSteps=0;


classdef prtRvUniform < prtRv
    % prtRvUniform  Uniform random variable
    %
    %   RV = prtRvUniform creates a prtRvUniform object with empty
    %   upperBounds and lowerBounds. The upperBounds and lowerBounds must
    %   be set either directly, or by calling the MLE method. upperBounds
    %   and lowerBounds specify the range of the uniform variable.
    %
    %   RV = prtRvUniform(PROPERTY1, VALUE1,...) creates a prtRvUniform
    %   object RV with properties as specified by PROPERTY/VALUE pairs.
    %
    %   A prtRvUniform object inherits all properties from the prtRv class.
    %   In addition, it has the following properties:
    %
    %   upperBounds - 1 x nDims double vector specifying the upper bound of
    %                 the region with uniform density
    %   lowerBounds - 1 x nDims double vector specifying the lower bound of
    %                 the region with uniform density
    %
    %  A prtRvUniform object inherits all methods from the prtRv class.
    %  The MLE  method can be used to estimate the distribution parameters
    %  from data.
    %
    %  Example:
    %
    %  dataSet = prtDataGenUnimodal;        % Load a dataset consisting of
    %                                       % 2 features
    %  dataSet = retainFeatures(dataSet,1); % Retain only the first feature
    %
    %  RV = prtRvUniform;                   % Create a prtRvUniform object
    %  RV = RV.mle(dataSet);                % Compute the bounds
    %                                       % form the data
    %  RV.plotPdf                           % Plot the pdf
    %
    %   See also: prtRv, prtRvMvn, prtRvGmm, prtRvMultinomial,
    %   prtRvVq, prtRvKde

    properties (SetAccess = private)
        name = 'Uniform Random Variable'
        nameAbbreviation = 'RVUnif';
    end

    properties (SetAccess = protected)
        isSupervised = false;
        isCrossValidateValid = true;
    end

    properties
        upperBounds  % The lower bounds of the random variable
        lowerBounds  % The upper bounds of the random variable
    end

    properties (Hidden = true, Dependent = true)
        nDimensions
    end

    methods
        % The Constructor
        function R = prtRvUniform(varargin)
            R = constructorInputParse(R,varargin{:});
        end

        function R = mle(R,X)
            X = R.dataInputParse(X); % Basic error checking etc
            R.upperBounds = max(X,[],1);
            R.lowerBounds = min(X,[],1);
        end

        function vals = pdf(R,X)

            [isValid, reasonStr] = R.isValid;
            assert(isValid,'PDF cannot yet be evaluated. This RV is not yet valid %s.',reasonStr);

            X = R.dataInputParse(X); % Basic error checking etc
            assert(size(X,2) == R.nDimensions,'Data, RV dimensionality missmatch. Input data, X, has dimensionality %d and this RV has dimensionality %d.', size(X,2), R.nDimensions)

            isIn = true(size(X,1),1);
            for iDim = 1:size(X,2)
                isIn = isIn & X(:,iDim) >= R.lowerBounds(iDim) & X(:,iDim) <= R.upperBounds(iDim);
            end

            vals = zeros(size(X,1),1);
            vals(isIn) = 1/R.area();
        end

        function vals = logPdf(R,X)
            [isValid, reasonStr] = R.isValid;
            assert(isValid,'LOGPDF cannot yet be evaluated. This RV is not yet valid %s.',reasonStr);

            vals = log(pdf(R,X));
        end

        function vals = cdf(R,X)
            X = R.dataInputParse(X); % Basic error checking etc
            assert(size(X,2) == R.nDimensions,'Data, RV dimensionality missmatch. Input data, X, has dimensionality %d and this RV has dimensionality %d.', size(X,2), R.nDimensions)
            vals = prod(max(bsxfun(@minus,bsxfun(@min,X,R.upperBounds),R.lowerBounds),0),2)./R.area();
        end

        function a = area(R)
            a = prod(R.upperBounds - R.lowerBounds);
        end

        function vals = draw(R,N)
            if nargin < 2
                N = 1;
            end

            assert(numel(N)==1 && N==floor(N) && N > 0,'N must be a positive integer scalar.')

            [isValid, reasonStr] = R.isValid;
            assert(isValid,'DRAW cannot yet be evaluated. This RV is not yet valid %s.',reasonStr);

            vals = bsxfun(@plus,bsxfun(@times,rand(N,R.nDimensions),R.upperBounds - R.lowerBounds),R.lowerBounds);
        end

        function val = get.nDimensions(R)
            val = length(R.upperBounds);
        end
    end

    methods (Hidden = true)
        function [val, reasonStr] = isValid(R)
            if numel(R) > 1
                val = false(size(R));
                for iR = 1:numel(R)
                    [val(iR), reasonStr] = isValid(R(iR));
                end
                return
            end

            badOrder = ~all(R.upperBounds>R.lowerBounds);

            val = ~isempty(R.upperBounds) && ~isempty(R.lowerBounds) && ~badOrder;

            if val
                reasonStr = '';
            else
                badUpper = isempty(R.upperBounds);
                badLower = isempty(R.lowerBounds);

                if badUpper && ~badLower
                    reasonStr = 'because upperBounds has not been set';
                elseif ~badUpper && badLower
                    reasonStr = 'because lowerBounds has not been set';
                elseif badUpper && badLower
                    reasonStr = 'because upperBounds and lowerBounds have not been set';
                elseif badOrder
                    reasonStr = 'because at least one entry of lowerBounds is greater than the corresponding entry of upperBounds';
                else
                    reasonStr = 'because of an unknown reason';
                end
            end
        end

        function val = plotLimits(R)
            [isValid, reasonStr] = R.isValid;
            if isValid

                range = R.upperBounds-R.lowerBounds;

                val = zeros(2*R.nDimensions,1);
                val(1:2:end) = R.lowerBounds-range/10;
                val(2:2:end) = R.upperBounds+range/10;

            else
                error('prtRvUniform:plotLimits','Plotting limits can not be determined for this RV. It is not yet valid %s.',reasonStr)
            end
        end
    end
end


function [rtk,stat0]=relpos(rtk,obsr,obsb,nav)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%% relative positioning %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%input:  rtk - rtk control struct
%        obsr - rover observations
%        obsb - base observations
%        nav - navigation message
%output: rtk - rtk control struct
%        stat0 - state (0:error 1:ok)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%relative positioning include PPD, PPK and PPS
%PPD:post-processing differenced, based on double-differenced PR
%PPK:post-processing kinematic, based on double-differenced CP and PR
%PPS:post-processing static, based on double-differenced CP and PR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global glc
opt=rtk.opt; nf=rtk.NF; dt=timediff(obsr(1).time,obsb(1).time);
nobsr=size(obsr,1); nobsb=size(obsb,1);

if rtk.opt.mode<=glc.PMODE_DGNSS
    stat=glc.SOLQ_DGNSS;
else
    stat=glc.SOLQ_FLOAT;
end

for i=1:glc.MAXSAT
    [sys,~]=satsys(i);
    rtk.sat(i).sys=sys;
    for j=1:glc.NFREQ
        rtk.sat(i).vsat(j)=0;
    end
    for j=1:glc.NFREQ
        rtk.sat(i).snr(j)=0;
    end
end

svr=satposs(obsr,nav,opt.sateph);
svb=satposs(obsb,nav,opt.sateph);

% zero-differenced residuals for base
[zdb,stat_tmp]=zdres(2,obsb,svb,nav,rtk.basepos,opt,rtk);
if stat_tmp==0,stat0=0;return;end

ind=selcomsat(obsr,obsb,opt,zdb);
if ind.ns==0,stat0=0;return;end

% debug tracing
% fprintf('\n\n\n');
% fprintf('time=%d\n',obsr(1).time.time);
% fprintf('before time update\n');
% printx(rtk.x,rtk);
% printP(rtk.P,rtk);

% time update
rtk=udstate(rtk,obsr,obsb,nav,ind);

% debug tracing
% fprintf('after time update\n');
% printx(rtk.x,rtk);
% printP(rtk.P,rtk);

xp=rtk.x; Pp=zeros(rtk.nx,rtk.nx);
for i=1:opt.niter

    % zero-differenced residuals for rover
    [zdr,stat_tmp]=zdres(1,obsr,svr,nav,xp(1:3),opt,rtk);
    if stat_tmp==0,stat=glc.SOLQ_NONE;break;end

    % double-differenced residuals,measurement matrix and noise matrix
    [rtk,v,H,R,nv,~]=ddres(rtk,nav,dt,xp,Pp,zdr,zdb,ind);
    if nv<=0,stat=glc.SOLQ_NONE;break;end

    % debug tracing
%     printv(v);
%     printH(H);
%     printR(R);

    % measurement update
    Pp=rtk.P;
    [xp,Pp,stat_tmp]=Kfilter_h(v,H,R,xp,Pp);
    if stat_tmp==0
        [week,sow]=time2gpst(obs(1).time);
        fprintf('Warning:GPS week = %d sow = %.3f,filter error!\n',week,sow);
        stat=glc.SOLQ_NONE;
        break;
    end

    % debug tracing
%     fprintf('after measurement update\n');
%     printx(xp,rtk);
%     printP(Pp,rtk);

end

if stat~=glc.SOLQ_NONE
    % zero-differenced residuals for rover
    [zdr,stat_tmp]=zdres(1,obsr,svr,nav,xp(1:3),opt,rtk);
    if stat_tmp~=0
        % post-fit residuals for float solution
        [rtk,v,~,R,nv,vflag]=ddres(rtk,nav,dt,xp,Pp,zdr,zdb,ind);
        % validation of float solution
        if valpos_rel(rtk,v,R,vflag,nv,4)
            rtk.x=xp; rtk.P=Pp;
            rtk.sol.ns=0;
            for i=1:ind.ns
                for f=1:nf
                    sati=ind.sat(i);
                    if ~rtk.sat(sati).vsat(f),continue;end
                    rtk.sat(sati).lock(f)=rtk.sat(sati).lock(f)+1;
                    rtk.sat(sati).outc(f)=0;
                    if f==1,rtk.sol.ns=rtk.sol.ns+1;end
                end
            end
            if rtk.sol.ns<4,stat=glc.SOLQ_NONE;end
        else
            stat=glc.SOLQ_NONE;
        end
    end
end

% ambiguity resolution
if stat~=glc.SOLQ_NONE
    [rtk,~,xa,nb]=resamb(rtk);
    if nb>1
        % zero-differenced residuals for rover
        [zdr,stat_tmp]=zdres(1,obsr,svr,nav,xa(1:3),opt,rtk);
        if stat_tmp~=0
            % post-fit reisiduals for fixed solution
            [rtk,v,~,R,nv,vflag]=ddres(rtk,nav,dt,xa,Pp,zdr,zdb,ind);
            % validation of fixed solution
            if valpos_rel(rtk,v,R,vflag,nv,4)
                rtk.nfix=rtk.nfix+1;
                if rtk.nfix>=opt.minfix&&rtk.opt.modear==glc.ARMODE_FIXHOLD
                    % hold integer ambiguity
                    rtk=holdamb(rtk,xa);
                end
                stat=glc.SOLQ_FIX;
            end
        end
    end
end

% save solution status
if stat==glc.SOLQ_FIX
    rtk.sol.pos=rtk.xa(1:3)';
    rtk.sol.posP(1)=rtk.Pa(1,1); rtk.sol.posP(2)=rtk.Pa(2,2);
    rtk.sol.posP(3)=rtk.Pa(3,3); rtk.sol.posP(4)=rtk.Pa(1,2);
    rtk.sol.posP(5)=rtk.Pa(2,3); rtk.sol.posP(6)=rtk.Pa(1,3);
    if rtk.opt.dynamics==1
        rtk.sol.vel=rtk.xa(4:6)';
        rtk.sol.velP(1)=rtk.Pa(4,4); rtk.sol.velP(2)=rtk.Pa(5,5);
        rtk.sol.velP(3)=rtk.Pa(6,6); rtk.sol.velP(4)=rtk.Pa(4,5);
        rtk.sol.velP(5)=rtk.Pa(5,6); rtk.sol.velP(6)=rtk.Pa(4,6);
    end
else
    rtk.sol.pos=rtk.x(1:3)';
    rtk.sol.posP(1)=rtk.P(1,1); rtk.sol.posP(2)=rtk.P(2,2);
    rtk.sol.posP(3)=rtk.P(3,3); rtk.sol.posP(4)=rtk.P(1,2);
    rtk.sol.posP(5)=rtk.P(2,3); rtk.sol.posP(6)=rtk.P(1,3);
    if rtk.opt.dynamics==1
        rtk.sol.vel=rtk.xa(4:6)';
        rtk.sol.velP(1)=rtk.P(4,4); rtk.sol.velP(2)=rtk.P(5,5);
        rtk.sol.velP(3)=rtk.P(6,6); rtk.sol.velP(4)=rtk.P(4,5);
        rtk.sol.velP(5)=rtk.P(5,6); rtk.sol.velP(6)=rtk.P(4,6);
    end
    rtk.nfix=0;
end

for i=1:nobsr
    for j=1:nf
        if obsr(i).L(j)==0,continue;end
        rtk.sat(obsr(i).sat).pt(1,j)=obsr(i).time;
        rtk.sat(obsr(i).sat).ph(1,j)=obsr(i).L(j);
    end
end
for i=1:nobsb
    for j=1:nf
        if obsb(i).L(j)==0,continue;end
        rtk.sat(obsb(i).sat).pt(2,j)=obsb(i).time;
        rtk.sat(obsb(i).sat).ph(2,j)=obsb(i).L(j);
    end
end

for i=1:ind.ns
    for j=1:nf
        rtk.sat(ind.sat(i)).snr(j)=obsr(ind.ir(i)).S(j);
    end
end

for i=1:glc.MAXSAT
    for j=1:nf
        if rtk.sat(i).fix(j)==2&&stat~=glc.SOLQ_FIX
            rtk.sat(i).fix(j)=1;
        end
        if bitand(rtk.sat(i).slip(j),1)
            rtk.sat(i).slipc(j)=rtk.sat(i).slipc(j)+1;
        end
    end
end

if stat~=glc.SOLQ_NONE,rtk.sol.stat=stat;end

stat0=(stat~=glc.SOLQ_NONE);

return


function varargout = MDOF_vibration_calculator3(varargin)
% MDOF_VIBRATION_CALCULATOR3 M-file for MDOF_vibration_calculator3.fig
%      MDOF_VIBRATION_CALCULATOR3, by itself, creates a new MDOF_VIBRATION_CALCULATOR3 or raises the existing
%      singleton*.
%
%      H = MDOF_VIBRATION_CALCULATOR3 returns the handle to a new MDOF_VIBRATION_CALCULATOR3 or the handle to
%      the existing singleton*.
%
%      MDOF_VIBRATION_CALCULATOR3('CALLBACK',hObject,eventData,handles,...) calls the local
%      function named CALLBACK in MDOF_VIBRATION_CALCULATOR3.M with the given input arguments.
%
%      MDOF_VIBRATION_CALCULATOR3('Property','Value',...) creates a new MDOF_VIBRATION_CALCULATOR3 or raises the
%      existing singleton*.  Starting from the left, property value pairs are
%      applied to the GUI before MDOF_vibration_calculator3_OpeningFcn gets called.  An
%      unrecognized property name or invalid value makes property application
%      stop.  All inputs are passed to MDOF_vibration_calculator3_OpeningFcn via varargin.
%
%      *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one
%      instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES

% Edit the above text to modify the response to help MDOF_vibration_calculator3

% Last Modified by GUIDE v2.5 11-Oct-2011 22:48:12

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @MDOF_vibration_calculator3_OpeningFcn, ...
                   'gui_OutputFcn',  @MDOF_vibration_calculator3_OutputFcn, ...
                   'gui_LayoutFcn',  [] , ...
                   'gui_Callback',   []);
if nargin && ischar(varargin{1})
    gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
    gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT

% --- Executes just before MDOF_vibration_calculator3 is made visible.
function MDOF_vibration_calculator3_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% varargin   command line arguments to MDOF_vibration_calculator3 (see VARARGIN)

% Choose default command line output for MDOF_vibration_calculator3
handles.output = hObject;

% Update handles structure
guidata(hObject, handles);
global CC
CC = 0;
initial_function(hObject, eventdata, handles)
% UIWAIT makes MDOF_vibration_calculator3 wait for user response (see UIRESUME)
% uiwait(handles.figure1);

% --- Outputs from this function are returned to the command line.
function varargout = MDOF_vibration_calculator3_OutputFcn(hObject, eventdata, handles)
% varargout  cell array for returning output args (see VARARGOUT);
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
varargout{1} = handles.output;

% --- Executes on button press in pushbutton1.
function pushbutton1_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
clc

AcquiringData(hObject, eventdata, handles);
Calculating_Modal_Equation(hObject, eventdata, handles)
[z,wn,f0,wdr,x0,v0] = Definding_Modal_Equa_values(hObject, eventdata, handles);
Displaying_Modal_Equation(z,wn,f0,wdr,x0,v0,hObject, eventdata, handles);
Calculation_XDisplacement(hObject, eventdata, handles);
% Displaying_XDisplacement(hObject, eventdata, handles)
Plotting_XT(hObject, eventdata, handles)
Plotting_RT(hObject, eventdata, handles)

function Animation_2mass(hObject, eventdata, handles)
global XT t CC

axes(handles.axes1);    cla(handles.axes1);
set(handles.pushbutton3, 'string','Stop');
    if CC == 0
        CC = 1;
    else
        CC = 0;
    end
posx=[0.5 -0.5 -0.5 0.5];
posy=[0.5 0.5 -0.5 -0.5];
h1=line(0,0);

line([-5 10],[-0.5 -0.5],'color','k','linewidth',2);%base line
line([0 0],[-0.5 5],'color','k','linewidth',2); %a wall on left
block1=patch(posx,posy,'y');
block2=patch(posx,posy,'g');

axes(handles.axes1);
axis([0 10 -1 1]);
Ratio = 1/max(max(abs(XT)));
xini = [3; 6];
for i = 1 : length(t)
            if ishandle(h1) == 0 || CC  == 0
                set(handles.pushbutton3, 'string','Play');
                cla(handles.axes1);
                break
            else
                Movx1 = xini(1) + Ratio * XT(1,i);
                Movx2 = xini(2) + Ratio * XT(2,i);
                set(h1,'xdata',[0 Movx1-0.5 Movx2-0.5],'ydata',[0 0 0]);
                set(block1,'xdata', posx + Movx1, 'ydata', posy);
                set(block2,'xdata', posx + Movx2, 'ydata', posy);

               pause(0.015);
            end
end
CC = 0;
set(handles.pushbutton3, 'string','Play');

% --- Executes on button press in pushbutton3.
function pushbutton3_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton3 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
Animation_2mass(hObject, eventdata, handles)

function Plotting_RT(hObject, eventdata, handles)
global t RT
plot(handles.axes2,t, RT);legend(handles.axes2,'r_1(t)','r_2(t)');
xlabel(handles.axes2,'Time, t'); ylabel(handles.axes2,'Displacement')

function Plotting_XT(hObject, eventdata, handles)
global t XT
plot(handles.axes3,t, XT);legend(handles.axes3,'x_1(t)','x_2(t)');
xlabel(handles.axes3,'Time, t'); ylabel(handles.axes3,'Displacement')

% function Displaying_XDisplacement(hObject, eventdata, handles)
% global S func
%
% switch func
%     case 1
%             R1 = get(handles.text10,'string');
%             R2 = get(handles.text11,'string');
%             R1a = str2num(R1(11:15)); R1b = R1(16:35);
%             R2a = str2num(R2(11:15)); R2b = R2(16:35);
%
%             X11 = S(1,1)*R1a;       X12 = S(1,2)*R2a;
%             X21 = S(2,1)*R1a;       X22 = S(2,2)*R2a;
%
%             set(handles.text14, 'string',['']);
%     case 2
%         set(handles.text14,'string','  Sorry ');
%         set(handles.text15,'string','  Sorry ');
%     case 3
%         set(handles.text14,'string','  Sorry ');
%         set(handles.text15,'string','  Sorry ');
% end

% set(handles.text14,'string','')
% set(handles.text15,'string','')

function Calculation_XDisplacement(hObject, eventdata, handles)
global RT S XT
XT = S*RT;

function Displaying_Modal_Equation(z,wn,f0,wdr,x0,v0, hObject, eventdata, handles)
global M t func RT S
zz = z*0.5./wn; wwn = wn; wwdr = wdr; xx0 = x0; vv0 = v0; ff0 = f0;
RT = zeros(length(M),length(t));

for i = 1 : length(M)
    z = zz(i);    wn = wwn(i);     wdr = wwdr(i);
    x0 = xx0(i);     v0 = vv0(i);     f0 = ff0(i);
%     z,wn,wdr,x0,v0,f0

    switch func
        case 1      % func 1 : F0 = 0
                    if z >= 1
                        warndlg('This is not underdamp system because ...the damping ratio is larger than 1. Please insert the value correctly.');
                        %XT = 0*t;
                    else
                        wd = wn*sqrt(1-z^2);
                        X     = f0/sqrt((wn^2 - wdr^2)^2 + (2*z*wn*wdr)^2);
                        Theta = atan(2*z*wn*wdr/(wn^2 - wdr^2));
                        Phi = atan((wd*(x0 - X*cos(Theta)))/(v0 + (x0 - X*cos(Theta))*z*wn - wdr*X*sin(Theta)));
                        A     = (x0 - X*cos(Theta))/sin(Phi);
                        RT(i,:) = A*exp(-z*wn*t).*sin(wd*t + Phi) + X*cos(wdr*t - Theta);
                        if i == 1
                            if z == 0
                                set(handles.text10, 'string',['  r1(t) = ',sprintf('%4.3f',A),...
                            ' sin(',sprintf('%4.3f',wd),...
                            't + ',sprintf('%4.3f',Phi),')']);
                            else
                            set(handles.text10, 'string',['  r1(t) = ',sprintf('%4.3f',A),...
                            'exp(',sprintf('%4.3f',-z*wn),'t) sin(',sprintf('%4.3f',wd),...
                            't + ',sprintf('%4.3f',Phi),')']);
                            end
                        else
                            if z == 0
                                set(handles.text11, 'string',['  r2(t) = ',sprintf('%4.3f',A),...
                            ' sin(',sprintf('%4.3f',wd),...
                            't + ',sprintf('%4.3f',Phi),')']);
                            else
                            set(handles.text11, 'string',['  r2(t) = ',sprintf('%4.3f',A),...
                            'exp(',sprintf('%4.3f',-z*wn),'t) sin(',sprintf('%4.3f',wd),...
                            't + ',sprintf('%4.3f',Phi),')']);
                            end
                        end
                    end
        case 2      % func 1 : F0 cos wt
                    wd = wn*sqrt(1-z^2);

                    X     = f0/sqrt((wn^2 - wdr^2)^2 + (2*z*wn*wdr)^2);
                    Theta = atan(2*z*wn*wdr/(wn^2 - wdr^2));
                    Phi = atan((wd*(x0 - X*cos(Theta)))/(v0 + (x0 - X*cos(Theta))*z*wn - wdr*X*sin(Theta)));
                    A     = (x0 - X*cos(Theta))/sin(Phi);
                    RT(i,:) = A*exp(-z*wn*t).*sin(wd*t + Phi) + X*cos(wdr*t - Theta);
                    if i ==1
                        if z == 0
                            set(handles.text10, 'string',['  r1(t) = ',sprintf('%4.3f',A),...
                            ' sin(', sprintf('%4.3f',wd),'t + ',...
                            sprintf('%4.3f',Phi),') + ',sprintf('%4.3f',X),'cos(',sprintf('%4.3f',wdr),...
                            't - ',sprintf('%4.3f',Theta),')']);
                        else
                            set(handles.text10, 'string',['  r1(t) = ',sprintf('%4.3f',S(1,1)*A),'exp(',...
                            sprintf('%4.3f',-z*wn),'t) sin(', sprintf('%4.3f',wd),'t + ',...
                            sprintf('%4.3f',Phi),') + ',sprintf('%4.3f',X),'cos(',sprintf('%4.3f',wdr),...
                            't - ',sprintf('%4.3f',Theta),')']);
                         end
                    else
                        if z == 0
                             set(handles.text11, 'string',['  r2(t) = ',sprintf('%4.3f',A),...
                            ' sin(', sprintf('%4.3f',wd),'t + ',...
                            sprintf('%4.3f',Phi),') + ',sprintf('%4.3f',X),'cos(',sprintf('%4.3f',wdr),...
                            't - ',sprintf('%4.3f',Theta),')']);
                        else
                            set(handles.text11, 'string',['  r2(t) = ',sprintf('%4.3f',A),'exp(',...
                            sprintf('%4.3f',-z*wn),'t) sin(', sprintf('%4.3f',wd),'t + ',...
                            sprintf('%4.3f',Phi),') + ',sprintf('%4.3f',X),'cos(',sprintf('%4.3f',wdr),...
                            't - ',sprintf('%4.3f',Theta),')']);
                        end
                    end

        case 3      % func 1 : F0 sin wt
                    wd    = wn*sqrt(1-z^2);
                    T     = f0/sqrt((wn^2 - wdr^2)^2 + (2*z*wn*wdr)^2);
                    X     = f0/((wn^2 - wdr^2)^2 + (2*z*wn*wdr)^2);
                    Theta = atan(2*z*wn*wdr/(wn^2 - wdr^2));
                    Phi   = atan((wd*(x0 + X*cos(Theta)))/(v0 + (x0 + X*cos(Theta))*z*wn + wdr*X*sin(Theta)));
                    A     = (x0 + X*cos(Theta))/sin(Phi);
                    RT(i,:)    = A*exp(-z*wn*t).*sin(wd*t + Phi) + T*sin(wdr*t + Theta);
                    if i == 1
                        if z == 0
                            set(handles.text10, 'string',['  r1(t) = ',sprintf('%4.3f',A),...
                            ' sin(', sprintf('%4.3f',wd),'t + ',...
                            sprintf('%4.3f',Phi),') + ',sprintf('%4.3f',T),'sin(',sprintf('%4.3f',wdr),...
                            't + ',sprintf('%4.3f',Theta),')']);
                        else
                         set(handles.text10, 'string',['  r1(t) = ',sprintf('%4.3f',A),'exp(',...
                            sprintf('%4.3f',-z*wn),'t) sin(', sprintf('%4.3f',wd),'t + ',...
                            sprintf('%4.3f',Phi),') + ',sprintf('%4.3f',T),'sin(',sprintf('%4.3f',wdr),...
                            't + ',sprintf('%4.3f',Theta),')']);
                        end
                    else
                        if z == 0
                            set(handles.text11, 'string',['  r2(t) = ',sprintf('%4.3f',A),' sin(', sprintf('%4.3f',wd),'t + ',...
                            sprintf('%4.3f',Phi),') + ',sprintf('%4.3f',T),'sin(',sprintf('%4.3f',wdr),...
                            't + ',sprintf('%4.3f',Theta),')']);
                        else
                        set(handles.text11, 'string',['  r2(t) = ',sprintf('%4.3f',A),'exp(',...
                            sprintf('%4.3f',-z*wn),'t) sin(', sprintf('%4.3f',wd),'t + ',...
                            sprintf('%4.3f',Phi),') + ',sprintf('%4.3f',T),'sin(',sprintf('%4.3f',wdr),...
                            't + ',sprintf('%4.3f',Theta),')']);
                        end
                    end
    end
end

function [z,wn,f0,wdr,x0,v0] = Definding_Modal_Equa_values(hObject, eventdata, handles)

global M B F0 w wn
global  PCMP PKMP  S Sinv

x0 = str2num(get(handles.edit9,'string'));
v0 = str2num(get(handles.edit10,'string'));

% x0, v0,
% Intial value of modal equation
r0 = Sinv*x0;       x0 = r0;
rdot0 = Sinv*v0;    v0 = rdot0;
zz = diag(PCMP);

% Define each of modal equation value

for i = 1 : length(M)
    z(i) = zz(i);
    wn(i) = sqrtm(PKMP(i,i));
    FF = S'*B.*F0;
    f0(i) = FF(i,2);
    wdr(i) = w;
end
z = z'; f0 = f0'; wdr = wdr';
%  r0, rdot0, f0, F0,z,wn,wdr

function Calculating_Modal_Equation(hObject, eventdata, handles)

global M C K
global CM PCMP KM PKMP U S Sinv wn

Minvsquare = inv(chol(M));
KM = Minvsquare*K*Minvsquare;
CM = Minvsquare*C*Minvsquare;
U = chol(M);
[P,lam] = eig(U'\K/U);
[w,k]   = sort(sqrt(diag(lam)));
P = P(:,k);
    for i = 1:length(M)
      if P(1,i) < 0
         P(:,i) = -P(:,i);
      end
    end
PKMP = P'*KM*P;
PCMP = P'*CM*P;
S = U\P;    Sinv = inv(S);  wn = w;
% S,Sinv,U,PKMP,PCMP, wn

function AcquiringData(hObject, eventdata, handles)

global M C K B F0 w t x0 v0

M = str2num(get(handles.edit1,'string'));
C = str2num(get(handles.edit2,'string'));
K = str2num(get(handles.edit3,'string'));
B = str2num(get(handles.edit4,'string'));
F0 = str2num(get(handles.edit5,'string'));
w  = str2num(get(handles.edit6,'string'));
t0 = str2num(get(handles.edit7,'string'));
tf = str2num(get(handles.edit8,'string'));
x0 = str2num(get(handles.edit9,'string'));
v0 = str2num(get(handles.edit10,'string'));

t  = t0:0.05:tf;
if size(C) ~= size(M)
    warndlg('Please check matrix size of M, C, K')
end

% M, C, K, B, F0, w, t, x0, v0

% --- Executes on selection change in popupmenu1.
function popupmenu1_Callback(hObject, eventdata, handles)
% hObject    handle to popupmenu1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: contents = cellstr(get(hObject,'String')) returns popupmenu1 contents as cell array
%        contents{get(hObject,'Value')} returns selected item from popupmenu1
global func

switch get(hObject,'Value')
    case 1
            func = 1;
            set(handles.edit5,'string',0);set(handles.edit5,'enable','off');
            set(handles.edit6,'string',0);set(handles.edit6,'enable','off');
    case 2
            func = 2;
            set(handles.edit5,'enable','on');
            set(handles.edit6,'enable','on');
    case 3
            func = 3;
            set(handles.edit5,'enable','on');
            set(handles.edit6,'enable','on');
end

function initial_function(hObject, eventdata, handles)
global func
        func = 1;
        set(handles.edit5,'string',0);set(handles.edit5,'enable','off');
        set(handles.edit6,'string',0);set(handles.edit6,'enable','off');
        set(handles.popupmenu1,'value',1);
        set(handles.axes1,'visible','off');

% --- Executes on button press in pushbutton2.
function pushbutton2_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global func

set(handles.edit1,'string','[3 0 ; 0 1]');
set(handles.edit2,'string','[0 0 ; 0 0]');
set(handles.edit3,'string','[3 -1 ; -1 3]');
set(handles.edit4,'string','[0 0 ; 0 1]');
set(handles.edit5,'string','1');set(handles.edit5,'enable','on');
set(handles.edit6,'string','1');set(handles.edit6,'enable','on');
set(handles.edit9,'string','[1;0]');
set(handles.edit10,'string','[0;0]');
set(handles.popupmenu1,'value',1);
 func = 1;

function edit1_Callback(hObject, eventdata, handles)
% hObject    handle to edit1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit1 as text
%        str2double(get(hObject,'String')) returns contents of edit1 as a double

% --- Executes during object creation, after setting all properties.
function edit1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit2_Callback(hObject, eventdata, handles)
% hObject    handle to edit2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit2 as text
%        str2double(get(hObject,'String')) returns contents of edit2 as a double

% --- Executes during object creation, after setting all properties.
function edit2_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit3_Callback(hObject, eventdata, handles)
% hObject    handle to edit3 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit3 as text
%        str2double(get(hObject,'String')) returns contents of edit3 as a double

% --- Executes during object creation, after setting all properties.
function edit3_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit3 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit4_Callback(hObject, eventdata, handles)
% hObject    handle to edit4 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit4 as text
%        str2double(get(hObject,'String')) returns contents of edit4 as a double

% --- Executes during object creation, after setting all properties.
function edit4_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit4 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

% --- Executes during object creation, after setting all properties.
function popupmenu1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to popupmenu1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: popupmenu controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit5_Callback(hObject, eventdata, handles)
% hObject    handle to edit5 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit5 as text
%        str2double(get(hObject,'String')) returns contents of edit5 as a double

% --- Executes during object creation, after setting all properties.
function edit5_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit5 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit6_Callback(hObject, eventdata, handles)
% hObject    handle to edit6 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit6 as text
%        str2double(get(hObject,'String')) returns contents of edit6 as a double

% --- Executes during object creation, after setting all properties.
function edit6_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit6 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit7_Callback(hObject, eventdata, handles)
% hObject    handle to edit7 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit7 as text
%        str2double(get(hObject,'String')) returns contents of edit7 as a double

% --- Executes during object creation, after setting all properties.
function edit7_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit7 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit8_Callback(hObject, eventdata, handles)
% hObject    handle to edit8 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit8 as text
%        str2double(get(hObject,'String')) returns contents of edit8 as a double

% --- Executes during object creation, after setting all properties.
function edit8_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit8 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit9_Callback(hObject, eventdata, handles)
% hObject    handle to edit9 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit9 as text
%        str2double(get(hObject,'String')) returns contents of edit9 as a double

% --- Executes during object creation, after setting all properties.
function edit9_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit9 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit10_Callback(hObject, eventdata, handles)
% hObject    handle to edit10 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit10 as text
%        str2double(get(hObject,'String')) returns contents of edit10 as a double

% --- Executes during object creation, after setting all properties.
function edit10_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit10 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end