%   SCRIPT TO COMPUTE THE TORQUES AT EACH JOINT FOR DIFFERENT MOTION STATES OF
%   a 3 DOF robotic arm.
%
%   In order to select an actuator (i.e. an electric motor, brushless for
%   example), we may need.
%   Nominal torque and speed: torque and speed for the 80% of the use of the motor.
%   Peak torque and speed: torque and speed for short periods of time. That
%   is: a higher torque that can be exerted at higher speeds during a
%   maximum of a 20% of the time.
%
%   Of course, the 80-20% are just bare numbers and should be given by the
%   robot manufacturer. Actually the torque in any motor depends directly
%   on the quantity of current that can be driven into the motor.
%   Typically, the peak torque is associated with a peak current. If the
%   peak current is maintained for a long time the motor will not be able
%   to dissipate the heat inside, thus generating high temperatures that
%   could melt the coils, conductors... etc.
%
%   The script uses the inverse dynamic model of the robot to simulate
%   different motion states and compute the torques for each situation.
%   The torques at each joint, as well as the torques at each motor are computed
%   (reduced by the gear ratio). A trapezoidal speed profile can be
%   selected at the parameters section of this file. This trapezoidal
%   profile should meet the desired features of the robot, such as maximum
%   joint speed, maximum acceleration/deceleration.
%   Please note that the movement of the robot is not simulated. The reader
%   should imagine that the manipulator is fixed at a given initial
%   position and the inverse dynamic function returns the torques at each
%   joint necessary to bring the arm to that motion state (defined by position q,
%   speed qd and acceleration qdd).
%
%   As a reference, below you can find the call to the inverse dynamics
%   function.
%
%   TAU = inversedynamic(robot, Q, QD, QDD, GRAV, FEXT)
%
% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.

function motor_selection_3dofplanar

close all
global robot
robot.dynamics.friction = 0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PARAMETERS SECTION
%   Feel free to change the values of q, maximum_speeds and
%   maximum_accels.
%
%   This script tries to allow the student to test any mechanism
%   at the worst case. In this sense, q should be adjusted
%   as the pose where each joint would (statically) be needing a
%   higher torque.
%   The maximum_speeds and maximum_acceleration define a trapezoidal
%   speed profile. This trapezoidal speed is used by most machines
%   to command changes in speed in any of their joints.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% robot pose: experiment by changing the pose while observing the different
%             torques at each joint
q=[0 0 0]; %rad
%maximum speeds at for each joint
maximum_speeds=[pi pi pi];%rad/second
%maximum acceleration/deceleration for each joint
maximum_accels=[pi/4 pi/4 pi/4]; %rad/second^2

% time of the trapezoidal profile that the joint moves at maximum speed
time_at_constant_speed=2; %seconds

%load robot parameters. Just uncomment this line
robot=load_robot('example', '3dofplanar');
drawrobot3d(robot, q)

% CUIDADO: los resultados se calculan con estas ratios.
robot.motors.G = [1 1 1]

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  FIRST, COMPUTE TRAPEZOIDAL PROFILES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%compute acceleration plus deceleration times for every joint
time_acc = 2*maximum_speeds./maximum_accels+time_at_constant_speed;

%compute the total time for the slowest joint
total_time=max(time_acc);

% Trapezoidal speed profiles for each joint
[input_speeds, input_accels, time]=build_trapezoidal_speed_profile(maximum_speeds, maximum_accels, total_time);

% EJERCICIO:
% REALIZA UN PLOT DE input_speeds e input_accels.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% FINALLY, COMPUTE TORQUES FOR EACH MOTION STATE.
% Please note that we consider that the robot is placed at a fixed position and consider
% different motion situations when we change the acceleration and speed at
% each joint. For each motion state, the inverse dynamic model returns the
% torques at each joint that would bring the robot to that motion.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
compute_inverse_dynamics(q, input_speeds, input_accels, time);

%Computes a trapezoidal speed profile for every joint given maximum
%permitted accelerations and maximum joint speeds
function [input_speeds, input_accelerations, time]=build_trapezoidal_speed_profile(maximum_speeds, maximum_accels, total_time)

delta_time=0.01;

%build time vector: twice acceleration time plus time at constant speed
time = 0:delta_time:total_time;

input_speeds=[];
input_accelerations=[];

for j=1:length(maximum_speeds),
    vel_row=[];
    acc_row=[];
    for i=1:length(time),
        [vel acc] = compute_values(time(i), maximum_speeds(j), maximum_accels(j), total_time);
        vel_row = [vel_row vel];
        acc_row = [acc_row acc];
    end
    input_speeds = [input_speeds; vel_row];
    input_accelerations = [input_accelerations; acc_row];
end

%returns the values of velocity and speed corresponding to a given time
function [vel acc]=compute_values(time_i, vel_max, acc_max, total_time)

tacc = vel_max/acc_max;
tdec = total_time-tacc;

if time_i < tacc
    vel = time_i.*acc_max;
    acc = acc_max;
    return;
elseif (time_i >= tacc) & (time_i < tdec)
    vel = vel_max;
    acc = 0;
    return;
else % time_i> tdec
    vel = vel_max-(time_i-tdec)*acc_max;
    acc = -acc_max;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   COMPUTE THE INVERSE DYNAMICS FOR EACH MOTION STATE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function compute_inverse_dynamics(q, input_speeds, input_accels, time)
global robot

%adjust_view(robot)
torques=[];
for j=1:length(time)
    fprintf('\nComputing time %d out of %d', j, length(time));
    % compute the torque to bring the robot instantaneously to this motion
    % state. change M=1  to add the effects of a 1kg mass load at the end effector
    M=0.1;
    %please note that the force due to the load acts on the z axis of
    tau=inversedynamic(robot, q, input_speeds(:,j), input_accels(:,j), [0  -9.81 0]', [0 -M*9.81 0 0 0 0]');
    torques=[torques tau];
end

%plot trapezoidal profiles
figure, hold, xlabel('time (s)'), ylabel('Input reference speeds (rad/s)')
plot(time, input_speeds(1,:), time, input_speeds(2,:), time, input_speeds(3,:) );
legend('Speed for joint 1 (qd1)','Speed for joint 2 (qd2)', 'Speed for joint 3 (qd3)')
%plot trapezoidal profiles, acceleration
figure, hold, xlabel('time (s)'), ylabel('Input reference acceleration (rad/s)')
plot(time, input_accels(1,:), time, input_accels(2,:), time, input_accels(3,:));
legend('Acceleration for joint 1 (qd1)','Acceleration for joint 2 (qd2)', 'Acceleration for joint 3 (qd3)')

% plot results. First, torques at each joint
figure, hold, xlabel('time (s)'), ylabel('Join Torques (N m)')
plot(time, torques(1,:), time, torques(2,:));
legend('Torque for joint 1 ','Torque  for joint 2 ')

% OTRA INFORMACION QUE ES ESENCIAL QUE PLOTEES:
% - Los pares en cada motor (utiliza robot.motors.G(1), .G(2)...

% - La potencia instantánea que realiza cada motor (la potencia es tau*w en
% cada articulación). La potencia pico del motor nos dará una idea de qué
% motor buscar en el catálogo.

% - Plotea la velocidad de cada motor en rpm: en efecto, los fabricantes
% tienen la costumbre de expresar la velocidad del motor en rpm y no en
% rad/s. Los fabricantes de robots tienen la costumbre de expresar las
% velocidades articulares en grados/s. Se te sugiere que plotees las
% velocidades de cada motor en rpm.

% Finalmente, en base al resultado, de cada motor, expresa:
% - El par pico (valor máximo en valor absoluto del par realizado por el motor).
% - El par nominal. Es el par en el punto intermedio del resultado del vector torques.
% - Velocidad nominal del motor en rpm.
% - La velocidad máxima en rpm del motor.


function [ r,f ] = GetReward( x )
% MountainCarGetReward returns the reward at the current state
% x: a vector of position and velocity of the car
% r: the returned reward.
% f: true if the car reached the goal, otherwise f is false

position = x(1);
% bound for position; the goal is to reach position = 0.5
bpright=0.5;
f=false;
r=-1;

if( position >= bpright)
	r = 100;
    f = true;
end


% DESCRIPTION:
%       subscript to display time steps and maximum supported frequency.
%
% ABOUT:
%       author      - Bradley Treeby
%       date        - 8th July 2014
%       last update - 8th July 2014
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

% This file is part of k-Wave. k-Wave is free software: you can
% redistribute it and/or modify it under the terms of the GNU Lesser
% General Public License as published by the Free Software Foundation,
% either version 3 of the License, or (at your option) any later version.
%
% k-Wave 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 Lesser General Public License for
% more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

% display time step information
disp(['  dt: ' scaleSI(dt) 's, t_end: ' scaleSI(kgrid.t_array(end)) 's, time steps: ' num2str(length(kgrid.t_array))]);

% if using the elastic code, get the minimum sound speeds (not including
% zero if set for the shear speed)
if ~elastic_code
    c_min = min(medium.sound_speed(:));
else
    c_min_comp = min(medium.sound_speed_compression(:));
    c_min_shear = min(medium.sound_speed_shear(medium.sound_speed_shear ~= 0));
end

% get suitable scaling factor
grid_size_metric = [kgrid.x_size, kgrid.y_size, kgrid.z_size];
[x_sc, scale, prefix] = scaleSI( min(grid_size_metric(grid_size_metric ~= 0)) ); %#ok<*ASGLU>
clear grid_size_metric;

% display the grid size and maximum supported frequency
switch kgrid.dim
    case 1

        % display grid size
        disp(['  input grid size: ' num2str(kgrid.Nx) ' grid points (' scaleSI(kgrid.x_size) 'm)']);

        % display maximum supported frequency
        disp(['  maximum supported frequency: ' scaleSI( kgrid.k_max * c_min / (2*pi) ) 'Hz']);

    case 2

        % display grid size
        disp(['  input grid size: ' num2str(kgrid.Nx) ' by ' num2str(kgrid.Ny) ' grid points (' num2str(kgrid.x_size*scale) ' by ' num2str(kgrid.y_size*scale) prefix 'm)']);

        if ~elastic_code

            % display maximum supported frequency
            if kgrid.kx_max == kgrid.ky_max
                disp(['  maximum supported frequency: ' scaleSI( kgrid.k_max * c_min / (2*pi) ) 'Hz']);
            else
                disp(['  maximum supported frequency: ' scaleSI( kgrid.kx_max * c_min / (2*pi) ) 'Hz by ' scaleSI( kgrid.ky_max * c_min / (2*pi) ) 'Hz']);
            end

        else

            % display the maximum supported frequency
            if kgrid.kx_max == kgrid.ky_max
                disp(['  maximum supported compressional frequency: ' scaleSI( kgrid.k_max * c_min_comp / (2*pi) ) 'Hz']);
                if isempty(c_min_shear)
                    disp('  maximum supported shear frequency: 0Hz');
                else
                    disp(['  maximum supported shear frequency: ' scaleSI( kgrid.k_max * c_min_shear / (2*pi) ) 'Hz']);
                end
            else
                disp(['  maximum supported compressional frequency: ' scaleSI( kgrid.kx_max * c_min_comp / (2*pi) ) 'Hz by ' scaleSI( kgrid.ky_max * c_min_comp / (2*pi) ) 'Hz']);
                if isempty(c_min_shear)
                    disp('  maximum supported shear frequency: 0Hz');
                else
                    disp(['  maximum supported compressional frequency: ' scaleSI( kgrid.kx_max * c_min_shear / (2*pi) ) 'Hz by ' scaleSI( kgrid.ky_max * c_min_shear / (2*pi) ) 'Hz']);
                end
            end

        end

    case 3

        % display grid size
        disp(['  input grid size: ' num2str(kgrid.Nx) ' by ' num2str(kgrid.Ny) ' by ' num2str(kgrid.Nz) ' grid points (' num2str(kgrid.x_size*scale) ' by ' num2str(kgrid.y_size*scale) ' by ' num2str(kgrid.z_size*scale) prefix 'm)']);

        if ~elastic_code

            % display maximum supported frequency
            if (kgrid.kx_max == kgrid.kz_max) && (kgrid.kx_max == kgrid.ky_max)
                disp(['  maximum supported frequency: ' scaleSI( kgrid.k_max * c_min / (2*pi) ) 'Hz']);
            else
                disp(['  maximum supported frequency: ' scaleSI( kgrid.kx_max * c_min / (2*pi) ) 'Hz by ' scaleSI( kgrid.ky_max * c_min / (2*pi) ) 'Hz by ' scaleSI( kgrid.kz_max * c_min / (2*pi) ) 'Hz']);
            end

        else

            % display the maximum supported frequency
            if (kgrid.kx_max == kgrid.kz_max) && (kgrid.kx_max == kgrid.ky_max)
                disp(['  maximum supported compressional frequency: ' scaleSI( kgrid.k_max * c_min_comp / (2*pi) ) 'Hz']);
                if isempty(c_min_shear)
                    disp('  maximum supported shear frequency: 0Hz');
                else
                    disp(['  maximum supported shear frequency: ' scaleSI( kgrid.k_max * c_min_shear / (2*pi) ) 'Hz']);
                end
            else
                disp(['  maximum supported frequency: ' scaleSI( kgrid.kx_max * c_min_comp / (2*pi) ) 'Hz by ' scaleSI( kgrid.ky_max * c_min_comp / (2*pi) ) 'Hz by ' scaleSI( kgrid.kz_max * c_min_comp / (2*pi) ) 'Hz']);
                if isempty(c_min_shear)
                    disp('  maximum supported shear frequency: 0Hz');
                else
                    disp(['  maximum supported frequency: ' scaleSI( kgrid.kx_max * c_min_shear / (2*pi) ) 'Hz by ' scaleSI( kgrid.ky_max * c_min_shear / (2*pi) ) 'Hz by ' scaleSI( kgrid.kz_max * c_min_shear / (2*pi) ) 'Hz']);
                end
            end

        end
end

% cleanup unused variables
clear c_min c_min_comp c_min_shear grid_size_metric


function [F,changed] = convertlogics(F)
%CONVERTLOGICS Internal function to convert logic constraints to mixed integer constraints

changed = 0;
if length(F)>0
    extvariables = yalmip('logicextvariables');
    if ~isempty(extvariables)
        for i = 1:length(F)
            if is(F(i),'elementwise')
                Fi = sdpvar(F(i));
                Fv =getvariables(Fi);
                if length(Fv)==1
                    xb = getbase(Fi);
                    if isequal(xb,[0 1])
                        if ismember(Fv,extvariables)
                            F(i) = (Fi >= 1);
                        end
                    end
                end
            end
        end
    end
end


function [done] = optMaxGenTerm(ops,bPop,endPop)
% function [done] = optMaxGenTerm(ops,bPop,endPop)
%
% Returns 1, i.e. terminates the GA, when either the maximal_generation is
% reached or when the optimal function val is found.
%
% ops    - a vector of options [maximum_generation optimal epsilon]
% bPop   - a matrix of best solutions [generation_found solution_string]
% endPop - the current generation of solutions

% Binary and Real-Valued Simulation Evolution for Matlab
% Copyright (C) 1996 C.R. Houck, J.A. Joines, M.G. Kay
%
% C.R. Houck, J.Joines, and M.Kay. A genetic algorithm for function
% optimization: A Matlab implementation. ACM Transactions on Mathmatical
% Software, Submitted 1996.
%
% 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 1, 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. A copy of the GNU
% General Public License can be obtained from the
% Free Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.

currentGen = ops(1);
maxGen     = ops(2);
optimal    = ops(3);
epsilon    = ops(4);
fitIndex   = size(endPop,2);
bestSolVal = max(endPop(:,fitIndex));
done       = (currentGen >= maxGen) | ((optimal - bestSolVal) <= epsilon);


function prob = sedumi2opti(prob)
%SEDUMI2OPTI  Converts a SEDUMI Problem (At, b, C, K) to OPTI Format
% prob = sedumi2opti(prob)

%   Copyright (C) 2013 Jonathan Currie (IPL)

if(~isfield(prob,'sdcone') || isempty(prob.sdcone) || ~isstruct(prob.sdcone))
    error('This function expects a prob.sdcone to contain a structure');
end
sdcone = prob.sdcone;
if((~isfield(sdcone,'A') && ~isfield(sdcone,'At')) || ~isfield(sdcone,'b') || (~isfield(sdcone,'C') && ~isfield(sdcone,'c')) || ~isfield(sdcone,'K'))
    error('This function expects prob.sdcone to contain the fields At (or A), b, C and K');
end
if(~isstruct(sdcone.K))
    error('SeDuMi prob.sdcone.K must be a structure!');
end
if(~isfield(sdcone.K,'s') && ~isfield(sdcone.K,'l'))
    error('SeDuMi prob.sdcone.K must contain the fields l or s (or both)');
end
if((isfield(sdcone.K,'scomplex') && ~isempty(sdcone.K.scomplex)) || (isfield(sdcone.K,'ycomplex') && ~isempty(sdcone.K.ycomplex)) || (isfield(sdcone.K,'xcomplex') && ~isempty(sdcone.K.xcomplex)))
    error('Complex variables are not supported in this interface - use SeDuMi directly to solve!');
end
if(isfield(sdcone.K,'f') && ~isempty(sdcone.K.f) && any(sdcone.K.f > 0))
    error('SeDuMi free variables are not supported in this interface - use SeDuMi directly to solve!');
end
if(isfield(sdcone.K,'q') && ~isempty(sdcone.K.q))
    error('SeDuMi SOCP constraints are not supported in this interface - use SeDuMi directly to solve!');
end
if(isfield(sdcone.K,'r') && ~isempty(sdcone.K.r))
    error('SeDuMi rotated cone constraints are not supported in this interface - use SeDuMi directly to solve!');
end

%Read SeDuMi problem, convert to OPTI standard primal form
% MINIMIZE   dot(f,x) SUCH THAT  SUM A_i*x_i - C >= 0.
top = 1; dims = 0;
haveSDP = false; haveLP = false;
K = sdcone.K;
if(isfield(sdcone,'C'))
    c = sdcone.C;
else
    c = sdcone.c;
end
if(isfield(sdcone,'A'))
    A = sdcone.A;
else
    A = sdcone.At;
end
b = full(sdcone.b);

%Check c, b is a vector
if(size(c,2) > 1 && size(c,1) > 1)
    error('SeDuMi c field must be a column vector!');
end
if(size(b,2) > 1 && size(b,1) > 1)
    error('SeDuMi b field must be a column vector!');
end

%Flip A based on b dimensions
if(size(A,2) ~= length(sdcone.b))
    if(size(A,1) == length(sdcone.b))
        A = A';
    else
        error('SeDuMi A (At) appears to  have incorrect dimensions.\nExpected one dimension of A to be the same as length as %s.','b');
    end
end
if(size(c,2) > size(c,1))
    c = c';
end
%Check length of C against A
if(length(c) ~= size(A,1))
    error('SeDuMi c does not have the same number of elements as rows in A (At)');
end
%Check dims entered in K
if(isfield(K,'s') && ~isempty(K.s) && K.s(1) > 0)
    haveSDP = true;
    dims = dims + sum(K.s.^2);
end
if(isfield(K,'l') && ~isempty(K.l) && K.l(1) > 0)
    if(length(K.l) > 1)
        error('The SeDuMi K.l field must contain a scalar, the number of linear constraints');
    end
    haveLP = true;
    dims = dims + K.l;
end
%Check dims against rows
if(dims ~= size(A,1))
    error('SeDuMi A (At) does not have correct dimensions as per the specifications in K. Expected %d rows.',dims);
end

%Extract Objective
prob.f = full(-b);

%Extract Linear Constraints
if(haveLP)
    n = K.l;
    if(isfield(prob,'A') && ~isempty(prob.A)) %concatenate
        prob.A = [prob.A; A(top:top+n-1,:)];
        prob.b = full([prob.b; prob.c(top:top+n-1,:)]);
    else
        prob.A = A(top:top+n-1,:);
        prob.b = full(c(top:top+n-1,:));
    end
    top = top+n;
end
%Extract Semidefinite Constraints
if (haveSDP)
    prob.sdcone = cell(length(K.s),1);
    for i = 1:length(K.s)
        n = K.s(i);
        prob.sdcone{i} = [-c(top:top+n^2-1,:) -A(top:top+n^2-1,:)];
        top = top+n^2;
    end
else
    prob.sdcone = [];
end


function som_stats_plot(csS,plottype,varargin)

%SOM_STATS_PLOT Plots of data set statistics.
%
% som_stats_plot(csS, plottype, [argID, value, ...])
%
%  som_stats_plot(csS,'stats')
%  som_stats_plot(csS,'stats','p','vert','color','r')
%
%  Input and output arguments ([]'s are optional):
%   csS         (cell array) of statistics structs
%               (struct) a statistics struct
%   plottype    (string) some of the following
%                        'hist'   histogram
%                        'box'    min, max, mean, and std shown as a boxplot
%                        'stats'  both histogram (with black) and the boxplot
%   [argID, (string) See below. The values which are unambiguous can
%    value] (varies) be given without the preceeding argID.
%
% Here are the valid argument IDs and corresponding values. The values which
% are unambiguous (marked with '*') can be given without the preceeding argID.
%   'counts'      *(string) 'c' (for counts, the default) or 'p' (for percentages)
%   'color'        (vector) size 1 x 3, color to be used
%                  (string) a color string
%   'title'        (string) 'on' (default) or 'off'
%   'orientation' *(string) 'horiz' or 'vert' (default): orientation for the
%                           bin values (horizontally or vertically)
%
% See also  SOM_STATS, SOM_STATS_TABLE, SOM_TABLE_PRINT, SOM_STATS_REPORT.

% Contributed to SOM Toolbox 2.0, December 31st, 2001 by Juha Vesanto
% Copyright (c) by Juha Vesanto
% http://www.cis.hut.fi/projects/somtoolbox/

% Version 2.0beta juuso 311201

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% arguments

% statistics
if isstruct(csS), csS = {csS}; end

% default values
useprob   = 0;
color     = [0 0 1];
showtitle = 1;
horiz     = 0;

% varargin
i=1;
while i<=length(varargin),
  argok = 1;
  if ischar(varargin{i}),
    switch varargin{i},
     % argument IDs
     case 'counts',      i=i+1; useprob = strcmp(varargin{i}(1),'p');
     case 'color',       i=i+1; color = varargin{i};
     case 'title',       i=i+1; showtitle = strcmp(varargin{i},'on');
     case 'orientation', i=i+1; horiz = strcmp(varargin{i},'horiz');
     % unambiguous values
     case {'horiz','vert'}, horiz = strcmp(varargin{i},'horiz');
     case {'c','p'}, useprob = strcmp(varargin{i}(1),'p');
     otherwise argok=0;
    end
  elseif isstruct(varargin{i}) && isfield(varargin{i},'type'),
    argok = 0;
  else
    argok = 0;
  end
  if ~argok,
    disp(['(som_stats_plot) Ignoring invalid argument #' num2str(i+2)]);
  end
  i = i+1;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
%% action

ss = ceil(sqrt(length(csS))); ss = [ss, ceil(length(csS)/ss)];

for j = 1:length(csS),
    sS = csS{j};
    subplot(ss(1),ss(2),j);
    switch plottype,
    case 'stats',
        cla, hold on
        Counts = sS.hist.counts;
        if useprob, for i=1:size(Counts,2), Counts(:,i) = Counts(:,i)/sum(Counts(:,i)); end, end
        hist_plot(sS.hist.bins,sS.hist.binlabels,Counts,color);
        box_plot(sS.min,sS.max,sS.mean,sS.std,[0 0 0]);
    case 'hist',
        cla, hold on
        Counts = sS.hist.counts;
        if useprob, for i=1:size(Counts,2), Counts(:,i) = Counts(:,i)/sum(Counts(:,i)); end, end
        hist_plot(sS.hist.bins,sS.hist.binlabels,Counts,color);
    case 'box',
        cla
	box_plot(sS.min,sS.max,sS.mean,sS.std,color);
    end
    if showtitle, title(sprintf('%s (valid: %d/%d)',sS.name,sS.nvalid,sS.ntotal)); end
    if ~horiz, view(90,-90); end
    a = axis; a(1) = sS.min; a(2) = sS.max; axis(a);
end

return;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
%% subfunctions

function hist_plot(bins,binlabels,Counts,color)

    if nargin<4, color = jet(size(Counts,2)); end
    h = bar(bins,Counts);
    for j=1:length(h), set(h(j),'facecolor',color(j,:),'edgecolor','none'); end
    a = axis; a(3:4) = [0 max(Counts(:))]; axis(a);
    set(gca,'XTick',bins,'XTickLabel',binlabels);
    return;

function vstr = numtostring(v,d)

    nearzero = (abs(v)/(max(v)-min(v)) < 10.^-d);
    i1 = find(v > 0 & nearzero);
    i2 = find(v < 0 & nearzero);
    vstr = strrep(cellstr(num2str(v,d)),' ','');
    vstr(i1) = {'0.0'};
    vstr(i2) = {'-0.0'};
    return;

function box_plot(mi,ma,me,st,Color)

    if nargin < 5, Color = jet(length(mi)); end
    a = axis;
    y = linspace(a(3),a(4),length(mi)+2); y = y(2:end);
    d = (y(2)-y(1))/20;
    for i=1:length(mi),
        h1 = line([mi(i) ma(i)],[y(i) y(i)]);
        h2 = line([mi(i) mi(i) NaN ma(i) ma(i)],[y(i)-d y(i)+d NaN y(i)-d y(i)+d]);
        h3 = line([me(i)-st(i) me(i)+st(i)],[y(i) y(i)]);
        h4 = line([me(i) me(i)],[y(i)-2*d y(i)+2*d]);
        set([h1 h2 h3 h4],'color',Color(i,:));
        set([h1 h2],'linewidth',1);
        set([h3 h4],'linewidth',3);
    end
    return;


function [superpixels, ucm, nSP, spArea] = getSuperpixels(imName, ucmThresh)
% function [superpixels, ucm, nSP, spArea] = getSuperpixels(imName, ucmThresh)
%  includes the boundary at value ucmThresh, ucmThresh must be in double, like 34/255.
%  Returns the UCM in uint8 format.

	paths = getPaths();
	ucm = getUCM(imName);
	ucmD = im2double(ucm);
	superpixels = bwlabel(ucmD < ucmThresh);
	superpixels = superpixels(2:2:end,2:2:end);
	nSP = max(superpixels(:));
	if(nargout > 2)
		spArea = histc(superpixels(:),1:nSP);
	end
end


function [inerr,dw,inLL,right ]=batch_equal_nomask_lstm(W,dataall  )
% gradient threshold for bptt need done   ,inLL,right
global   mzeros convert in_size gate_size out_size share_size usegpu...
    share_size2  in ingate cellstate cells outgate globalData    ...
    node_outgateInit cellinInit node_cellbiasInit delta_outInit
if nargin==1
    data=globalData;
else
    data=dataall;
end
if usegpu
    fun=@sigmoidnGpu;
    delta_fun=@delta_sigmoidnGpu;
    activation=@activationGpu;
    deactivation=@deactivationGpu;
else
    fun=@sigmoidnCpu;
    delta_fun=@delta_sigmoidnCpu;
    activation=@activationCpu;
    deactivation=@deactivationCpu;
end

actNum1=convert(1);
actNum2=2*actNum1;
numsamples=size(data,1) ;

dw= zeros(1 ,2*share_size2 +share_size2*numMmcell + (gate_size*numMmcell+1)*out_size);
[ Wingate,Wcell , Woutgate, Wout]=unpack(W);

[dwingate,dwcell,dwoutgate,dwout] = unpack2(dw);
dwingate=convert(dwingate);
dwcell=convert(dwcell);
dwoutgate=convert(dwoutgate);
dwout=convert(dwout);

right=zeros(numsamples,1,'int32');
inLL = 0;
inerr = zeros( numsamples,out_size);

timespan = size( data  , 3 );
node_tmpindex=cell(1,timespan);
node_tmpindex2=cell(1,timespan);
node_tmpindex3=cell(1,timespan);

node_outgate= node_outgateInit;
cellin=cellinInit;
node_cellbias=node_cellbiasInit;
delta_out=delta_outInit;
cellstatus= cellstatusInit;

node_ingate = node_outgate;
node_cell = cellin;
Y_cellout = cellin;
delta_outgate=node_outgate;

errorstate=cellin;
delta_cellin=cellin;
delta_ingate=node_outgate;

Wingate_in=convert( Wingate(in,:) );
Wingate_ingate= convert( Wingate(ingate,:) );
Wingate_cellstate=convert( Wingate(cellstate,:) );
Wingate_cell=convert( Wingate(cells,:) );
Wingate_outgate=convert( Wingate(outgate,:) );

% to cellin
Wcell_in=convert( Wcell(in,:) ) ;
Wcell_ingate=convert( Wcell(ingate,:) );
Wcell_cell=convert( Wcell(cells,:) );
Wcell_outgate=convert( Wcell(outgate,:) );

% to outgate
Woutgate_in=convert( Woutgate(in,:) );
Woutgate_ingate=convert( Woutgate(ingate,:) );
Woutgate_cellstate=convert( Woutgate(cellstate,:) );
Woutgate_cell = convert( Woutgate(cells,:) );
Woutgate_outgate=convert( Woutgate(outgate,:) );

t=2;
while t<=timespan

    output = data(:,1:out_size,t ) ;

    % forward pass
    node_in = data(:,1:in_size,t-1);
    node_cellbias{t } = data(:,in_size+1,t-1);

    % to ingate
    tmpp=node_in*Wingate_in + node_outgate{t-1}*Wingate_outgate +...
        cellstatus{t-1} * Wingate_cellstate + node_cell{t-1}*Wingate_cell +  node_ingate{t-1} * Wingate_ingate;
    node_ingate{t}=fun( tmpp ,signum);

    tmpp=node_in*Wcell_in +  node_ingate{t-1}  * Wcell_ingate  +...
        + node_cell{t-1}*Wcell_cell +node_outgate{t-1} * Wcell_outgate ;
    cellin{t} = activation( tmpp,actNum2 ) ;

    cellstatus{t}  =  cellstatus{t-1}  +    cellin{t }  .* repmat(node_ingate{t},1,numMmcell )  ;

    Y_cellout{t}= activation( cellstatus{t } ,actNum1 ) ;

    tmpp=node_in*Woutgate_in +   node_outgate{t-1} *Woutgate_outgate +...
        cellstatus{t}  * Woutgate_cellstate + node_cell{t-1}*Woutgate_cell +  node_ingate{t-1} * Woutgate_ingate;
    node_outgate{t }  = fun(tmpp  ,signum);

    node_cell{t }  = repmat(node_outgate{t},1,numMmcell )   .* Y_cellout{t }  ;

    node_tmpindex{t } = [ node_in  node_ingate{t-1} cellstatus{t-1} node_cell{t-1} node_outgate{t-1} ];
    node_tmpindex2{t} = [ node_in  node_ingate{t-1} cellstatus{t} node_cell{t-1} node_outgate{t-1} ];
    node_tmpindex3{t } = [ node_in  node_ingate{t-1}  node_cell{t-1} node_outgate{t-1} ];

    node_out  = fun(  [ node_cell{t}     node_cellbias{t}   ] * Wout    ,signum ) ;

    delta_out{t} =  ( - output + node_out  )  .* delta_fun(node_out,signum);

    inerr = inerr +     (  (output  - node_out  )).^2    ;
    %       right = right +   rightfun(masko,output,node_out) ;
    t=t+1;
end
t=t-1;
% just  cell
tmp_outgate=repmat(node_outgate{t},1,numMmcell);
tmp_ingate=repmat(node_ingate{t},1,numMmcell);
errorcell = delta_out{t} * Wout(1:end-1,:)';

% just cell to outgate
delta_outgate{t} =  squeezing(delta_fun( tmp_outgate,signum) .* errorcell  .* Y_cellout{t})  ;  %  .* nodenew_outgate.* ( 1 - nodenew_outgate  )  ;
% peephole
errorstate{t} = errorcell .* tmp_outgate .* deactivation(Y_cellout{t},actNum1)+...
    delta_outgate{t} * Woutgate_cellstate' ;
delta_cellin{t} =tmp_ingate .* errorstate{t}.* deactivation(cellin{t},actNum2);
delta_ingate{t} =  squeezing(cellin{t} .* errorstate{t} .* delta_fun(tmp_ingate,signum));
for t = timespan-1:-1:2
    % just  cell
    tmp_outgate=repmat(node_outgate{t},1,numMmcell);
    tmp_ingate=repmat(node_ingate{t},1,numMmcell);
    errorcell = delta_out{t } * Wout(1:end-1,:)' + delta_outgate{t+1} * Woutgate_cell' +...
        delta_cellin{t+1} * Wcell_cell' + delta_ingate{t+1} * Wingate_cell'; % W( out , %cell);

    % just cell to outgate
    delta_outgate{t} = delta_fun(  node_outgate{t},signum )  .* ( squeezing(errorcell  .* Y_cellout{t})  +...
        delta_ingate{t+1} * Wingate_outgate'+ delta_cellin{t+1} * Wcell_outgate'+...
        delta_outgate{t+1} *Woutgate_outgate') ;  %  .* nodenew_outgate.* ( 1 - nodenew_outgate  )  ;
    % peephole
    errorstate{t} = errorcell .* tmp_outgate .* deactivation( Y_cellout{t} ,actNum1 )  + ...
        errorstate{t+1} + delta_ingate{t+1}* Wingate_cellstate' +...
        delta_outgate{t} * Woutgate_cellstate' ;

    delta_cellin{t} = tmp_ingate .* deactivation( cellin{t } ,actNum2) .* errorstate{t} ;

    delta_ingate{t} = delta_fun( node_ingate{t} ,signum).* ( squeezing(cellin{t} .* errorstate{t})  +...
        + delta_ingate{t+1} * Wingate_ingate' +delta_cellin{t+1} * Wcell_ingate' + ...
        delta_outgate{t+1} * Woutgate_ingate' );
end

for t = 2:timespan
    dwout = dwout + [node_cell{t} node_cellbias{t} ]' *  delta_out{t} ;
    dwoutgate =dwoutgate + node_tmpindex2{t}' * delta_outgate{t} ;
    dwcell=dwcell+ node_tmpindex3{t}'*delta_cellin{t};
    dwingate = dwingate + node_tmpindex{t}'  * delta_ingate{t}   ;
end

inerr = gather(  1/2*    sum(    inerr)   /numsamples)  ;%  1/2*  for gradient checking
%right=gather(sum(right));

dw=pack2(dwingate,dwcell ,dwoutgate,dwout);
dw = gather(dw / numsamples );

    function [Wingate,Wcell ,Woutgate,Wout] = unpack(W)
        Wingate= reshape( W( 1:share_size2 )   , share_size ,gate_size );
        Wcell  = reshape( W( share_size2 +1 :   share_size2 *(1+numMmcell) ),  share_size, numMmcell*gate_size);
        Woutgate = reshape( W( (1+numMmcell) * share_size2 +1 : (2+numMmcell)* share_size2  ), share_size , gate_size);
        Wout = reshape( W( (2+numMmcell) * share_size2 +1 : end ) ,gate_size*numMmcell+1 ,  out_size);
    end
    function [Wingate,Wcell ,Woutgate,Wout] = unpack2(W)
        Wingate= reshape( W( 1:share_size2 )   , share_size ,gate_size );
        % Wcell  = reshape( W( share_size2 +1 : 2* share_size2  ),  share_size,gate_size);
        Wcell  = reshape( W( share_size2 +1 :   share_size2 *(1+numMmcell) ),  share_size, numMmcell*gate_size);
        %   share_size = in_size + gate_size * (2+2*problem.numMmcell) ;
        Wcell =  Wcell([1:in_size+gate_size (in_size+gate_size+gate_size*numMmcell+1):end],:) ;
        Woutgate = reshape( W( (1+numMmcell) * share_size2 +1 : (2+numMmcell)* share_size2  ), share_size , gate_size);
        Wout = reshape( W( (2+numMmcell) * share_size2 +1 : end ) ,gate_size*numMmcell+1 ,  out_size);
    end
    function [W]=pack2(Wingate,Wcell , Woutgate,Wout)
        tmp=mzeros(share_size,gate_size*numMmcell);
        tmp([1:in_size+gate_size (in_size+gate_size+gate_size*numMmcell+1):end],:)=Wcell;
        Wcell=tmp;
        W = [Wingate(:);Wcell(:) ;Woutgate(:);Wout(:)];
    end

    function y=sigmoidnGpu(x,num)
        y=arrayfun(@(x,num)1./(1+exp(- num*x)),x,num);
    end
    function y=delta_sigmoidnGpu(x,num)
        y=arrayfun(@(x,num)num*x.*(1-x),x,num);
    end
    function y=sigmoidnCpu(x,num)
        y=1./(1+exp(- num*x));
    end
    function y=delta_sigmoidnCpu(x,num)
        y=  num*x.*(1-x);
    end
    function y =  activationCpu(x,num)
        y=num*2./(1+exp(-x))-num;
    end
    function y =  deactivationCpu(x,num)
        y=0.5/num*(num+x).*(num-x);
    end
    function y =  activationGpu(x,num)
        y= arrayfun(@(x,num)num*2./(1+exp(-x))-num,x,num);
    end
    function y =  deactivationGpu(x,num)
        y=arrayfun(@(x,num)0.5/num*(num+x).*(num-x),x,num);
    end
    function y = squeezing(x)
        y=zeros(size(x,1),gate_size);
        for i = 1:numMmcell
            y = y + x(:,1+(i-1)*gate_size: i*gate_size);
        end
        %         x=mat2cell(x,size(x,1),gate_size*ones(1,numMmcell));
        %
        %         for i=2:numMmcell
        %             x{1}=x{1}+x{i};
        %         end
        %         y=x{1};
        %    y= cell2mat(cellfun(@plus  ,x, 'UniformOutput',false));
    end

end


function [returnVar,msg] = ReRefFilFile(fname,nbChan,refChan,rerefChan)

% USAGE:
%     ReRefFilFile(fbasename, nbChan, refChan, rerefChan)
%     This function substracts one channel or the median of a series of
%     channels (the 'reference' channel) from other channels.
%
%     Note: this uses 1-based indexing, not zero-based, so you have to take
%     the channel number in neuroscope and add one to it, e.g. channel 72
%     in neuroscope is channel 73 here.
%
% INPUTS:
%     fname: fil file name. Could be also a file basename not finishing
%     with '.fil' and the program will be then executed for all the fil
%     files begining with this file basename.
%     nbChan: total number of channels in fil file
%     refChan: reference channel (could be a vector, in this case the median will be the new reference)
%     rerefChan: vector of channels to rereference
%
% Adrien Peyrache 2013

if ~strcmp(fname(end-2:end),'fil')
    datFiles = dir([fname '*.fil']);
    for ii=1:length(datFiles)
        ReRefFilFile(datFiles(ii).name,nbChan,refChan,rerefChan)
    end

else
    fprintf('ReReferencing %s\n',fname)
    try
        infoFile = dir(fname);

        chunk = 1e6;
        nbChunks = floor(infoFile.bytes/(nbChan*chunk*2));
        warning off
        if nbChunks==0
            chunk = infoFile.bytes/(nbChan*2);
        end

        for ix=0:nbChunks-1
            m = memmapfile(fname,'Format','int16','Offset',ix*chunk*nbChan*2,'Repeat',chunk*nbChan,'writable',true);
            d = m.Data;
            d = reshape(d,[nbChan chunk]);
            ref = d(refChan,:);
            if length(refChan)>1
                ref = median(double(ref));
            end
            ref = repmat(ref,[length(rerefChan) 1]);
            d(rerefChan,:) = d(rerefChan,:)-int16(ref);

            m.Data = d(:);
            clear d m
        end
        %close(h)

        newchunk = infoFile.bytes/(2*nbChan)-nbChunks*chunk;

        if newchunk
            m = memmapfile(fname,'Format','int16','Offset',nbChunks*chunk*nbChan*2,'Repeat',newchunk*nbChan,'writable',true);
            d = m.Data;
            d = reshape(d,[nbChan newchunk]);
            ref = d(refChan,:);
            if length(refChan)>1
                ref = median(ref);
            end
            ref = repmat(ref,[length(rerefChan) 1]);
            d(rerefChan,:) = d(rerefChan,:)-int16(ref);
            m.Data = d(:);
         clear d m
        end
        warning on
        returnVar = 1;
        msg = '';

    catch
        fprintf(['Error occurred in processing ' fname '. File not rereferenced.\n']);
        keyboard
        returnVar = 0;
        msg = lasterr;
    end
    clear m
end


function v = gradient(f)
%GRADIENT Gradient of a BALLFUN in cartesian coordinates.
%   GRADIENT(F) is the gradient of the BALLFUN F expressed in
%   cartesian coordinates.
%
% See also DIV, CURL

% Copyright 2019 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

Vx = diff(f, 1);
Vy = diff(f, 2);
Vz = diff(f, 3);
v = ballfunv(Vx, Vy, Vz);
end


function garchplot(data, ht, resids)
%{
-----------------------------------------------------------------------
 PURPOSE:
 Plots the data, volatility and residuals
-----------------------------------------------------------------------
 USAGE:
 garch(data, ht, resids)

 INPUTS:
 data:      (T x 1) vector of data
 ht:        (T x 1) vector of conditional variance
 resids:    (T x 1) vector of residuals

-----------------------------------------------------------------------
Author:
 Alexandros Gabrielsen, a.gabrielsen@city.ac.uk
 Date: 08/2011
-----------------------------------------------------------------------
%}
if nargin == 0
    error('Data, Conditional Variance, Residuals')
end

if size(data,2) > 1 | size(resids,2) > 1 | size(resids,2) > 2
     error('Data, variance and residual vectors should be column vectors')
end

figure
subplot(3,1,1), plot(data);
title('Returns');
subplot(3,1,2), plot(resids);
title('Residuals');
subplot(3,1,3), plot( ht);
title('Conditional Variance');

end


function [cs,mineral] = loadCIF(fname,varargin)
% import crystal symmetry from cif file
%
% if cif file not found and input name is a valid COD entry, this function
% tries to download the file from <http://www.crystallography.net/cif/
% http://www.crystallography.net/cif/>
%
% Syntax
%   loadCIF('5000035.cif')
%   loadCIF(5000035)       % lookup online
%
% See also
% symmetry

if ~iscell(fname)
  if isnumeric(fname)
    fname = copyonline(fname);
  end
  [pathstr, name, ext] = fileparts(char(fname));

  if isempty(ext), ext = '.cif';end
  if isempty(pathstr) && ~exist([name,ext],'file')
    pathstr = mtexCifPath;
  end

  % load file
  if ~exist(fullfile(pathstr,[name ext]),'file')
    try
      fname = copyonline(fname);
    catch %#ok<CTCH>
      dir(fullfile(mtexCifPath,'*.cif'))
      error('I could not find the corresponding cif. Above you see the list of localy avaible cif files.')
    end
  else
    fname = fullfile(pathstr,[name ext]);
  end
  str = file2cell(fname);
else
  str = fname;
  name = '';
end

try
  % get a name for it
  mineral_names = {...
    '_chemical_name_mineral',...
    '_chemical_name_systematic',...
    '_chemical_formula_structural',...
    '_chemical_formula_sum'};

  for alias = mineral_names
    mineral = extract_token(str,alias{:});
    if ~isempty(mineral), break; end
  end

  % find space group
  group_aliases = {...
    '_symmetry_space_group_name_H-M',...
    '_symmetry_point_group_name_H-M',...
    '_symmetry_cell_setting'};
  for gp = group_aliases
    group = extract_token(str,gp{:});
    if ~isempty(group), break; end
  end

  % find a,b,c
  axis = [extract_token(str,'_cell_length_a',true) ...
    extract_token(str,'_cell_length_b',true) ...
    extract_token(str,'_cell_length_c',true)];

  % find alpha, beta, gamma
  angles = [extract_token(str,'_cell_angle_alpha',true) ...
    extract_token(str,'_cell_angle_beta',true) ...
    extract_token(str,'_cell_angle_gamma',true)];

  if length(axis)<3,
%     warning('crystallographic axis mismatch');
    axis = [1 1 1];
  end
  if length(angles)<3,
%     warning('crystallographic angles mismatch');
    angles = [90 90 90];
  end

  assert(~isempty(group));

  cs = crystalSymmetry(group,axis,angles*degree,'mineral',mineral);

catch
  error(['Error reading cif file', fname]);
end

function t = extract_token(str,token,numeric)

pos = strmatch(token,str);
if ~isempty(pos)
  t = strtrim(regexprep(str{pos(1)},[token '|'''],''));
  if ~isempty(t)
    if nargin>2 && numeric
      t = sscanf(t,'%f');
    end
  end
else
  t = '';
end

function fname = copyonline(cod)

try
  if isnumeric(cod)
    cod = num2str(cod);
  end

  fname = fullfile(mtexCifPath,[cod '.cif']);
  if exist(fname,'file')
    return
  end

  if ~isempty(str2num(cod))
    disp('CIF-File from Crystallography Open Database')
    disp(['> download : http://www.crystallography.net/cif/' cod '.cif'])
    cif = urlread(['http://www.crystallography.net/cif/' cod '.cif']);
  else
    cif = urlread(cod);
  end
catch
  disp('> unluckily failed to find cif-file')
  error('CIF-file not valid online')
end

fname = fullfile(mtexCifPath,[cod '.cif']);

fid = fopen(fname,'w');
fwrite(fid,cif);
fclose(fid);
disp(['> copied to: ' fname]);


% DEMO6 - Gratuitis ncdataset demo showing AUV path

echo('on')
% Starting DEMO6 ----------------------------------------------------------
% Gratuitis demo showing AUV path
%% ---- open datset
nav = ncdataset('http://dods.mbari.org/cgi-bin/nph-nc/data/ssdsdata/ssds/generated/netcdf/files/ssds.shore.mbari.org/auvbi/missionlogs/2009/2009173/2009.173.03/navigation.nc');
y = nav.data('mPos_y');
x = nav.data('mPos_x');
z = nav.data('mDepth');
%% ---- 3d plot of AUV location
plot3(x, y, z, '.')
set(gca, 'ZDir', 'reverse')
axis('equal')
grid('on')
xlabel('meters')
ylabel('meters')
zlabel('meters')
title({'Gratuitis demo showing AUV path',nav.location})
echo('off') % Ending DEMO6 ------------------------------------------------


function lagplot(c);

% Author: Michael West, Geophysical Institute, Univ. of Alaska Fairbanks
% $Date$
% $Revision$

% PREP PLOT
figure('Color','w','Position',[50 50 600 500]);
set(gcf,'DefaultAxesFontSize',14);
imagesc(c.L);
title('Lag time for maximum correlation (s)');

% ADD DATES TO AXES
n = length(c.trig);
set(gca,'XTick',[1:round(n/25):n]);
set(gca,'YTick',[1:round(n/25):n]);
yt = get(gca,'YTick');
set(gca,'YTickLabel',datestr(c.trig(yt),'yyyy-mm-dd HH:MM'),'FontSize',6);

% DRESS UP THE FIGURE
cmap = load('colormap_lag.txt');
colormap(cmap);
colorbar;
xlabel('Event number');
ylabel('Event date');

%PRINT OUT FIGURE
set(gcf, 'paperorientation', 'portrait');
set(gcf, 'paperposition', [1.25 2.5 6 6] );
%print(gcf, '-depsc2', 'FIG_tartan.ps')


function vscl = vscale( f )
%VSCALE   Estimate the vertical scale of a function.
%   VSCALE(F) estimates the vertical scale (also known as the dynamical range)
%   of a function. This is required because a TRIGTECH does not store its
%   interpolation data. If F is an array-valued TRIGTECH with K columns, then
%   the result is a row vector of length K.

% Copyright 2014 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Get the coefficients:
c = f.coeffs;

% Check isempty:
if ( isempty( c ) )
    vscl = 0;
elseif ( size(c, 1) == 1 )
    vscl = abs(c);
else
    % Compute values:
    vals = f.coeffs2vals(c);
    % Take max:
    vscl = max(abs(vals), [], 1);
end

end


function [hfig,finalim,final_pforefly] = PlotSampleFramesColor(trx,readframe,bkgdim0,predictions,mainfly,otherflies,ts,varargin)

colorpos = [.7,0,0];
colorneg = [0,0,.7];
border = 20;

[colorpos,colorneg,border,hfig,figpos,...
  cm,fg_thresh,bg_thresh,sigma_bkgd,wmah,frac_a_back,dist_epsilon,...
  ncolors_reference,solidbkgdcolor,...
  flipim,intensityoff] = ...
  myparse(varargin,'colorpos',colorpos,...
  'colorneg',colorneg,...
  'border',border,...
  'hfig',[],...
  'figpos',[],...
  'colormap',{@jet,@jet},...
  'fg_thresh',115/255,...
  'bg_thresh',10/255,...
  'sigma_bkgd',115/255-10/255,...
  'wmah',.5,...
  'frac_a_back',1,...
  'dist_epsilon',.1,...
  'ncolors_reference',[],...
  'solidbkgdcolor',[.85,.85,.85],...
  'flipim',false,...
  'intensityoff',0);

if isempty(otherflies),
  flygroups = {mainfly};
else
  flygroups = {mainfly,otherflies};
end
nflygroups = numel(flygroups);
nframes = numel(ts);
allts = ts(1):ts(end);
nframes_total = numel(allts);
allflies = [flygroups{:}];

[nr0,nc0,ncolors] = size(bkgdim0);

%% grab out relevant time points
% not rotating currently, maybe rotate in the future?

x = nan(trx.nflies,nframes_total);
y = nan(trx.nflies,nframes_total);
a = nan(trx.nflies,nframes_total);
b = nan(trx.nflies,nframes_total);
theta = nan(trx.nflies,nframes_total);
imcenter = [(nc0+1)/2,(nr0+1)/2]; %#ok<NASGU>
for fly = 1:trx.nflies,
  idx = allts+trx(fly).off;
  i0 = find(idx>=1,1);
  i1 = find(idx<=trx(fly).nframes,1,'last');
  if isempty(i0) || isempty(i1),
    continue;
  end
  x(fly,i0:i1) = trx(fly).x(idx(i0:i1));
  y(fly,i0:i1) = trx(fly).y(idx(i0:i1));
  a(fly,i0:i1) = trx(fly).a(idx(i0:i1));
  b(fly,i0:i1) = trx(fly).b(idx(i0:i1));
  theta(fly,i0:i1) = trx(fly).theta(idx(i0:i1));
end

bkgdim = bkgdim0;

%% figure out the axes limits
xlim = [inf,-inf];
ylim = [inf,-inf];
for fly = allflies,
  for j = 1:nframes_total,
    [x1,x2,y1,y2] = ellipse_to_bounding_box(x(fly,j),y(fly,j),a(fly,j),b(fly,j),theta(fly,j));
    xlim(1) = min([xlim(1),x1,x2]);
    xlim(2) = max([xlim(2),x1,x2]);
    ylim(1) = min([ylim(1),y1,y2]);
    ylim(2) = max([ylim(2),y1,y2]);
  end
end
xlim = [floor(xlim(1))-border,ceil(xlim(2))+border];
ylim = [floor(ylim(1))-border,ceil(ylim(2))+border];
bkgdim = bkgdim(ylim(1):ylim(2),xlim(1):xlim(2),:);
x = x - xlim(1)+1;
y = y - ylim(1)+1;

%% colors for each frame
colors_all = nan(ts(end)-ts(1)+1,3,nflygroups);
for i = 1:nflygroups,
  cmcurr = cm{min(numel(cm),i)};
  if isempty(cmcurr),
    colors_all(:,:,i) = 1;
  elseif isnumeric(cmcurr),
    colors_all(:,:,i) = cmcurr;
  elseif ~isempty(ncolors_reference),
    colors_reference = cmcurr(ncolors_reference);
    idxcurr = round(linspace(1,ncolors_reference,ts(end)-ts(1)+1));
    colors_all(:,:,i) = colors_reference(idxcurr,:);
  else
    colors_all(:,:,i) = cmcurr(ts(end)-ts(1)+1);
  end
end
colors = colors_all(ts-ts(1)+1,:,:);

%% read in the frames, convert to grayscale
for i = 1:nframes,
  t = ts(i);
  imcurr = readframe(t);
  imcurr = im2double(imcurr(ylim(1):ylim(2),xlim(1):xlim(2),:));
  if ncolors > 1,
    grayim_curr = rgb2gray(imcurr);
  else
    grayim_curr = imcurr;
    imcurr = repmat(imcurr,[1,1,3]);
  end
  if i == 1,
    ims = repmat(imcurr,[1,1,1,nframes]);
    gray_ims = repmat(grayim_curr,[1,1,nframes]);
  else
    ims(:,:,:,i) = imcurr;
    gray_ims(:,:,i) = grayim_curr;
  end
end

%% probability that each pixel is foreground
dbkgd = permute(sum(abs(bsxfun(@minus,ims,bkgdim)),3),[1,2,4,3])/3;
pback = exp(-dbkgd/sigma_bkgd^2/2);
pback = pback / max(pback(:));
minv = exp(-fg_thresh/sigma_bkgd^2/2);
maxv = exp(-bg_thresh/sigma_bkgd^2/2);
pback = min(1,max(0,(pback-minv) / (maxv-minv)));
pfore = 1-pback;

%% make a connectivity graph for computing distances within foreground
% pixels
nr = ylim(2)-ylim(1)+1;
nc = xlim(2)-xlim(1)+1;
np = nr*nc;

[cgrid,rgrid] = meshgrid(1:nc,1:nr);
rconn1 = repmat(rgrid,[1,1,4]);
cconn1 = repmat(cgrid,[1,1,4]);
rconn2 = nan(size(rconn1));
cconn2 = nan(size(cconn1));

rconn2(:,:,1) = rgrid+1; % above
cconn2(:,:,1) = cgrid;
rconn2(:,:,2) = rgrid-1; % below
cconn2(:,:,2) = cgrid;
rconn2(:,:,3) = rgrid; % right
cconn2(:,:,3) = cgrid+1;
rconn2(:,:,4) = rgrid; % left
cconn2(:,:,4) = cgrid-1;
badidx = rconn2 < 1 | rconn2 > nr | cconn2 < 1 | cconn2 > nc;
rconn1(badidx) = [];
cconn1(badidx) = [];
rconn2(badidx) = [];
cconn2(badidx) = [];
conn1 = sub2ind([nr,nc],rconn1,cconn1);
conn2 = sub2ind([nr,nc],rconn2,cconn2);

%isdy = conn2 == conn1+1 | conn2 == conn1-1;
%w = min(maxdist,-log(pfore)+dist_epsilon)/maxdist;
w = reshape((pback+dist_epsilon)/(1+dist_epsilon),[np,nframes]);

%% assign pixels to flies

% probability that each pixel belongs to each fly
pfly = zeros(nr*nc,trx.nflies,nframes);
%allflies = [flygroups{:}];

for ii = 1:nframes,
  t = ts(ii);
  i = t-ts(1)+1;
  for fly = 1:trx.nflies,
    if t > trx(fly).endframe || t < trx(fly).firstframe,
      continue;
    end
    mu = [x(fly,i)-frac_a_back*a(fly,i)*cos(theta(fly,i)),...
      y(fly,i)-frac_a_back*a(fly,i)*sin(theta(fly,i))];
%     mu = [trx(fly).x(j)-frac_a_back*trx(fly).a(j)*cos(trx(fly).theta(j)),...
%       trx(fly).y(j)-frac_a_back*trx(fly).a(j)*sin(trx(fly).theta(j))];

    s = sub2ind([nr,nc],min(nr,max(1,round(mu(2)))),...
      min(nc,max(1,round(mu(1)))));
    wcurr = w(conn2,ii);
    S = axes2cov(a(fly,i),b(fly,i),theta(fly,i));
    diffs = bsxfun(@minus,[cgrid(:),rgrid(:)],mu);
    c = chol(S);
    dmah = sum((diffs/c).^2',1); %#ok<UDIM>
%     ratio = S(1,1)/S(2,2);
%     rationorm = 2 / (ratio+1);
%     wcurr(isdy) = wcurr(isdy)*ratio*rationorm;
%     wcurr(~isdy) = wcurr(~isdy)*rationorm;
    G = sparse(conn1,conn2,wcurr,np,np);
    d = graphshortestpath(G,s,'Directed',true);
    %pfly(:,fly,i) = exp(-d.^2/(trx(fly).a(j)*dist_epsilon)^2);
    pfly(:,fly,ii) = exp(-(1-wmah)*d.^2/(a(fly,i)*dist_epsilon)^2 - wmah*dmah);
  end
end
Z = sum(pfly,2);
Z(Z==0) = 1;
pfly = bsxfun(@rdivide,pfly,Z);

gray_ims = reshape(gray_ims,[nr*nc,nframes]);
pfore = reshape(pfore,[nr*nc,nframes]);
psomefly = reshape(1-prod(1-pfly(:,[flygroups{:}],:),2),[nr*nc,nframes]);
pforefly = pfore.*psomefly;
%pfore_any = 1 - prod(1-pfore.*psomefly,2);

%% color the images

im = zeros(nr*nc,3,nframes);
if flipim,
  if intensityoff > 0,
    gray_ims = intensityoff+(1 - gray_ims)*(1-intensityoff);
  end
end
for i = 1:nframes,
  imcurr = zeros(nr*nc,3);
  for flygroupi = 1:nflygroups,
    flygroup = flygroups{flygroupi};
    pcurr = 1 - prod(1-pfly(:,flygroup,i),2);
    colorcurr = colors(i,:,flygroupi);
    imcurr = imcurr + bsxfun(@times,colorcurr,pcurr);
  end
  if ~isempty(solidbkgdcolor),
    imcurr = bsxfun(@times,imcurr,gray_ims(:,i).*pforefly(:,i)) + ...
      bsxfun(@times,1-pforefly(:,i),repmat(reshape(solidbkgdcolor,[1,3]),[nr*nc,1]));
  else
    imcurr = bsxfun(@plus,bsxfun(@times,imcurr,gray_ims(:,i).*pforefly(:,i)),...
      (1-pforefly(:,i)).*reshape(bkgdim,[nr*nc,1]));
  end
  im(:,:,i) = imcurr;
end
finalim = reshape(im,[nr,nc,3,nframes]);

final_pforefly = reshape(pforefly,[nr,nc,nframes]);

%%

if isempty(hfig),
  hfig = figure;
end

figure(hfig);
clf;
hax = createsubplots(1,numel(ts),.01);

idxpos = predictions{mainfly}(ts(1):ts(end))>0;
idxneg = predictions{mainfly}(ts(1):ts(end))==0;
idxunlabeled = predictions{mainfly}(ts(1):ts(end))<0;

imid = ceil(numel(ts)/2);
try
  timestamps = trx(mainfly).timestamps(ts+trx(mainfly).off)-...
    trx(mainfly).timestamps(ts(imid)+trx(mainfly).off);
catch
  timestamps = [0,cumsum(trx(mainfly).dt)];
  timestamps = timestamps(ts+trx(mainfly).off)-...
    timestamps(ts(imid)+trx(mainfly).off);
end

for i = 1:numel(ts),
  t = ts(i);
  image(finalim(:,:,:,i),'Parent',hax(i));
  axis(hax(i),'image','off');
  hold(hax(i),'on');
  plot(hax(i),x(mainfly,:),y(mainfly,:),'k.-');
  plot(hax(i),x(mainfly,idxunlabeled),y(mainfly,idxunlabeled),'.','Color','w');
  plot(hax(i),x(mainfly,idxneg),y(mainfly,idxneg),'.','Color',colorneg);
  plot(hax(i),x(mainfly,idxpos),y(mainfly,idxpos),'.','Color',colorpos);
  if predictions{mainfly}(t)>0
    colorcurr = colorpos;
  elseif predictions{mainfly}(t)==0,
    colorcurr = colorneg;
  else
    colorcurr = [1,1,1];
  end
  plot(hax(i),x(mainfly,t-ts(1)+1),y(mainfly,t-ts(1)+1),'o','color',colorcurr,'markerfacecolor',colorcurr);
  text(1,1,sprintf('t = %.2fs',timestamps(i)),'HorizontalAlignment','left','VerticalAlignment','top','Parent',hax(i));
end

axes(hax(end));
colormap(colors_all(:,:,1)*.75);
tlim = timestamps(end)-timestamps(1);
set(hax,'CLim',[0,tlim]);
hcb = colorbar('East');
tticks = [0,tlim];
realylim = get(hcb,'YLim');
yticks = realylim(1)+(tticks/tlim)*(realylim(2)-realylim(1));
set(hcb,'YTick',yticks,'YTickLabel',tticks');

if isempty(figpos),
  truesize;
else
  set(hfig,'Units','pixels','Position',figpos);
end


function x = mods235(p)
  n=2;

  x(1)=0.1*(p(1)-1.0);
  x(2)=p(2)-p(1)*p(1);


function [mix, num_iter, ll] = gmmem_kpm(mix, x, varargin)
%GMMEM_KPM Like GMMEM, but with additional optional arguments
% function [mix, num_iter, ll] = gmmem_kpm(mix, x, varargin)
%
% Input:
% mix - structure created by gmminit or gmmem_multi_restart
% data - each row is an example
%
% Output:
% mix - modified structure
% num_iter - number of iterations needed to reach convergence
% ll - final log likelihood
%
% [ ... ] = gmmem_kpm(..., 'param1',val1, 'param2',val2, ...) allows you to
% specify optional parameter name/value pairs.
% Parameters are below [default value in brackets]
%
% 'max_iter' - maximum number of EM iterations [10]
% 'll_thresh' - change in log-likelihood threshold for convergence [1e-2]
% 'verbose' - 1 means display output while running [0]
% 'prior_cov' - this will be added to each estimated covariance
%               to prevent singularities  [1e-3*eye(d)]
% 'fn'        - this function, if non-empty,  will be called at every iteration
%               (e.g., to display the parameters as they evolve) [ [] ]
%               The fn is called as fn(mix, x, iter_num, fnargs).
%               It is also called before the iteration starts as
%               fn(mix, x, -1, fnargs), which can be used to initialize things.
% 'fnargs'    - additional arguments to be passed to fn [ {} ]
%
% Modified by Kevin P Murphy, 29 Dec 2002

% Check that inputs are consistent
errstring = consist(mix, 'gmm', x);
if ~isempty(errstring)
  error(errstring);
end

[ndata, xdim] = size(x);

[max_iter, ll_thresh, verbose, prior_cov, fn, fnargs] = ...
    process_options(varargin, ...
	'max_iter', 10, 'll_thresh', 1e-2, 'verbose', 1, ...
	'prior_cov', 1e-3*eye(xdim), 'fn', [], 'fnargs', {});

options = foptions;
if verbose, options(1)=1; else options(1)=-1; end
options(14) = max_iter;
options(3) = ll_thresh;

% Sort out the options
if (options(14))
  niters = options(14);
else
  niters = 100;
end

display = options(1);
test = 0;
if options(3) > 0.0
  test = 1;	% Test log likelihood for termination
end

check_covars = 0;
if options(5) >= 1
  if display >= 0
    disp('check_covars is on');
  end
  check_covars = 1;	% Ensure that covariances don't collapse
  MIN_COVAR = eps;	% Minimum singular value of covariance matrix
  init_covars = mix.covars;
end

mix0 = mix; % save init values for debugging

if ~isempty(fn)
  feval(fn, mix, x, -1, fnargs{:});
end

% Main loop of algorithm
for n = 1:niters

  % Calculate posteriors based on old parameters
  [post, act] = gmmpost(mix, x);

  % Calculate error value if needed
  if (display |  test)
    prob = act*(mix.priors)';
    % Error value is negative log likelihood of data
    e = - sum(log(prob + eps));
    if display > 0
      fprintf(1, 'Cycle %4d  Error %11.6f\n', n, e);
    end
    if test
      if (n > 1 & abs(e - eold) < options(3))
        options(8) = e;
	ll = -e;
	num_iter = n;
        return; %%%%%%%%%%%%%%%% Exit here if converged
      else
        eold = e;
      end
    end
  end

  if ~isempty(fn)
    feval(fn, mix, x, n, fnargs{:});
  end

  % Adjust the new estimates for the parameters
  new_pr = sum(post, 1);
  new_c = post' * x;

  % Now move new estimates to old parameter vectors
  mix.priors = new_pr ./ ndata;

  mix.centres = new_c ./ (new_pr' * ones(1, mix.nin));

  switch mix.covar_type
  case 'spherical'
    n2 = dist2(x, mix.centres);
    for j = 1:mix.ncentres
      v(j) = (post(:,j)'*n2(:,j));
    end
    mix.covars = ((v./new_pr) + sum(diag(prior_cov)))./mix.nin;
    if check_covars
      % Ensure that no covariance is too small
      for j = 1:mix.ncentres
        if mix.covars(j) < MIN_COVAR
          mix.covars(j) = init_covars(j);
        end
      end
    end
  case 'diag'
    for j = 1:mix.ncentres
      diffs = x - (ones(ndata, 1) * mix.centres(j,:));
      wts = (post(:,j)*ones(1, mix.nin));
      mix.covars(j,:) = sum((diffs.*diffs).*wts + prior_cov, 1)./new_pr(j);
    end
    if check_covars
      % Ensure that no covariance is too small
      for j = 1:mix.ncentres
        if min(mix.covars(j,:)) < MIN_COVAR
          mix.covars(j,:) = init_covars(j,:);
        end
      end
    end
  case 'full'
    for j = 1:mix.ncentres
      diffs = x - (ones(ndata, 1) * mix.centres(j,:));
      diffs = diffs.*(sqrt(post(:,j))*ones(1, mix.nin));
      mix.covars(:,:,j) = (diffs'*diffs + prior_cov)/new_pr(j);
    end
    if check_covars
      % Ensure that no covariance is too small
      for j = 1:mix.ncentres
        if min(svd(mix.covars(:,:,j))) < MIN_COVAR
          mix.covars(:,:,j) = init_covars(:,:,j);
        end
      end
    end
  case 'ppca'
    for j = 1:mix.ncentres
      diffs = x - (ones(ndata, 1) * mix.centres(j,:));
      diffs = diffs.*(sqrt(post(:,j))*ones(1, mix.nin));
      [mix.covars(j), mix.U(:,:,j), mix.lambda(j,:)] = ...
        ppca((diffs'*diffs)/new_pr(j), mix.ppca_dim);
    end
    if check_covars
      if mix.covars(j) < MIN_COVAR
        mix.covars(j) = init_covars(j);
      end
    end
    otherwise
      error(['Unknown covariance type ', mix.covar_type]);
  end
end

ll = sum(log(gmmprob(mix, x)));
num_iter = n;

%if (display >= 0)
%  disp('Warning: Maximum number of iterations has been exceeded');
%end


filename='Cantileverbeam_Tetrahedra_Linear_Structured_SYM';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance'};
weights = [1];
constraint = {'volume'};
optimizer = 'MMA';
incrementFactor = 1;
designVariable = 'Density';
filterType = 'P1';
constraint_case = 'INEQUALITY';
line_search_initiator = 'INCREASING LAST STEP';
showBC = true;

nsteps = 10;
Vfrac_final = 0.1;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-3;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


function [MStitch, result] = RGBListMain_Process(file)

for i = 1 : length(file)-1
    if i == 1
        file1 = file{i};
        im1 = imread(file1);
        im1 = rgb2gray(im1);
        MStitch.im1 = double(im1);

        [Pheight, Pwidth] = size(im1);

        MStitch.Pwidth = Pwidth;
        MStitch.Pheight = Pheight;

        MStitch.W_min = round(0.60*Pwidth);
        MStitch.W_max = round(0.83*Pwidth);
        MStitch.H_min = round(0.98*Pheight);
        MStitch.minval = 255;

        im1 = imread(file1);
        MStitch.imrgb1 = double(im1);
        im1 = rgb2gray(im1);
        MStitch.im1 = double(im1);
    else
        MStitch.im1 = double(result1);
    end
    file2 = file{i+1};
    im2 = imread(file2);
    MStitch.imrgb2 = double(im2);
    im2 = rgb2gray(im2);
    im2 = double(im2);
    [W_box, H_box, bdown, MStitch] = Fun_Match(im2, MStitch);
    [MStitch, result1] = Fun_StitchRGB(im2, W_box, H_box, bdown, MStitch);
end
result = result1;


filename='Cantileverbeam_Tetrahedra_Linear_Structured_SYM';
ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'holes';
cost = {'compliance'};
weights = [1];
constraint = {'volume'};
optimizer = 'HAMILTON-JACOBI';
incrementFactor = 1;
designVariable = 'LevelSet';
filterType = 'P1';
constraint_case = 'INEQUALITY';
line_search_initiator = 'INCREASING LAST STEP';
showBC = true;

HJiter0 = 1;
e2 = 100;
N_holes = [12 5 5];
R_holes = 0.2;
phase_holes = [0 0 0];
nsteps = 10;
Vfrac_final = 0.1;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 0.7;
optimality_initial = 5e-2;
constr_initial = 5e-2;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


%-------------------------------------------------------------------------------------------------------------------%
%
% 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:  nickabattista@gmail.com
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs, non-invariant beams*)
% 	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 (nickabattista@gmail.com) 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

%RL = springs_info(:,4); % resting-length vector

% Contraction Frequency
freq = 1.0;

% CHANGE RESTING LENGTH BTWN SIDES OF JELLYFISH BELL
springs_info(319:end,4) = abs( cos(freq*current_time*pi) );

%NOTE: not 2*pi*ft b/c of abs()


%-------------------------------------------------------------------------------------------------------------------%
%
% 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:  nickabattista@gmail.com
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs, non-invariant beams*)
% 	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 (nickabattista@gmail.com) 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

%RL = springs_info(:,4); % resting-length vector

% Contraction Frequency
freq = 1.0;

% CHANGE RESTING LENGTH BTWN SIDES OF JELLYFISH BELL
springs_info(637:end,4) = abs( cos(freq*current_time*pi) );

%NOTE: not 2*pi*ft b/c of abs()


classdef LiftForceModel < AbstractForceModel
    %LiftForceModel Summary of this class goes here
    %   Detailed explanation goes here

    properties

    end

    methods
        function obj = LiftForceModel()

        end

        function [forceVect, tankMdots, ecStgDots] = getForce(obj, ut, rVect, vVect, mass, bodyInfo, aero, ~, ~, ~, ~, ~, ~, ~, ~, ~, ~, attState)
            if(norm(rVect) - (bodyInfo.radius + bodyInfo.atmohgt) > 0)
                forceVect = [0;0;0];
            else
                forceVect = getLiftForce(bodyInfo, ut, rVect, vVect, aero, mass, attState);
            end

            tankMdots = [];
            ecStgDots = [];
        end
    end
end

function forceVect = getLiftForce(bodyInfo, ut, rVectECI, vVectECI, aero, mass, attState)
    arguments
        bodyInfo(1,1) KSPTOT_BodyInfo
        ut(1,1) double
        rVectECI(3,1) double
        vVectECI(3,1) double
        aero(1,1) LaunchVehicleAeroState
        mass(1,1) double
        attState(1,1) LaunchVehicleAttitudeState
    end

    rVectECI = reshape(rVectECI,3,1);
    vVectECI = reshape(vVectECI,3,1);

    altitude = norm(rVectECI) - bodyInfo.radius;

    if(altitude <= bodyInfo.atmohgt && altitude >= 0)
        [lat, long, ~, ~, ~, ~, ~, vVectECEF] = getLatLongAltFromInertialVect(ut, rVectECI, bodyInfo, vVectECI);
        [density, pressureKPA, ~] = getAtmoDensityAtAltitude(bodyInfo, altitude, lat, ut, long);
    elseif(altitude <= 0)
        density = 0;
    else
        density = 0;
    end

    if(density > 0)
        [ClS, liftUnitVectInertial] = aero.getLiftCoeffAndDir(ut, rVectECI, vVectECI, bodyInfo, mass, altitude, pressureKPA, density, vVectECEF, attState);

        vVectECEFMag = norm(vVectECEF);

        %all forces are returned in units of mT*km/s^2 = (1000000)*N
        FL = (1/2)*density*(vVectECEFMag^2)*ClS; %kg/m^3 * (km^2/s^2) * m^2 = kg/m * km^2/s^2 = kg*(km/m)*km/s^2 = kg*(1000)*km/s^2

        forceVect = FL * liftUnitVectInertial;

%         Cl_level = aero.Cl_0;
%         A = aero.areaLift;
%         bodyLiftVect = normVector(aero.bodyLiftVect);
%
%         vVectEcefMag = norm(vVectECEF);
%
%         body2InertDcm = steeringModel.getBody2InertialDcmAtTime(ut, rVectECI, vVectECI, bodyInfo);
%         [~,angOfAttack,~] = computeAeroAnglesFromBodyAxes(ut, rVectECI, vVectECI, bodyInfo, body2InertDcm(:,1), body2InertDcm(:,2), body2InertDcm(:,3));
%
%         Cl = Cl_level + 2*pi*angOfAttack;
%
%         ClA = Cl * A;
%
%         L = (1/2) * density * (vVectEcefMag^2) * ClA; %kg/m^3 * (km^2/s^2) * m^2 = kg/m * km^2/s^2 = kg*(km/m)*km/s^2 = kg*(1000)*km/s^2
%
%         bodyLiftForce = L * bodyLiftVect;
%
%         liftForce = body2InertDcm * bodyLiftForce;
    else
        forceVect = [0;0;0];
    end
end


function [g,dgdx,dgdp] = g_GLM_missingData(x,P,u,in)
X = in.X;
md = P(in.md);
b = P(in.b);
X(in.xmd) = md;
g = X*b;
n = length(g);
dgdx = [];
dgdp = zeros(length(P),n);
dgdp(in.b,:) = X';
nmd = length(in.md);
A = zeros(n,nmd);
A([0:nmd-1].*n+mod(in.xmd,n)) = b(ceil(in.xmd./n));
dgdp(in.md,:) = A';


function [post nlZ dnlZ] = infGrid(hyp, mean, cov, lik, x, y, opt)

% Inference for a GP with Gaussian likelihood and covGrid covariance.
% The (Kronecker) covariance matrix used is given by:
%   K = kron( kron(...,K{2}), K{1} ) = K_p x .. x K_2 x K_1.
%
% Compute a parametrization of the posterior, the negative log marginal
% likelihood and its derivatives w.r.t. the hyperparameters.
% The result is exact for complete grids, otherwise results are approximate.
% See also "help infMethods".
%
% The function takes a specified covariance function (see covFunctions.m) and
% likelihood function (see likFunctions.m), and is designed to be used with
% gp.m and in conjunction with covFITC and likGauss.
%
% Copyright (c) by Hannes Nickisch and Andrew Wilson, 2014-12-03.
%
% See also INFMETHODS.M, COVGRID.M.

if iscell(lik), likstr = lik{1}; else likstr = lik; end
if ~ischar(likstr), likstr = func2str(likstr); end
if ~strcmp(likstr,'likGauss')               % NOTE: no explicit call to likGauss
  error('Inference with infGrid only possible with Gaussian likelihood.');
end
cov1 = cov{1}; if isa(cov1, 'function_handle'), cov1 = func2str(cov1); end
if ~strcmp(cov1,'covGrid'); error('Only covGrid supported.'), end    % check cov

xg = cov{3}; p = numel(xg);                                    % underlying grid
N = 1; D = 0; for i=1:p, N = N*size(xg{i},1); D = D+size(xg{i},2); end    % dims
[K,M,xe] = feval(cov{:}, hyp.cov, x);     % evaluate covariance mat constituents
xe = M*xe; n = size(xe,1);
m = feval(mean{:}, hyp.mean, xe);                         % evaluate mean vector

if nargin<=6, opt = []; end                        % make opt variable available
if isfield(opt,'cg_tol'),   cgtol = opt.cg_tol;       % stop conjugate gradients
else cgtol = 1e-5; end
if isfield(opt,'cg_maxit'), cgmit = opt.cg_maxit;      % number of cg iterations
else cgmit = min(n,20); end
for j=1:numel(K)
  if iscell(K{j}) && strcmp(K{j}{1},'toep'), K{j} = toeplitz(K{j}{2}); end
end

sn2 = exp(2*hyp.lik);                               % noise variance of likGauss
V = cell(size(K)); E = cell(size(K));                 % eigenvalue decomposition
for j=1:numel(K)
  if iscell(K{j}) && strcmp(K{j}{1},'toep')
       [V{j},E{j}] = eigr(toeplitz(K{j}{2}));
  else [V{j},E{j}] = eigr(K{j});
  end
end
e = 1; for i=1:numel(E), e = kron(diag(E{i}),e); end       % eigenvalue diagonal
if numel(m)==N
  s = 1./(e+sn2); ord = 1:N;                  % V*diag(s)*V' = inv(K+sn2*eye(N))
else
  [eord,ord] = sort(e,'descend');              % sort long vector of eigenvalues
  s = 1./((n/N)*eord(1:n) + sn2);               % approx using top n eigenvalues
end

if numel(m)==N               % decide for Kronecker magic or conjugate gradients
  L = @(k) kronmvm(V,repmat(-s,1,size(k,2)).*kronmvm(V,k,1));     % mvm callback
else                                                % go for conjugate gradients
  mvm = @(t) mvmK(t,K,sn2,M);                             % single parameter mvm
  L = @(k) -solveMVM(k,mvm,cgtol,cgmit);
end
alpha = -L(y-m);

post.alpha = alpha;                            % return the posterior parameters
post.sW = ones(n,1)/sqrt(sn2);                         % sqrt of noise precision
post.L = L;                           % function to compute inv(K+sn2*eye(N))*Ks

if nargout>1                               % do we want the marginal likelihood?
  lda = -sum(log(s));                                               % exact kron
  % exact: lda = 2*sum(log(diag(chol(M*kronmvm(K,eye(N))*M'+sn2*eye(n)))));
  nlZ = (y-m)'*alpha/2 + n*log(2*pi)/2 + lda/2;
  if nargout>2                                         % do we want derivatives?
    dnlZ = hyp;     % allocate space for derivatives, define Q=inv(M*K*M'+sn2*I)
    Mtal = M'*alpha;                          % blow up alpha vector from n to N
    for i = 1:numel(hyp.cov)
      dK = feval(cov{:}, hyp.cov, x, [], i);
      for j=1:numel(dK)                                        % expand Toeplitz
        if iscell(dK{j}) && strcmp(dK{j}{1},'toep')
          dK{j} = toeplitz(dK{j}{2});
        end
      end
      P = cell(size(dK));               % auxiliary Kronecker matrix P = V'*dK*V
      for j = 1:numel(dK), P{j} = sum((V{j}'*dK{j}).*V{j}',2); end
      p = 1; for j=1:numel(P), p = kron(P{j},p); end               % p = diag(P)
      p = (n/N)*p(ord(1:n));        % approximate if incomplete grid observation
      dnlZ.cov(i) = (p'*s - Mtal'*kronmvm(dK,Mtal))/2;
    end
    dnlZ.lik = sn2*(sum(s) - alpha'*alpha);                  % sum(s) = trace(Q)
    for i = 1:numel(hyp.mean)
      dnlZ.mean(i) = -feval(mean{:}, hyp.mean, xe, i)'*alpha;
    end
  end
end

% Solve x=A*b with symmetric A(n,n), b(n,m), x(n,m) using conjugate gradients.
% The method is along the lines of PCG but suited for matrix inputs b.
function [x,flag,relres,iter,r] = conjgrad(A,b,tol,maxit)
if nargin<3, tol = 1e-10; end
if nargin<4, maxit = min(size(b,1),20); end
x0 = zeros(size(b)); x = x0;
if isnumeric(A), r = b-A*x; else r = b-A(x); end, r2 = sum(r.*r,1); r2new = r2;
nb = sqrt(sum(b.*b,1)); flag = 0; iter = 1;
relres = sqrt(r2)./nb; todo = relres>=tol; if ~any(todo), flag = 1; return, end
on = ones(size(b,1),1); r = r(:,todo); d = r;
for iter = 2:maxit
  if isnumeric(A), z = A*d; else z = A(d); end
  a = r2(todo)./sum(d.*z,1);
  a = on*a;
  x(:,todo) = x(:,todo) + a.*d;
  r = r - a.*z;
  r2new(todo) = sum(r.*r,1);
  relres = sqrt(r2new)./nb; cnv = relres(todo)<tol; todo = relres>=tol;
  d = d(:,~cnv); r = r(:,~cnv);                           % get rid of converged
  if ~any(todo), flag = 1; return, end
  b = r2new./r2;                                               % Fletcher-Reeves
  d = r + (on*b(todo)).*d;
  r2 = r2new;
end

% solve q = mvm(p) via conjugate gradients
function q = solveMVM(p,mvm,varargin)
  [q,flag,relres,iter] = conjgrad(mvm,p,varargin{:});                 % like pcg
  if ~flag,error('Not converged after %d iterations, r=%1.2e\n',iter,relres),end

% mvm so that q = M*K*M'*p + sn2*p using the Kronecker representation
function q = mvmK(p,K,sn2,M)
  q = M*kronmvm(K,M'*p) + sn2*p;

% Perform a matrix vector multiplication b = A*x with a matrix A being a
% Kronecker product given by A = kron( kron(...,As{2}), As{1} ).
function b = kronmvm(As,x,transp)
if nargin>2 && ~isempty(transp) && transp   % transposition by transposing parts
  for i=1:numel(As), As{i} = As{i}'; end
end
m = zeros(numel(As),1); n = zeros(numel(As),1);                  % extract sizes
for i=1:numel(n)
  if iscell(As{i}) && strcmp(As{i}{1},'toep')
    m(i) = size(As{i}{2},1); n(i) = size(As{i}{2},1);
  else [m(i),n(i)] = size(As{i});
  end
end
d = size(x,2);
b = x;
for i=1:numel(n)
  a = reshape(b,[prod(m(1:i-1)), n(i), prod(n(i+1:end))*d]);    % prepare  input
  tmp = reshape(permute(a,[1,3,2]),[],n(i))*As{i}';
  b = permute(reshape(tmp,[size(a,1),size(a,3),m(i)]),[1,3,2]);
end
b = reshape(b,prod(m),d);                        % bring result in correct shape

% Real eigenvalues and eigenvectors up to the rank of a real symmetric matrix.
% Decompose A into V*D*V' with orthonormal matrix V and diagonal matrix D.
% Diagonal entries of D obave the rank r of the matrix A as returned by
% the call rank(A,tol) are zero.
function [V,D] = eigr(A,tol)
[V,D] = eig((A+A')/2); n = size(A,1);    % decomposition of strictly symmetric A
d = max(real(diag(D)),0); [d,ord] = sort(d,'descend');        % tidy up and sort
if nargin<2, tol = size(A,1)*eps(max(d)); end, r = sum(d>tol);     % get rank(A)
d(r+1:n) = 0; D = diag(d);                % set junk eigenvalues to strict zeros
V(:,1:r) = real(V(:,ord(1:r))); V(:,r+1:n) = null(V(:,1:r)'); % ortho completion


%%*****************************************************************
%% schurmat_lblk: compute A*D*A'
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

   function [schur,UU,EE] = schurmat_lblk(blk,At,par,schur,UU,EE,p,dd)

   global idxdenAl

   iter = par.iter;
   n = sum(blk{p,2});

   if (iter==1)
      idxdenAl{p} = checkdense(At{p}');
   end
   ddsch = dd{p};
   if ~isempty(idxdenAl{p});
      idxden = idxdenAl{p};
      len = length(idxden);
      Ad = At{p}(idxden,:)' *spdiags(sqrt(ddsch(idxden)),0,len,len);
      UU = [UU, Ad];
      if isempty(EE)
         count = 0;
      else
         count = max(max(EE(:,1)),max(EE(:,2)));
      end
      tmp = count + [1:len]';
      EE = [EE; [tmp, tmp, -ones(len,1)] ];
      ddsch(idxden) = zeros(len,1);
   end
   schurtmp = At{p}' *spdiags(ddsch,0,n,n) *At{p};
   schur = schur + schurtmp;
%%*******************************************************************


function Y = squeeze(X)
%SQUEEZE Remove singleton dimensions from a tensor.
%
%   Y = SQUEEZE(X) returns a tensor Y with the same elements as
%   X but with all the singleton dimensions removed.  A singleton
%   is a dimension such that size(X,dim)==1.
%
%   If X has *only* singleton dimensions, then Y is a scalar.
%
%   Examples
%   squeeze( tenrand([2,1,3]) ) %<-- returns a 2-by-3 tensor
%   squeeze( tenrand([1 1]) ) %<-- returns a scalar
%
%   See also TENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

if all(X.size > 1)
  % No singleton dimensions to squeeze
  Y = X;
else
  idx = find(X.size > 1);
  if numel(idx) == 0
    % Scalar case - only singleton dimensions
    Y = X.data;
  else
    siz = X.size(idx);
    Y = tensor(squeeze(X.data),siz);
  end
end

return;


%% DEMO_febio_0025_cube_uniaxial_stiffness analysis
% Below is a demonstration for:
%
% * Building geometry for a cube with hexahedral elements
% * Defining the boundary conditions
% * Coding the febio structure
% * Running the model
% * Importing and visualizing the displacement and stress results

%% Keywords
%
% * febio_spec version 3.0
% * febio, FEBio
% * uniaxial loading
% * compression, tension, compressive, tensile
% * displacement control, displacement boundary condition
% * hexahedral elements, hex8
% * cube, box, rectangular
% * static, solid
% * hyperelastic, Ogden
% * displacement logfile
% * stress logfile

%%

clear; close all; clc;

%% Plot settings
fontSize=20;
faceAlpha1=0.8;
markerSize=40;
markerSize2=35;
lineWidth=3;
cMap=viridis(20); %colormap

%% Control parameters

% Path names
defaultFolder = fileparts(fileparts(mfilename('fullpath')));
savePath=fullfile(defaultFolder,'data','temp');

% Defining file names
febioFebFileNamePart='tempModel';
febioFebFileName=fullfile(savePath,[febioFebFileNamePart,'.feb']); %FEB file name
febioLogFileName=[febioFebFileNamePart,'.txt']; %FEBio log file name
febioLogFileName_disp=[febioFebFileNamePart,'_disp_out.txt']; %Log file name for exporting displacement
febioLogFileName_stress=[febioFebFileNamePart,'_stress_out.txt']; %Log file name for exporting stress
febioLogFileName_stress_prin=[febioFebFileNamePart,'_stress_prin_out.txt']; %Log file name for exporting principal stress
febioLogFileName_stiffness=[febioFebFileNamePart,'_stiffness_out.txt']; %Log file name for exporting stiffness

%Specifying dimensions and number of elements
cubeSize=10;
sampleWidth=cubeSize; %Width
sampleThickness=cubeSize; %Thickness
sampleHeight=cubeSize; %Height
pointSpacings=2*ones(1,3); %Desired point spacing between nodes
numElementsWidth=round(sampleWidth/pointSpacings(1)); %Number of elemens in dir 1
numElementsThickness=round(sampleThickness/pointSpacings(2)); %Number of elemens in dir 2
numElementsHeight=round(sampleHeight/pointSpacings(3)); %Number of elemens in dir 3

%Define applied displacement
appliedStrain=0.4; %Linear strain (Only used to compute applied stretch)
loadingOption='compression'; % or 'tension'
switch loadingOption
    case 'compression'
        stretchLoad=1-appliedStrain; %The applied stretch for uniaxial loading
    case 'tension'
        stretchLoad=1+appliedStrain; %The applied stretch for uniaxial loading
end
displacementMagnitude=(stretchLoad*sampleHeight)-sampleHeight; %The displacement magnitude

%Material parameter set
E_youngs1=0.1; %Material Young's modulus
nu1=0.4; %Material Poisson's ratio

% FEA control settings
numTimeSteps=10; %Number of time steps desired
max_refs=25; %Max reforms
max_ups=0; %Set to zero to use full-Newton iterations
opt_iter=6; %Optimum number of iterations
max_retries=5; %Maximum number of retires
dtmin=(1/numTimeSteps)/100; %Minimum time step size
dtmax=1/numTimeSteps; %Maximum time step size

%% Creating model geometry and mesh
% A box is created with tri-linear hexahedral (hex8) elements using the
% |hexMeshBox| function. The function offers the boundary faces with
% seperate labels for the top, bottom, left, right, front, and back sides.
% As such these can be used to define boundary conditions on the exterior.

% Create a box with hexahedral elements
cubeDimensions=[sampleWidth sampleThickness sampleHeight]; %Dimensions
cubeElementNumbers=[numElementsWidth numElementsThickness numElementsHeight]; %Number of elements
outputStructType=2; %A structure compatible with mesh view
[meshStruct]=hexMeshBox(cubeDimensions,cubeElementNumbers,outputStructType);

%Access elements, nodes, and faces from the structure
E=meshStruct.elements; %The elements
V=meshStruct.nodes; %The nodes (vertices)
Fb=meshStruct.facesBoundary; %The boundary faces
Cb=meshStruct.boundaryMarker; %The "colors" or labels for the boundary faces
elementMaterialIndices=ones(size(E,1),1); %Element material indices

%%
% Plotting model boundary surfaces and a cut view

hFig=cFigure;

subplot(1,2,1); hold on;
title('Model boundary surfaces and labels','FontSize',fontSize);
gpatch(Fb,V,Cb,'k',faceAlpha1);
colormap(gjet(6)); icolorbar;
axisGeom(gca,fontSize);

hs=subplot(1,2,2); hold on;
title('Cut view of solid mesh','FontSize',fontSize);
optionStruct.hFig=[hFig hs];
meshView(meshStruct,optionStruct);
axisGeom(gca,fontSize);

drawnow;

%% Defining the boundary conditions
% The visualization of the model boundary shows colors for each side of the
% cube. These labels can be used to define boundary conditions.

%Define supported node sets
logicFace=Cb==1; %Logic for current face set
Fr=Fb(logicFace,:); %The current face set
bcSupportList_X=unique(Fr(:)); %Node set part of selected face

logicFace=Cb==3; %Logic for current face set
Fr=Fb(logicFace,:); %The current face set
bcSupportList_Y=unique(Fr(:)); %Node set part of selected face

logicFace=Cb==5; %Logic for current face set
Fr=Fb(logicFace,:); %The current face set
bcSupportList_Z=unique(Fr(:)); %Node set part of selected face

%Prescribed displacement nodes
logicPrescribe=Cb==6; %Logic for current face set
Fr=Fb(logicPrescribe,:); %The current face set
bcPrescribeList=unique(Fr(:)); %Node set part of selected face

%%
% Visualizing boundary conditions. Markers plotted on the semi-transparent
% model denote the nodes in the various boundary condition lists.

hf=cFigure;
title('Boundary conditions','FontSize',fontSize);
xlabel('X','FontSize',fontSize); ylabel('Y','FontSize',fontSize); zlabel('Z','FontSize',fontSize);
hold on;

gpatch(Fb,V,'kw','k',0.5);

hl(1)=plotV(V(bcSupportList_X,:),'r.','MarkerSize',markerSize);
hl(2)=plotV(V(bcSupportList_Y,:),'g.','MarkerSize',markerSize);
hl(3)=plotV(V(bcSupportList_Z,:),'b.','MarkerSize',markerSize);
hl(4)=plotV(V(bcPrescribeList,:),'k.','MarkerSize',markerSize);

legend(hl,{'BC x support','BC y support','BC z support','BC z prescribe'});

axisGeom(gca,fontSize);
camlight headlight;
drawnow;

%% Defining the FEBio input structure
% See also |febioStructTemplate| and |febioStruct2xml| and the FEBio user
% manual.

%Get a template with default settings
[febio_spec]=febioStructTemplate;

%febio_spec version
febio_spec.ATTR.version='3.0';

%Module section
febio_spec.Module.ATTR.type='solid';

%Control section
febio_spec.Control.analysis='STATIC';
febio_spec.Control.time_steps=numTimeSteps;
febio_spec.Control.step_size=1/numTimeSteps;
febio_spec.Control.solver.max_refs=max_refs;
febio_spec.Control.solver.max_ups=max_ups;
febio_spec.Control.time_stepper.dtmin=dtmin;
febio_spec.Control.time_stepper.dtmax=dtmax;
febio_spec.Control.time_stepper.max_retries=max_retries;
febio_spec.Control.time_stepper.opt_iter=opt_iter;

%Material section
materialName1='Material1';
febio_spec.Material.material{1}.ATTR.name=materialName1;
febio_spec.Material.material{1}.ATTR.type='neo-Hookean';
febio_spec.Material.material{1}.ATTR.id=1;
febio_spec.Material.material{1}.E=E_youngs1;
febio_spec.Material.material{1}.v=nu1;

% Mesh section
% -> Nodes
febio_spec.Mesh.Nodes{1}.ATTR.name='Object1'; %The node set name
febio_spec.Mesh.Nodes{1}.node.ATTR.id=(1:size(V,1))'; %The node id's
febio_spec.Mesh.Nodes{1}.node.VAL=V; %The nodel coordinates

% -> Elements
partName1='Part1';
febio_spec.Mesh.Elements{1}.ATTR.name=partName1; %Name of this part
febio_spec.Mesh.Elements{1}.ATTR.type='hex8'; %Element type
febio_spec.Mesh.Elements{1}.elem.ATTR.id=(1:1:size(E,1))'; %Element id's
febio_spec.Mesh.Elements{1}.elem.VAL=E; %The element matrix

% -> NodeSets
nodeSetName1='bcSupportList_X';
nodeSetName2='bcSupportList_Y';
nodeSetName3='bcSupportList_Z';
nodeSetName4='bcPrescribeList';

febio_spec.Mesh.NodeSet{1}.ATTR.name=nodeSetName1;
febio_spec.Mesh.NodeSet{1}.node.ATTR.id=bcSupportList_X(:);

febio_spec.Mesh.NodeSet{2}.ATTR.name=nodeSetName2;
febio_spec.Mesh.NodeSet{2}.node.ATTR.id=bcSupportList_Y(:);

febio_spec.Mesh.NodeSet{3}.ATTR.name=nodeSetName3;
febio_spec.Mesh.NodeSet{3}.node.ATTR.id=bcSupportList_Z(:);

febio_spec.Mesh.NodeSet{4}.ATTR.name=nodeSetName4;
febio_spec.Mesh.NodeSet{4}.node.ATTR.id=bcPrescribeList(:);

%MeshDomains section
febio_spec.MeshDomains.SolidDomain.ATTR.name=partName1;
febio_spec.MeshDomains.SolidDomain.ATTR.mat=materialName1;

%Boundary condition section
% -> Fix boundary conditions
febio_spec.Boundary.bc{1}.ATTR.type='fix';
febio_spec.Boundary.bc{1}.ATTR.node_set=nodeSetName1;
febio_spec.Boundary.bc{1}.dofs='x';

febio_spec.Boundary.bc{2}.ATTR.type='fix';
febio_spec.Boundary.bc{2}.ATTR.node_set=nodeSetName2;
febio_spec.Boundary.bc{2}.dofs='y';

febio_spec.Boundary.bc{3}.ATTR.type='fix';
febio_spec.Boundary.bc{3}.ATTR.node_set=nodeSetName3;
febio_spec.Boundary.bc{3}.dofs='z';

febio_spec.Boundary.bc{4}.ATTR.type='prescribe';
febio_spec.Boundary.bc{4}.ATTR.node_set=nodeSetName4;
febio_spec.Boundary.bc{4}.dof='z';
febio_spec.Boundary.bc{4}.scale.ATTR.lc=1;
febio_spec.Boundary.bc{4}.scale.VAL=displacementMagnitude;
febio_spec.Boundary.bc{4}.relative=0;

%LoadData section
% -> load_controller
febio_spec.LoadData.load_controller{1}.ATTR.id=1;
febio_spec.LoadData.load_controller{1}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{1}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{1}.points.point.VAL=[0 0; 1 1];

%Output section
% -> log file
febio_spec.Output.logfile.ATTR.file=febioLogFileName;
febio_spec.Output.logfile.node_data{1}.ATTR.file=febioLogFileName_disp;
febio_spec.Output.logfile.node_data{1}.ATTR.data='ux;uy;uz';
febio_spec.Output.logfile.node_data{1}.ATTR.delim=',';

febio_spec.Output.logfile.element_data{1}.ATTR.file=febioLogFileName_stress;
febio_spec.Output.logfile.element_data{1}.ATTR.data='sz';
febio_spec.Output.logfile.element_data{1}.ATTR.delim=',';

febio_spec.Output.logfile.element_data{2}.ATTR.file=febioLogFileName_stress_prin;
febio_spec.Output.logfile.element_data{2}.ATTR.data='s1;s2;s3';
febio_spec.Output.logfile.element_data{2}.ATTR.delim=',';

febio_spec.Output.logfile.element_data{3}.ATTR.file=febioLogFileName_stiffness;
febio_spec.Output.logfile.element_data{3}.ATTR.data=['cxxxx;',...
                                                     'cxxyy;','cyyyy;',...
                                                     'cxxzz;','cyyzz;','czzzz;',...
                                                     'cxxxy;','cyyxy;','czzxy;','cxyxy;',...
                                                     'cxxyz;','cyyyz;','czzyz;','cxyyz;','cyzyz;',...
                                                     'cxxxz;','cyyxz;','czzxz;','cxyxz;','cyzxz;','cxzxz'];
febio_spec.Output.logfile.element_data{3}.ATTR.delim=',';

%% Quick viewing of the FEBio input file structure
% The |febView| function can be used to view the xml structure in a MATLAB
% figure window.

%%
% |febView(febio_spec); %Viewing the febio file|

%% Exporting the FEBio input file
% Exporting the febio_spec structure to an FEBio input file is done using
% the |febioStruct2xml| function.

febioStruct2xml(febio_spec,febioFebFileName); %Exporting to file and domNode
%system(['gedit ',febioFebFileName,' &']);

%% Running the FEBio analysis
% To run the analysis defined by the created FEBio input file the
% |runMonitorFEBio| function is used. The input for this function is a
% structure defining job settings e.g. the FEBio input file name. The
% optional output runFlag informs the user if the analysis was run
% succesfully.

febioAnalysis.run_filename=febioFebFileName; %The input file name
febioAnalysis.run_logname=febioLogFileName; %The name for the log file
febioAnalysis.disp_on=1; %Display information on the command window
febioAnalysis.runMode='external';%'internal';
febioAnalysis.maxLogCheckTime=10; %Max log file checking time

[runFlag]=runMonitorFEBio(febioAnalysis);%START FEBio NOW!!!!!!!!

%% Import FEBio results

if runFlag==1 %i.e. a succesful run

    %%

    % Importing nodal displacements from a log file
    dataStruct=importFEBio_logfile(fullfile(savePath,febioLogFileName_disp),1,1);

    %Access data
    N_disp_mat=dataStruct.data; %Displacement
    timeVec=dataStruct.time; %Time

    %Create deformed coordinate set
    V_DEF=N_disp_mat+repmat(V,[1 1 size(N_disp_mat,3)]);

    %%
    % Plotting the simulated results using |anim8| to visualize and animate
    % deformations

    DN_magnitude=sqrt(sum(N_disp_mat(:,:,end).^2,2)); %Current displacement magnitude

    % Create basic view and store graphics handle to initiate animation
    hf=cFigure; %Open figure
    gtitle([febioFebFileNamePart,': Press play to animate']);
    title('Displacement magnitude [mm]','Interpreter','Latex')
    hp=gpatch(Fb,V_DEF(:,:,end),DN_magnitude,'k',1,2); %Add graphics object to animate
    hp.Marker='.';
    hp.MarkerSize=markerSize2;
    hp.FaceColor='interp';
    gpatch(Fb,V,0.5*ones(1,3),'none',0.25); %A static graphics object

    axisGeom(gca,fontSize);
    colormap(cMap); colorbar;
    caxis([0 max(DN_magnitude)]); caxis manual;
    axis(axisLim(V_DEF)); %Set axis limits statically
    view(140,30);
    camlight headlight;

    % Set up animation features
    animStruct.Time=timeVec; %The time vector
    for qt=1:1:size(N_disp_mat,3) %Loop over time increments
        DN_magnitude=sqrt(sum(N_disp_mat(:,:,qt).^2,2)); %Current displacement magnitude

        %Set entries in animation structure
        animStruct.Handles{qt}=[hp hp]; %Handles of objects to animate
        animStruct.Props{qt}={'Vertices','CData'}; %Properties of objects to animate
        animStruct.Set{qt}={V_DEF(:,:,qt),DN_magnitude}; %Property values for to set in order to animate
    end
    anim8(hf,animStruct); %Initiate animation feature
    drawnow;

    %%
    % Importing element stress from a log file
    dataStruct=importFEBio_logfile(fullfile(savePath,febioLogFileName_stress),1,1);

    %Access data
    E_stress_mat=dataStruct.data;

    %%
    % Plotting the simulated results using |anim8| to visualize and animate
    % deformations

    [CV]=faceToVertexMeasure(E,V,E_stress_mat(:,:,end));

    % Create basic view and store graphics handle to initiate animation
    hf=cFigure; %Open figure  /usr/local/MATLAB/R2020a/bin/glnxa64/jcef_helper: symbol lookup error: /lib/x86_64-linux-gnu/libpango-1.0.so.0: undefined symbol: g_ptr_array_copy

    gtitle([febioFebFileNamePart,': Press play to animate']);
    title('$\sigma_{zz}$ [MPa]','Interpreter','Latex')
    hp=gpatch(Fb,V_DEF(:,:,end),CV,'k',1,2); %Add graphics object to animate
    hp.Marker='.';
    hp.MarkerSize=markerSize2;
    hp.FaceColor='interp';
    gpatch(Fb,V,0.5*ones(1,3),'none',0.25); %A static graphics object

    axisGeom(gca,fontSize);
    colormap(cMap); colorbar;
    caxis([min(E_stress_mat(:)) max(E_stress_mat(:))]);
    axis(axisLim(V_DEF)); %Set axis limits statically
    view(140,30);
    camlight headlight;

    % Set up animation features
    animStruct.Time=timeVec; %The time vector
    for qt=1:1:size(N_disp_mat,3) %Loop over time increments

        [CV]=faceToVertexMeasure(E,V,E_stress_mat(:,:,qt));

        %Set entries in animation structure
        animStruct.Handles{qt}=[hp hp]; %Handles of objects to animate
        animStruct.Props{qt}={'Vertices','CData'}; %Properties of objects to animate
        animStruct.Set{qt}={V_DEF(:,:,qt),CV}; %Property values for to set in order to animate
    end
    anim8(hf,animStruct); %Initiate animation feature
    drawnow;

    %%
    % Calculate metrics to visualize stretch-stress curve

    DZ_set=N_disp_mat(bcPrescribeList,end,:); %Z displacements of the prescribed set
    DZ_set=mean(DZ_set,1); %Calculate mean Z displacements across nodes
    stretch_sim=(DZ_set(:)+sampleHeight)./sampleHeight; %Derive stretch
    stress_cauchy_sim=mean(squeeze(E_stress_mat(:,end,:)),1)';

    %%
    % Visualize stress-stretch curve

    cFigure; hold on;
    title('Uniaxial stress-stretch curve','FontSize',fontSize);
    xlabel('$\lambda$ [.]','FontSize',fontSize,'Interpreter','Latex');
    ylabel('$\sigma_{zz}$ [MPa]','FontSize',fontSize,'Interpreter','Latex');

    plot(stretch_sim(:),stress_cauchy_sim(:),'r-','lineWidth',lineWidth);

    view(2); axis tight;  grid on; axis square; box on;
    set(gca,'FontSize',fontSize);
    drawnow;

    %%
    % Importing element principal stresses from a log file
    dataStruct=importFEBio_logfile(fullfile(savePath,febioLogFileName_stress_prin),1,1);

    %Access data
    E_stress_prin_mat=dataStruct.data;

    %Compute pressure
    P = squeeze(-1/3*mean(sum(E_stress_prin_mat,2),1));

    %%
    % Visualize pressure-stretch curve

    cFigure; hold on;
    title('Pressure-stretch curve','FontSize',fontSize);
    xlabel('$\lambda$ [.]','FontSize',fontSize,'Interpreter','Latex');
    ylabel('$p$ [MPa]','FontSize',fontSize,'Interpreter','Latex');

    plot(stretch_sim(:),P(:),'r-','lineWidth',lineWidth);

    view(2); axis tight;  grid on; axis square; box on;
    set(gca,'FontSize',fontSize);
    drawnow;

    %% IMPORTING ELEMENT STIFFNESS MATRICES
    % Importing element stiffness tensors from a log file
    dataStruct=importFEBio_logfile(fullfile(savePath,febioLogFileName_stiffness),0,1); %Nodal forces

    stiffness_mat=dataStruct.data(:,1:end,end); %Final stiffness state

    stiffness_mat_voigt=stiffness_mat(:,[1  2  4  11 16 7;...
                                         2  3  5  12 17 8;...
                                         4  5  6  13 18 9;...
                                         11 12 13 15 20 14;...
                                         16 17 18 20 21 19;...
                                         7  8  9  14 19 10]);
    stiffness_mat_voigt=reshape(stiffness_mat_voigt',6,6,size(stiffness_mat_voigt,1));
    stiffness_mat_voigt=reshape(mat2cell(stiffness_mat_voigt,6,6,...
        ones(size(stiffness_mat_voigt,3),1)),[size(stiffness_mat,1),1]);

    stiffness_mat_kelvin=stiffness_mat_voigt;
    for q=1:1:numel(stiffness_mat_voigt)
        cVoigt=stiffness_mat_voigt{q};
        c=voigtUnMap(cVoigt);
        cKelvin=kelvinMap(c);
        stiffness_mat_kelvin{q}=cKelvin;
    end

    %%
    % Visualize an element stiffness tensor

    viewFourthOrderTensor(c); %Visualize tensor C

end

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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 3 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, see <http://www.gnu.org/licenses/>.


%code for supplementary material
clear;
netStruct = load('../data/res152_batch32_Rankloss_2:1:0.1_margin1_both_drop0.75_shift_hard_256_152x152/net-epoch-80.mat');
net = dagnn.DagNN.loadobj(netStruct.net);
clear netStruct;
net.mode = 'test' ;
net.move('gpu') ;
net.removeLayer('RankLoss');
net.conserveMemory = true;
im_mean = reshape(net.meta.normalization.averageImage,1,1,3);
load('../url_data.mat');
p = imdb.images.data(imdb.images.set==3);
%%-------image feature
which_img = 621;
str = [ p{which_img}];
im = imread(['.',str]);
imshow(im);
oim = im;   % or oim = im;
f = getFeature2(net,oim,im_mean,'data','fc1_1bn');
f = sum(sum(f,1),2);
f2 = getFeature2(net,fliplr(oim),im_mean,'data','fc1_1bn');
f2 = sum(sum(f2,1),2);
f = f+f2;
size4 = size(f,4);
f = reshape(f,[],size4)';
f_img = norm_zzd(f);

% get raw text
load('../Flickr30k/rawdata-txt.mat');
raw_test = [];
load('../Flickr30k/train_val_test_split.mat');
test_set = find(set==3);
for i=1:1000
    tmp = test_set(i)*5-4 : test_set(i)*5;
    raw_test = cat(1,raw_test,raw_txt(tmp));
end
which_sentence = which_img*5-3;
title(raw_test{which_sentence});
load('../Flickr30k/dense_feature_word2.1.mat');
test_set = find(imdb.images.set==3);

%get text feature
content = wordcnn(:,test_set(which_img)*5-3);
len = sum(content>0);
load('/home/zzd/nlp/word2vector_matlab/flickr30k_dictionary.mat');
word_name = subset.names;
for k=1:len
    fprintf('%s ',word_name{content(k)});
end
fprintf('\n');

for i = 0:len
    content_tmp = content;
    if(i~=0) %start block words
        content_tmp(i)=0;
    end
    txtinput = zeros(len,20074,'single');
    kk = 1;
    for k=1:32
        if(content_tmp(k)==0)
            continue;
        end
        txtinput(kk,content_tmp(k))=1;
        kk = kk+1;
    end
    %transfer it to different location
    win = 33-len;
    input = zeros(32,20074,win,'single');
    for kk = 1:win
        input(kk:kk+len-1,:,kk) = txtinput;
    end

    input = reshape(input,1,32,20074,[]);
    f = getFeature2(net,input,[],'data2','fc6_2bn');
    f = sum(f,4);
    size4 = size(f,4);
    f = reshape(f,[],size4)';
    f_txt = norm_zzd(f);

    score = f_img * f_txt';
    if(i==0)
        s0 = score;
    else
      fprintf('%s,%.4f\n',word_name{content(i)},score-s0);
    end
end