function lax_wendroff_2d_display ( )

%*****************************************************************************80
%
%% LAX_WENDROFF_2D_DISPLAY illustrates the Lax-Wendroff procedure in 2D.
%
%  Discussion:
%
%    A sequence of ball and stick drawings is displayed in steps 0 through 9.
%
%    Hitting return causes the next item to be drawn.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 February 2003
%
%  Author:
%
%    John Burkardt
%
  for frame = 0 : 9
%
%  Clear the plotting frame.
%
    clf
%
%  Make all the subsequent plotting "cumulative".
%
    hold on

    if ( 0 == frame )
      title ( 'Lax-Wendroff 2D scheme' )
    end
%
%  Central nodes on the blue level.
%
    if ( 1 <= frame )
      plot3 (  0.0,  0.0, 0.0, 'o', 'MarkerFaceColor', 'b', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
    end
    if ( frame == 1 )
      title ( 'A node where we have the solution.' )
    end
%
%  The region associated with the central node, blue level.
%
    if ( 2 <= frame )
      plot3 (  [ -0.5,  0.5 ],  [ -0.5, -0.5 ],  [ 0.0, 0.0 ], 'b', 'LineWidth', 3 )
      plot3 (  [  0.5,  0.5 ],  [ -0.5,  0.5 ],  [ 0.0, 0.0 ], 'b', 'LineWidth', 3 )
      plot3 (  [  0.5, -0.5 ],  [  0.5,  0.5 ],  [ 0.0, 0.0 ], 'b', 'LineWidth', 3 )
      plot3 (  [ -0.5, -0.5 ],  [  0.5, -0.5 ],  [ 0.0, 0.0 ], 'b', 'LineWidth', 3 )
    end
    if ( frame == 2 )
      title ( 'The region associated with the node.' )
    end
%
%  Neighbor nodes on the blue level.
%
    if ( 3 <= frame )
      plot3 ( -1.0,  0.0, 0.0, 'o', 'MarkerFaceColor', 'b', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  1.0,  0.0, 0.0, 'o', 'MarkerFaceColor', 'b', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  0.0, -1.0, 0.0, 'o', 'MarkerFaceColor', 'b', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  0.0,  1.0, 0.0, 'o', 'MarkerFaceColor', 'b', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
    end
    if ( frame == 3 )
      title ( 'The neighboring nodes.' )
    end
%
%  Midside nodes on the blue level.
%
    if ( 4 <= frame )
      plot3 ( -0.5,  0.0, 0.0, 'o', 'MarkerFaceColor', 'b', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  0.5,  0.0, 0.0, 'o', 'MarkerFaceColor', 'b', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  0.0, -0.5, 0.0, 'o', 'MarkerFaceColor', 'b', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  0.0,  0.5, 0.0, 'o', 'MarkerFaceColor', 'b', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
    end
    if ( frame == 4 )
      plot3 (  [ -1.0, +1.0 ],  [  0.0,  0.0 ],  [ 0.0, 0.0 ], 'b', 'LineWidth', 3 )
      plot3 (  [  0.0,  0.0 ],  [ -1.0,  1.0 ],  [ 0.0, 0.0 ], 'b', 'LineWidth', 3 )
    end
    if ( frame == 4 )
      title ( 'Values at midsides by averaging.' )
    end
%
%  Center node on the green level.
%  Line from center node on blue to center node on green.
%
    if ( 5 <= frame )
      plot3 ( -0.5,  0.0, 0.5, 'o', 'MarkerFaceColor', 'g', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  0.5,  0.0, 0.5, 'o', 'MarkerFaceColor', 'g', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  0.0, -0.5, 0.5, 'o', 'MarkerFaceColor', 'g', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  0.0,  0.5, 0.5, 'o', 'MarkerFaceColor', 'g', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  [ -0.5,  0.5 ],  [ -0.5, -0.5 ],  [ 0.5, 0.5 ], 'g', 'LineWidth', 3 )
      plot3 (  [  0.5,  0.5 ],  [ -0.5,  0.5 ],  [ 0.5, 0.5 ], 'g', 'LineWidth', 3 )
      plot3 (  [  0.5, -0.5 ],  [  0.5,  0.5 ],  [ 0.5, 0.5 ], 'g', 'LineWidth', 3 )
      plot3 (  [ -0.5, -0.5 ],  [  0.5, -0.5 ],  [ 0.5, 0.5 ], 'g', 'LineWidth', 3 )
    end
    if ( frame == 5 )
      plot3 (  [ -0.5, -0.5 ],  [  0.0,  0.0 ],  [ 0.0, 0.5 ], 'k', 'LineWidth', 3 )
      plot3 (  [  0.5,  0.5 ],  [  0.0,  0.0 ],  [ 0.0, 0.5 ], 'k', 'LineWidth', 3 )
      plot3 (  [  0.0,  0.0 ],  [ -0.5, -0.5 ],  [ 0.0, 0.5 ], 'k', 'LineWidth', 3 )
      plot3 (  [  0.0,  0.0 ],  [  0.5,  0.5 ],  [ 0.0, 0.5 ], 'k', 'LineWidth', 3 )
    end
    if ( frame == 5 )
      title ( 'Values at midside, half time step.' )
    end
%
%  Lines from midside nodes to center.
%
    if ( 6 == frame )
      plot3 (  [  0.5,  0.0 ],  [  0.0,  0.0 ],  [ 0.5, 0.5 ], 'g', 'LineWidth', 3 )
      plot3 (  [ -0.5,  0.0 ],  [  0.0,  0.0 ],  [ 0.5, 0.5 ], 'g', 'LineWidth', 3 )
      plot3 (  [  0.0,  0.0 ],  [  0.5,  0.0 ],  [ 0.5, 0.5 ], 'g', 'LineWidth', 3 )
      plot3 (  [  0.0,  0.0 ],  [ -0.5,  0.0 ],  [ 0.5, 0.5 ], 'g', 'LineWidth', 3 )
    end
    if ( frame == 6 )
      title ( 'Estimate fluxes' )
    end
%
%  Carry neighbor nodes to green level, and draw lines.
%
    if ( 7 <= frame )
      plot3 (  0.0,  0.0, 0.5, 'o', 'MarkerFaceColor', 'g', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
    end
    if ( frame == 7 )
      title ( 'Estimate derivative at node, half time step.' )
    end
%
%  Advance solution to full time.
%
    if ( 8 <= frame )
      plot3 (  [  0.0,  0.0 ],  [  0.0,  0.0 ],  [ 0.0, 1.0 ], 'k', 'LineWidth', 3 )
      plot3 (  0.0,  0.0, 1.0, 'o', 'MarkerFaceColor', 'r', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  [ -0.5,  0.5 ],  [ -0.5, -0.5 ],  [ 1.0, 1.0 ], 'r', 'LineWidth', 3 )
      plot3 (  [  0.5,  0.5 ],  [ -0.5,  0.5 ],  [ 1.0, 1.0 ], 'r', 'LineWidth', 3 )
      plot3 (  [  0.5, -0.5 ],  [  0.5,  0.5 ],  [ 1.0, 1.0 ], 'r', 'LineWidth', 3 )
      plot3 (  [ -0.5, -0.5 ],  [  0.5, -0.5 ],  [ 1.0, 1.0 ], 'r', 'LineWidth', 3 )
    end
    if ( frame == 8 )
      title ( 'Take full step to new time.' )
    end
%
%  Advance all data.
%
    if ( 9 <= frame )
      plot3 (  [ -1.0, -1.0 ],  [  0.0,  0.0 ],  [ 0.0, 1.0 ], 'k', 'LineWidth', 3 )
      plot3 (  [  1.0,  1.0 ],  [  0.0,  0.0 ],  [ 0.0, 1.0 ], 'k', 'LineWidth', 3 )
      plot3 (  [  0.0,  0.0 ],  [ -1.0, -1.0 ],  [ 0.0, 1.0 ], 'k', 'LineWidth', 3 )
      plot3 (  [  0.0,  0.0 ],  [  1.0,  1.0 ],  [ 0.0, 1.0 ], 'k', 'LineWidth', 3 )
      plot3 ( -1.0,  0.0, 1.0, 'o', 'MarkerFaceColor', 'r', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  1.0,  0.0, 1.0, 'o', 'MarkerFaceColor', 'r', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  0.0, -1.0, 1.0, 'o', 'MarkerFaceColor', 'r', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
      plot3 (  0.0,  1.0, 1.0, 'o', 'MarkerFaceColor', 'r', 'MarkerEdgeColor', 'k', 'MarkerSize', 12 )
    end
    if ( frame == 9 )
      title ( 'Advance all data to new time.' )
    end

    xlabel ( '- X -' )
    ylabel ( '- Y -' )
    zlabel ( '- Time -' )

    axis ( [ -1, +1, -1, +1, 0, 1] )
    view ( 3 )
%
%  "Release" the plotting screen.
%
    hold off

    pause

  end

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   PARAMETERS Returns a data structure containing the parameters of the
%   example 4 DOF planar robot.
%
%   Author: Arturo Gil. Universidad Miguel Hernandez de Elche.
%   email: arturo.gil@umh.es date:   05/03/2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 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 robot = parameters()

robot.name='Example 4DOF planar arm';

%kinematic data DH parameters
robot.DH.theta='[q(1) q(2) q(3) q(4)]';
robot.DH.d='[0  0  0  0]';
robot.DH.a='[1  1  1  1]';
robot.DH.alpha='[0  0  0  0]';

%number of degrees of freedom
robot.DOF = 4;

%Jacobian matrix
% if not defined, compute it with manipulator_jacobian
robot.J=[];
%the proper rows of J to compute manipulability
robot.selJ =[1,2,6];
robot.kind=['R' 'R' 'R' 'R'];

%Function name to compute inverse kinematic
robot.inversekinematic_fn = 'inverse_kinematics_4dofplanar(robot, T, q)';
robot.directkinematic_fn = 'directkinematic(robot, q)';

%minimum and maximum rotation angle in rad
robot.maxangle =[deg2rad(-180) deg2rad(180); %Axis 1, minimum, maximum
                deg2rad(-180) deg2rad(180);
                deg2rad(-180) deg2rad(180)]; %Axis 2, minimum, maximum

            %minimum and maximum rotation angle in rad
robot.maxangle =[deg2rad(-90) deg2rad(90); %Axis 1, minimum, maximum
                deg2rad(-90) deg2rad(90);
                deg2rad(-180) deg2rad(180)]; %Axis 2, minimum, maximum

%maximum absolute speed of each joint rad/s or m/s
robot.velmax = []; %empty, not available

robot.accelmax=robot.velmax/0.1; % 0.1 is here an acceleration time
% end effectors maximum velocity
robot.linear_velmax = 0; %m/s, example, not available

%base reference system
robot.T0 = eye(4);

%INITIALIZATION OF VARIABLES REQUIRED FOR THE SIMULATION
%position, velocity and acceleration
robot=init_sim_variables(robot);
robot.path = pwd;

%this is an ad hoc transformation between the piece and the robot's end
%effector
% robot.Tcoupling=[-1 0 0  0;
%      0 1 0  0;
%      0 0 -1 0;
%      0 0 0 1];
% Tc1 = [cos(-pi/2) 0 sin(-pi/2) 0;
%        0 1 0 0;
%        -sin(-pi/2) 0 cos(-pi/2) 0;
%        0  0  0  1       ];
% Tc2 = [1 0 0 0;
%        0 cos(pi) -sin(pi) 0;
%        0  sin(pi) cos(pi) 0;
%        0 0 0 1];
% robot.Tcoupling =Tc1*Tc2;
robot.Tcoupling = eye(4);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%GRAPHICS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%read graphics files
robot.graphical.has_graphics=1;
robot.graphical.color = [255 20 40]./255;
%for transparency
robot.graphical.draw_transparent=0;
%draw DH systems
robot.graphical.draw_axes=1;
%DH system length and Font size, standard is 1/10. Select 2/20, 3/30 for
%bigger robots
robot.graphical.axes_scale=1;
%adjust for a default view of the robot
robot.axis=[-3.5 3.5 -3.5 3.5 0 1]
robot = read_graphics(robot);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%DYNAMIC PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
robot.has_dynamics=1;

%consider friction in the computations
robot.dynamics.friction=0;

%link masses (kg)
robot.dynamics.masses=[1 1 1] ;

%COM of each link with respect to own reference system
robot.dynamics.r_com=[-0.5      0         0; %(rx, ry, rz) link 1
                      -0.5      0         0;
                      -0.5      0         0];%(rx, ry, rz) link 2

%link masses
m1 = robot.dynamics.masses(1);
m2 = robot.dynamics.masses(2);
m3 = robot.dynamics.masses(3);

a=eval(robot.DH.a);
L1 = a(1);
L2 = a(2);
L3 = a(3);

%Momentos de inercia de cada eslabon.
% Ixx	Iyy	Izz	Ixy	Iyz	Ixz, por cada fila
robot.dynamics.Inertia=[0   m1*L1^2/3   m1*L1^2/3    0	0	0;
                        0   m2*L2^2/3   m2*L2^2/3    0	0	0;
                        0   m3*L3^2/3   m3*L3^2/3    0	0	0];

%Actuator rotor inertia
robot.motors.Inertia=[0 0 0];
%Reduction ratio motor/joint speed
robot.motors.G=[1 1 1];
%consider friction
robot.friction=0;
%Viscous friction factor, motor referred
%robot.B = [1e-3  1e-3 1e-3];
robot.motors.Viscous = [10  10 10];
%Coulomb friction, motor referred
robot.motors.Coulomb = [0 0;
            0 0;
            0 0];

%SPECIAL PARAMETERS TO SOLVE THE INVERSE KINEMATICS
robot.parameters.step_time=0.1;
%Error in XYZ to stop inverse kinematics
robot.parameters.epsilonXYZ=0.01;
%Error in Quaternion to stop inverse kinematics.
robot.parameters.epsilonQ=0.01;
robot.parameters.stop_iterations=500;


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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the HEART-TUBE-EXAMPLE geometry and prints associated input files
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function HeartTube()

%
% Grid Parameters (MAKE SURE MATCHES IN input2d !!!)
%
Nx =  128;        % # of Eulerian Grid Pts. in x-Direction (MUST BE EVEN!!!)
Ny =  128;        % # of Eulerian Grid Pts. in y-Direction (MUST BE EVEN!!!)
Lx = 5.0;        % Length of Eulerian Grid in x-Direction
Ly = 5.0;        % Length of Eulerian Grid in y-Direction

% Immersed Structure Geometric / Dynamic Parameters %
ds = Lx/(2*Nx);  % Lagrangian Spacing
d = 1.0;         % Diameter of the tube
L = 3.0;         % Length of the heart-tube
struct_name = 'HeartTube'; % Name for .vertex, .spring, etc files.

% Call function to construct geometry
[xLag,yLag] = give_Me_Immsersed_Boundary_Geometry(ds,L,d,Lx,Ly);

% Plot Geometry to test
plot(xLag(1:end/2),yLag(1:end/2),'r-'); hold on;
plot(xLag(end/2+1:end),yLag(end/2+1:end),'r-'); hold on;
plot(xLag,yLag,'*'); hold on;
xlabel('x'); ylabel('y');
axis([0 Lx 0 Ly]);

% Prints .vertex file!
print_Lagrangian_Vertices(xLag,yLag,struct_name);

% Prints .spring file!
k_Spring = 1e7;
print_Lagrangian_Springs(xLag,k_Spring,ds,struct_name);

% Prints .muscle file! [ a_f * Fmax *exp( -( (Q-1)/SK )^2 ) * (1/P0)*(b*P0-a*v)/(v+b); Q = LF/LFO ]
LFO = d; SK = 0.3; a = 0.25; b = 4.0; Fmax = 1e5;
print_Lagrangian_Muscles(xLag,LFO,SK,a,b,Fmax,struct_name)

% Prints .beam file!
k_Beam = 7.5e7; C = 0.0;
print_Lagrangian_Beams(xLag,k_Beam,C,struct_name);

% Prints .target file!
k_Target = 1e6;
print_Lagrangian_Target_Pts(xLag,k_Target,struct_name);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints VERTEX points to a file called rubberband.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Vertices(xLag,yLag,struct_name)

    N = length(xLag);

    vertex_fid = fopen([struct_name '.vertex'], 'w');

    fprintf(vertex_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 1:N
        X_v = xLag(s);
        Y_v = yLag(s);
        fprintf(vertex_fid, '%1.16e %1.16e\n', X_v, Y_v);
    end

    fclose(vertex_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints Vertex points to a file called rubberband.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Target_Pts(xLag,k_Target,struct_name)

    N = length(xLag); % Total Number of Lagrangian Pts
    num = 1;          % Number of target points on each end

    target_fid = fopen([struct_name '.target'], 'w');

    fprintf(target_fid, '%d\n', 4*num );

    %Loops over all Lagrangian Pts.
    %for s = 1:N

    %Left Bottom Target Points
    for s=1:num
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    %Right Bottom Target Points
    for s=(N/2)-(num-1):N/2
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    %Left Top Target Points
    for s=(N/2+1):(N/2+1)+(num-1)
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    %Right Top Target Points
    for s=N-(num-1):N
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    %end

    fclose(target_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints BEAM (Torsional Spring) points to a file called rubberband.beam
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Beams(xLag,k_Beam,C,struct_name)

    % k_Beam: beam stiffness
    % C: beam curvature

    N = length(xLag); % NOTE: Total number of beams = Number of Total Lag Pts. - 2

    beam_fid = fopen([struct_name '.beam'], 'w');

    fprintf(beam_fid, '%d\n', N - 4 );

    %spring_force = kappa_spring*ds/(ds^2);

    %BEAMS BETWEEN VERTICES
    for s = 2:N-1
            if  s <= N/2-1
                % Bottom of tube
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, s+1, k_Beam, C);
            elseif ( ( s >= N/2+2 ) && ( s <= N-1 ) )
                % Top of Tube
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, s+1,   k_Beam, C);
            end
    end
    fclose(beam_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints MUSCLE points to a file called "struct_name".muscle
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Muscles(xLag,LFO,SK,a,b,Fmax,struct_name)

    N = length(xLag); %Number of Lagrangian Pts. Total

    muscle_fid = fopen([struct_name '.muscle'], 'w');

    fprintf(muscle_fid, '%d\n', N/2-2 );

    %spring_force = kappa_spring*ds/(ds^2);

    %MUSCLES BETWEEN VERTICES
    for s = 2:N/2-1
            fprintf(muscle_fid, '%d %d %1.16e %1.16e %1.16e %1.16e %1.16e\n', s, s+N/2, LFO, SK, a,b,Fmax);
    end
    fclose(muscle_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints SPRING points to a file called rubberband.spring
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Springs(xLag,k_Spring,ds_Rest,struct_name)

    N = length(xLag); %Number of Lagrangian Pts. Total

    spring_fid = fopen([struct_name '.spring'], 'w');

    fprintf(spring_fid, '%d\n', N-2 );

    %spring_force = kappa_spring*ds/(ds^2);

    %SPRINGS BETWEEN VERTICES
    for s = 1:N
            if s < N/2
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds_Rest);
            elseif ( ( s >= N/2+1 ) && ( s < N ) )
                %Case s=N
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1,   k_Spring, ds_Rest);
            end
    end
    fclose(spring_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Immsersed_Boundary_Geometry(ds,L,d,Lx,Ly)

% The immsersed structure is a straight heart-tube %
x1 = [-L/2:ds:L/2 L/2];             % Constructs x-Values for bottom of tube
y1 = -d/2*ones(1,length(x1)); % Constructs y-Values for bottom of tube

x2 = x1;                      % Constructs x-Values for top of tube
y2 = -y1;                     % Constructs y-Values for top of tube

x1 = x1 + Lx/2;               % Shift into correct box
x2 = x2 + Lx/2;                % Shift into correct box

y1 = y1 + Ly/2;               % Shift into correct box
y2 = y2 + Ly/2;               % Shift into correct box

xLag = [x1 x2];
yLag = [y1 y2];


%SerialLink.ikine6s Analytical inverse kinematics
%
% Q = R.ikine(T) are the joint coordinates (1xN) corresponding to the robot
% end-effector pose T which is an SE3 object or homogenenous transform
% matrix (4x4), and N is the number of robot joints.  This is a analytic
% solution for a 6-axis robot with a spherical wrist (the most common form
% for industrial robot arms).
%
% If T represents a trajectory (4x4xM) then the inverse kinematics is
% computed for all M poses resulting in Q (MxN) with each row representing
% the joint angles at the corresponding pose.
%
% Q = R.IKINE6S(T, CONFIG) as above but specifies the configuration of the arm in
% the form of a string containing one or more of the configuration codes:
%
% 'l'   arm to the left (default)
% 'r'   arm to the right
% 'u'   elbow up (default)
% 'd'   elbow down
% 'n'   wrist not flipped (default)
% 'f'   wrist flipped (rotated by 180 deg)
%
% Trajectory operation::
%
% In all cases if T is a vector of SE3 objects (1xM) or a homogeneous
% transform sequence (4x4xM) then R.ikcon() returns the joint coordinates
% corresponding to each of the transforms in the sequence.
%
% Notes::
% - Treats a number of specific cases:
%   - Robot with no shoulder offset
%   - Robot with a shoulder offset (has lefty/righty configuration)
%   - Robot with a shoulder offset and a prismatic third joint (like Stanford arm)
%   - The Puma 560 arms with shoulder and elbow offsets (4 lengths parameters)
%   - The Kuka KR5 with many offsets (7 length parameters)
% - The inverse kinematics for the various cases determined using ikine_sym.
% - The inverse kinematic solution is generally not unique, and
%   depends on the configuration string.
% - Joint offsets, if defined, are added to the inverse kinematics to
%   generate Q.
% - Only applicable for standard Denavit-Hartenberg parameters
%
% Reference::
% - Inverse kinematics for a PUMA 560,
%   Paul and Zhang,
%   The International Journal of Robotics Research,
%   Vol. 5, No. 2, Summer 1986, p. 32-44
%
% Author::
% - The Puma560 case: Robert Biro with Gary Von McMurray,
%   GTRI/ATRP/IIMB, Georgia Institute of Technology, 2/13/95
% - Kuka KR5 case: Gautam Sinha,
%   Autobirdz Systems Pvt. Ltd.,  SIDBI Office,
%   Indian Institute of Technology Kanpur, Kanpur, Uttar Pradesh.
%
% See also SerialLink.fkine, SerialLink.ikine, SerialLink.ikine_sym.

function thetavec = ikine6s(robot, TT, varargin)

    if robot.mdh ~= 0
        error('RTB:ikine:notsupported','Solution only applicable for standard DH conventions');
    end

    if robot.n ~= 6
        error('RTB:ikine:notsupported','Solution only applicable for 6-axis robot');
    end

    %
    %     % recurse over all poses in a trajectory
    %     if ndims(T) == 3
    %         theta = zeros(size(T,3),robot.n);
    %         for k=1:size(T,3)
    %             theta(k,:) = ikine6s(robot, T(:,:,k), varargin{:});
    %         end
    %         return;
    %     end
    %
    %
    %     if ~ishomog(T)
    %         error('RTB:ikine:badarg', 'T is not a homog xform');
    %     end

    TT = SE3(TT);

    L = robot.links;

    if ~robot.isspherical()
        error('RTB:ikine:notsupported', 'wrist is not spherical');
    end

    % The configuration parameter determines what n1,n2,n4 values are used
    % and how many solutions are determined which have values of -1 or +1.

    if nargin < 3
        configuration = '';
    else
        configuration = lower(varargin{1});
    end

    % default configuration

    sol = [1 1 1];  % left, up, noflip

    for c=configuration
        switch c
            case 'l'
                sol(1) = 1;
            case 'r'
                sol(1) = 2;
            case 'u'
                sol(2) = 1;
            case 'd'
                sol(2) = 2;
            case 'n'
                sol(3) = 1;
            case 'f'
                sol(3) = 2;
        end
    end

    % determine the arm structure and the relevant solution to use
    if isempty(robot.ikineType)
        if is_simple(L)
            robot.ikineType = 'nooffset';
        elseif is_puma(L)
            robot.ikineType = 'puma';
        elseif is_offset(L)
            robot.ikineType = 'offset';
        elseif is_rrp(L)
            robot.ikineType = 'rrp';
        else
            error('RTB:ikine6s:badarg', 'This kinematic structure not supported');
        end
    end

    for k=1:length(TT)

        % undo base and tool transformations
        T = inv(robot.base) * TT(k) * inv(robot.tool);

        % drop back to matrix form
        T = T.T;

        %% now solve for the first 3 joints, based on position of the spherical wrist centre

        switch robot.ikineType
            case 'puma'
                % Puma model with shoulder and elbow offsets
                %
                % - Inverse kinematics for a PUMA 560,
                %   Paul and Zhang,
                %   The International Journal of Robotics Research,
                %   Vol. 5, No. 2, Summer 1986, p. 32-44
                %
                % Author::
                % Robert Biro with Gary Von McMurray,
                % GTRI/ATRP/IIMB,
                % Georgia Institute of Technology
                % 2/13/95

                a2 = L(2).a;
                a3 = L(3).a;
                d1 = L(1).d;
                d3 = L(3).d;
                d4 = L(4).d;

                % The following parameters are extracted from the Homogeneous
                % Transformation as defined in equation 1, p. 34

                Ox = T(1,2);
                Oy = T(2,2);
                Oz = T(3,2);

                Ax = T(1,3);
                Ay = T(2,3);
                Az = T(3,3);

                Px = T(1,4);
                Py = T(2,4);
                Pz = T(3,4) - d1;

                %
                % Solve for theta(1)
                %
                % r is defined in equation 38, p. 39.
                % theta(1) uses equations 40 and 41, p.39,
                % based on the configuration parameter n1
                %

                r = sqrt(Px^2 + Py^2);
                if sol(1) == 1
                    theta(1) = atan2(Py,Px) + pi - asin(d3/r);
                else
                    theta(1) = atan2(Py,Px) + asin(d3/r);
                end

                %
                % Solve for theta(2)
                %
                % V114 is defined in equation 43, p.39.
                % r is defined in equation 47, p.39.
                % Psi is defined in equation 49, p.40.
                % theta(2) uses equations 50 and 51, p.40, based on the configuration
                % parameter n2
                %
                if sol(2) == 1
                    n2 = -1;
                else
                    n2 = 1;
                end
                if sol(1) == 2
                    n2 = -n2;
                end

                V114 = Px*cos(theta(1)) + Py*sin(theta(1));
                r = sqrt(V114^2 + Pz^2);
                Psi = acos((a2^2-d4^2-a3^2+V114^2+Pz^2)/(2.0*a2*r));
                if ~isreal(Psi)
                    theta = [];
                else

                    theta(2) = atan2(Pz,V114) + n2*Psi;

                    %
                    % Solve for theta(3)
                    %
                    % theta(3) uses equation 57, p. 40.
                    %
                    num = cos(theta(2))*V114+sin(theta(2))*Pz-a2;
                    den = cos(theta(2))*Pz - sin(theta(2))*V114;
                    theta(3) = atan2(a3,d4) - atan2(num, den);
                end
            case 'nooffset'
                a2 = L(2).a;
                a3 = L(3).a;
                d1 = L(1).d;

                px = T(1,4); py = T(2,4); pz = T(3,4);

                %%% autogenerated code
                if L(1).alpha < 0
                    if sol(1) == 1
                        q(1) = angle(-px-py*1i);
                    else
                        q(1) = angle(px+py*1i);
                    end
                    S1 = sin(q(1));
                    C1 = cos(q(1));

                    if sol(2) == 1
                        q(2) = -angle(a2*d1*-2.0+a2*pz*2.0-C1*a2*px*2.0i-S1*a2*py*2.0i)+angle(d1*pz*2.0i-a2^2*1i+a3^2*1i-d1^2*1i-pz^2*1i-C1^2*px^2*1i-sqrt((a2*d1*2.0-a2*pz*2.0)^2+(C1*a2*px*2.0+S1*a2*py*2.0)^2-(d1*pz*-2.0+a2^2-a3^2+d1^2+pz^2+C1^2*px^2+S1^2*py^2+C1*S1*px*py*2.0)^2)-S1^2*py^2*1i-C1*S1*px*py*2.0i);
                    else
                        q(2) = -angle(a2*d1*-2.0+a2*pz*2.0-C1*a2*px*2.0i-S1*a2*py*2.0i)+angle(d1*pz*2.0i-a2^2*1i+a3^2*1i-d1^2*1i-pz^2*1i-C1^2*px^2*1i+sqrt((a2*d1*2.0-a2*pz*2.0)^2+(C1*a2*px*2.0+S1*a2*py*2.0)^2-(d1*pz*-2.0+a2^2-a3^2+d1^2+pz^2+C1^2*px^2+S1^2*py^2+C1*S1*px*py*2.0)^2)-S1^2*py^2*1i-C1*S1*px*py*2.0i);
                    end
                    S2 = sin(q(2));
                    C2 = cos(q(2));

                    if sol(3) == 1
                        q(3) = -angle(a2*-1i+C2*d1-C2*pz+S2*d1*1i-S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i-sqrt((-a2+S2*d1-S2*pz+C1*C2*px+C2*S1*py)^2+(-C2*d1+C2*pz+C1*S2*px+S1*S2*py)^2-a3^2));
                    else
                        q(3) = -angle(a2*-1i+C2*d1-C2*pz+S2*d1*1i-S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i+sqrt((-a2+S2*d1-S2*pz+C1*C2*px+C2*S1*py)^2+(-C2*d1+C2*pz+C1*S2*px+S1*S2*py)^2-a3^2));
                    end
                else
                    if sol(1) == 1
                        q(1) = angle(px+py*1i);
                    else
                        q(1) = angle(-px-py*1i);
                    end
                    S1 = sin(q(1));
                    C1 = cos(q(1));

                    if sol(2) == 1
                        q(2) = -angle(a2*d1*2.0-a2*pz*2.0-C1*a2*px*2.0i-S1*a2*py*2.0i)+angle(d1*pz*2.0i-a2^2*1i+a3^2*1i-d1^2*1i-pz^2*1i-C1^2*px^2*1i-sqrt((a2*d1*2.0-a2*pz*2.0)^2+(C1*a2*px*2.0+S1*a2*py*2.0)^2-(d1*pz*-2.0+a2^2-a3^2+d1^2+pz^2+C1^2*px^2+S1^2*py^2+C1*S1*px*py*2.0)^2)-S1^2*py^2*1i-C1*S1*px*py*2.0i);
                    else
                        q(2) = -angle(a2*d1*2.0-a2*pz*2.0-C1*a2*px*2.0i-S1*a2*py*2.0i)+angle(d1*pz*2.0i-a2^2*1i+a3^2*1i-d1^2*1i-pz^2*1i-C1^2*px^2*1i+sqrt((a2*d1*2.0-a2*pz*2.0)^2+(C1*a2*px*2.0+S1*a2*py*2.0)^2-(d1*pz*-2.0+a2^2-a3^2+d1^2+pz^2+C1^2*px^2+S1^2*py^2+C1*S1*px*py*2.0)^2)-S1^2*py^2*1i-C1*S1*px*py*2.0i);
                    end
                    S2 = sin(q(2));
                    C2 = cos(q(2));

                    if sol(3) == 1
                        q(3) = -angle(a2*-1i-C2*d1+C2*pz-S2*d1*1i+S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i-sqrt((-a2-S2*d1+S2*pz+C1*C2*px+C2*S1*py)^2+(C2*d1-C2*pz+C1*S2*px+S1*S2*py)^2-a3^2));
                    else
                        q(3) = -angle(a2*-1i-C2*d1+C2*pz-S2*d1*1i+S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i+sqrt((-a2-S2*d1+S2*pz+C1*C2*px+C2*S1*py)^2+(C2*d1-C2*pz+C1*S2*px+S1*S2*py)^2-a3^2));
                    end
                end

                theta(1:3) = q;

            case'offset'
                % general case with 6 length parameters
                a1 = L(1).a;
                a2 = L(2).a;
                a3 = L(3).a;
                d1 = L(1).d;
                d2 = L(2).d;
                d3 = L(3).d;

                px = T(1,4); py = T(2,4); pz = T(3,4);

                %%% autogenerated code
                if L(1).alpha < 0

                    if sol(1) == 1
                        q(1) = -angle(-px+py*1i)+angle(d2*1i+d3*1i-sqrt(d2*d3*-2.0-d2^2-d3^2+px^2+py^2));
                    else
                        q(1) = angle(d2*1i+d3*1i+sqrt(d2*d3*-2.0-d2^2-d3^2+px^2+py^2))-angle(-px+py*1i);
                    end
                    S1 = sin(q(1));
                    C1 = cos(q(1));

                    if sol(2) == 1
                        q(2) = angle(d1*pz*2.0i-sqrt(d1*pz^3*4.0+d1^3*pz*4.0-a1^4-a2^4-a3^4-d1^4-py^4-pz^4-C1^4*px^4+C1^2*py^4*2.0-C1^4*py^4+a1^2*a2^2*2.0+a1^2*a3^2*2.0+a2^2*a3^2*2.0-a1^2*d1^2*2.0+a2^2*d1^2*2.0+a3^2*d1^2*2.0-a1^2*py^2*6.0-a2^2*py^2*2.0+a3^2*py^2*2.0-a1^2*pz^2*2.0+a2^2*pz^2*2.0+a3^2*pz^2*2.0-d1^2*py^2*2.0-d1^2*pz^2*6.0-py^2*pz^2*2.0+d1*py^2*pz*4.0+C1^3*a1*px^3*4.0+S1^3*a1*py^3*4.0-C1^2*a1^2*px^2*6.0+C1^2*a2^2*px^2*2.0+C1^2*a3^2*px^2*2.0+C1^2*a1^2*py^2*6.0+C1^2*a2^2*py^2*1.0e1-C1^2*a3^2*py^2*2.0-C1^4*a2^2*py^2*1.2e1+C1^6*a2^2*py^2*4.0-C1^2*d1^2*px^2*2.0+C1^2*d1^2*py^2*2.0-C1^2*px^2*py^2*6.0+C1^4*px^2*py^2*6.0-C1^2*px^2*pz^2*2.0+C1^2*py^2*pz^2*2.0+S1^6*a2^2*py^2*4.0+C1*a1^3*px*4.0+S1*a1^3*py*4.0+a1^2*d1*pz*4.0-a2^2*d1*pz*4.0-a3^2*d1*pz*4.0-C1*a1*a2^2*px*4.0-C1*a1*a3^2*px*4.0-C1*S1*px^3*py*4.0+C1*a1*d1^2*px*4.0+C1*a1*px*py^2*1.2e1+C1*a1*px*pz^2*4.0+S1*a1*a2^2*py*4.0-S1*a1*a3^2*py*4.0+S1*a1*d1^2*py*4.0+S1*a1*px^2*py*4.0+S1*a1*py*pz^2*4.0-C1*S1^3*px*py^3*4.0+C1*S1^3*px^3*py*4.0-C1^3*a1*px*py^2*1.2e1-S1^3*a1*a2^2*py*8.0+C1^2*d1*px^2*pz*4.0-C1^2*d1*py^2*pz*4.0-S1^3*a1*px^2*py*4.0-C1*a1*d1*px*pz*8.0-S1*a1*d1*py*pz*8.0-C1*S1*a1^2*px*py*1.2e1-C1*S1*a2^2*px*py*4.0+C1*S1*a3^2*px*py*4.0-C1*S1*d1^2*px*py*4.0-C1*S1*px*py*pz^2*4.0-C1^2*S1*a1*a2^2*py*8.0+C1^2*S1*a1*px^2*py*8.0+C1*S1^3*a2^2*px*py*8.0+C1^3*S1*a2^2*px*py*8.0+C1*S1*d1*px*py*pz*8.0)-a1^2*1i-a2^2*1i+a3^2*1i-d1^2*1i-py^2*1i-pz^2*1i-C1^2*px^2*1i+C1^2*py^2*1i+C1*a1*px*2.0i+S1*a1*py*2.0i-C1*S1*px*py*2.0i)-angle(-a2*(a1*-1i+d1-pz+C1*px*1i+S1^3*py*1i+C1^2*S1*py*1i));
                    else
                        q(2) = angle(d1*pz*2.0i+sqrt(d1*pz^3*4.0+d1^3*pz*4.0-a1^4-a2^4-a3^4-d1^4-py^4-pz^4-C1^4*px^4+C1^2*py^4*2.0-C1^4*py^4+a1^2*a2^2*2.0+a1^2*a3^2*2.0+a2^2*a3^2*2.0-a1^2*d1^2*2.0+a2^2*d1^2*2.0+a3^2*d1^2*2.0-a1^2*py^2*6.0-a2^2*py^2*2.0+a3^2*py^2*2.0-a1^2*pz^2*2.0+a2^2*pz^2*2.0+a3^2*pz^2*2.0-d1^2*py^2*2.0-d1^2*pz^2*6.0-py^2*pz^2*2.0+d1*py^2*pz*4.0+C1^3*a1*px^3*4.0+S1^3*a1*py^3*4.0-C1^2*a1^2*px^2*6.0+C1^2*a2^2*px^2*2.0+C1^2*a3^2*px^2*2.0+C1^2*a1^2*py^2*6.0+C1^2*a2^2*py^2*1.0e1-C1^2*a3^2*py^2*2.0-C1^4*a2^2*py^2*1.2e1+C1^6*a2^2*py^2*4.0-C1^2*d1^2*px^2*2.0+C1^2*d1^2*py^2*2.0-C1^2*px^2*py^2*6.0+C1^4*px^2*py^2*6.0-C1^2*px^2*pz^2*2.0+C1^2*py^2*pz^2*2.0+S1^6*a2^2*py^2*4.0+C1*a1^3*px*4.0+S1*a1^3*py*4.0+a1^2*d1*pz*4.0-a2^2*d1*pz*4.0-a3^2*d1*pz*4.0-C1*a1*a2^2*px*4.0-C1*a1*a3^2*px*4.0-C1*S1*px^3*py*4.0+C1*a1*d1^2*px*4.0+C1*a1*px*py^2*1.2e1+C1*a1*px*pz^2*4.0+S1*a1*a2^2*py*4.0-S1*a1*a3^2*py*4.0+S1*a1*d1^2*py*4.0+S1*a1*px^2*py*4.0+S1*a1*py*pz^2*4.0-C1*S1^3*px*py^3*4.0+C1*S1^3*px^3*py*4.0-C1^3*a1*px*py^2*1.2e1-S1^3*a1*a2^2*py*8.0+C1^2*d1*px^2*pz*4.0-C1^2*d1*py^2*pz*4.0-S1^3*a1*px^2*py*4.0-C1*a1*d1*px*pz*8.0-S1*a1*d1*py*pz*8.0-C1*S1*a1^2*px*py*1.2e1-C1*S1*a2^2*px*py*4.0+C1*S1*a3^2*px*py*4.0-C1*S1*d1^2*px*py*4.0-C1*S1*px*py*pz^2*4.0-C1^2*S1*a1*a2^2*py*8.0+C1^2*S1*a1*px^2*py*8.0+C1*S1^3*a2^2*px*py*8.0+C1^3*S1*a2^2*px*py*8.0+C1*S1*d1*px*py*pz*8.0)-a1^2*1i-a2^2*1i+a3^2*1i-d1^2*1i-py^2*1i-pz^2*1i-C1^2*px^2*1i+C1^2*py^2*1i+C1*a1*px*2.0i+S1*a1*py*2.0i-C1*S1*px*py*2.0i)-angle(-a2*(a1*-1i+d1-pz+C1*px*1i+S1^3*py*1i+C1^2*S1*py*1i));
                    end
                    S2 = sin(q(2));
                    C2 = cos(q(2));

                    if sol(3) == 1
                        q(3) = angle(a3*1i-sqrt(d1*pz*-2.0+a1^2+a2^2-a3^2+d1^2+py^2+pz^2+C1^2*px^2-C1^2*py^2+C2*a1*a2*2.0-C1*a1*px*2.0-S2*a2*d1*2.0-S1*a1*py*2.0+S2*a2*pz*2.0-C1*C2*a2*px*2.0-C2*S1*a2*py*2.0+C1*S1*px*py*2.0))-angle(a2*-1i-C2*a1*1i+C2*d1-C2*pz+S2*a1+S2*d1*1i-S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py);
                    else
                        q(3) = -angle(a2*-1i-C2*a1*1i+C2*d1-C2*pz+S2*a1+S2*d1*1i-S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i+sqrt(d1*pz*-2.0+a1^2+a2^2-a3^2+d1^2+py^2+pz^2+C1^2*px^2-C1^2*py^2+C2*a1*a2*2.0-C1*a1*px*2.0-S2*a2*d1*2.0-S1*a1*py*2.0+S2*a2*pz*2.0-C1*C2*a2*px*2.0-C2*S1*a2*py*2.0+C1*S1*px*py*2.0));
                    end
                else
                    if sol(1) == 1
                        q(1) = -angle(px-py*1i)+angle(d2*1i+d3*1i-sqrt(d2*d3*-2.0-d2^2-d3^2+px^2+py^2));
                    else
                        q(1) = -angle(px-py*1i)+angle(d2*1i+d3*1i+sqrt(d2*d3*-2.0-d2^2-d3^2+px^2+py^2));
                    end
                    S1 = sin(q(1));
                    C1 = cos(q(1));

                    if sol(2) == 1
                        q(2) = angle(d1*pz*2.0i-sqrt(d1*pz^3*4.0+d1^3*pz*4.0-a1^4-a2^4-a3^4-d1^4-py^4-pz^4-C1^4*px^4+C1^2*py^4*2.0-C1^4*py^4+a1^2*a2^2*2.0+a1^2*a3^2*2.0+a2^2*a3^2*2.0-a1^2*d1^2*2.0+a2^2*d1^2*2.0+a3^2*d1^2*2.0-a1^2*py^2*6.0-a2^2*py^2*2.0+a3^2*py^2*2.0-a1^2*pz^2*2.0+a2^2*pz^2*2.0+a3^2*pz^2*2.0-d1^2*py^2*2.0-d1^2*pz^2*6.0-py^2*pz^2*2.0+d1*py^2*pz*4.0+C1^3*a1*px^3*4.0+S1^3*a1*py^3*4.0-C1^2*a1^2*px^2*6.0+C1^2*a2^2*px^2*2.0+C1^2*a3^2*px^2*2.0+C1^2*a1^2*py^2*6.0+C1^2*a2^2*py^2*1.0e1-C1^2*a3^2*py^2*2.0-C1^4*a2^2*py^2*1.2e1+C1^6*a2^2*py^2*4.0-C1^2*d1^2*px^2*2.0+C1^2*d1^2*py^2*2.0-C1^2*px^2*py^2*6.0+C1^4*px^2*py^2*6.0-C1^2*px^2*pz^2*2.0+C1^2*py^2*pz^2*2.0+S1^6*a2^2*py^2*4.0+C1*a1^3*px*4.0+S1*a1^3*py*4.0+a1^2*d1*pz*4.0-a2^2*d1*pz*4.0-a3^2*d1*pz*4.0-C1*a1*a2^2*px*4.0-C1*a1*a3^2*px*4.0-C1*S1*px^3*py*4.0+C1*a1*d1^2*px*4.0+C1*a1*px*py^2*1.2e1+C1*a1*px*pz^2*4.0+S1*a1*a2^2*py*4.0-S1*a1*a3^2*py*4.0+S1*a1*d1^2*py*4.0+S1*a1*px^2*py*4.0+S1*a1*py*pz^2*4.0-C1*S1^3*px*py^3*4.0+C1*S1^3*px^3*py*4.0-C1^3*a1*px*py^2*1.2e1-S1^3*a1*a2^2*py*8.0+C1^2*d1*px^2*pz*4.0-C1^2*d1*py^2*pz*4.0-S1^3*a1*px^2*py*4.0-C1*a1*d1*px*pz*8.0-S1*a1*d1*py*pz*8.0-C1*S1*a1^2*px*py*1.2e1-C1*S1*a2^2*px*py*4.0+C1*S1*a3^2*px*py*4.0-C1*S1*d1^2*px*py*4.0-C1*S1*px*py*pz^2*4.0-C1^2*S1*a1*a2^2*py*8.0+C1^2*S1*a1*px^2*py*8.0+C1*S1^3*a2^2*px*py*8.0+C1^3*S1*a2^2*px*py*8.0+C1*S1*d1*px*py*pz*8.0)-a1^2*1i-a2^2*1i+a3^2*1i-d1^2*1i-py^2*1i-pz^2*1i-C1^2*px^2*1i+C1^2*py^2*1i+C1*a1*px*2.0i+S1*a1*py*2.0i-C1*S1*px*py*2.0i)-angle(-a2*(a1*-1i-d1+pz+C1*px*1i+S1^3*py*1i+C1^2*S1*py*1i));
                    else
                        q(2) = angle(d1*pz*2.0i+sqrt(d1*pz^3*4.0+d1^3*pz*4.0-a1^4-a2^4-a3^4-d1^4-py^4-pz^4-C1^4*px^4+C1^2*py^4*2.0-C1^4*py^4+a1^2*a2^2*2.0+a1^2*a3^2*2.0+a2^2*a3^2*2.0-a1^2*d1^2*2.0+a2^2*d1^2*2.0+a3^2*d1^2*2.0-a1^2*py^2*6.0-a2^2*py^2*2.0+a3^2*py^2*2.0-a1^2*pz^2*2.0+a2^2*pz^2*2.0+a3^2*pz^2*2.0-d1^2*py^2*2.0-d1^2*pz^2*6.0-py^2*pz^2*2.0+d1*py^2*pz*4.0+C1^3*a1*px^3*4.0+S1^3*a1*py^3*4.0-C1^2*a1^2*px^2*6.0+C1^2*a2^2*px^2*2.0+C1^2*a3^2*px^2*2.0+C1^2*a1^2*py^2*6.0+C1^2*a2^2*py^2*1.0e1-C1^2*a3^2*py^2*2.0-C1^4*a2^2*py^2*1.2e1+C1^6*a2^2*py^2*4.0-C1^2*d1^2*px^2*2.0+C1^2*d1^2*py^2*2.0-C1^2*px^2*py^2*6.0+C1^4*px^2*py^2*6.0-C1^2*px^2*pz^2*2.0+C1^2*py^2*pz^2*2.0+S1^6*a2^2*py^2*4.0+C1*a1^3*px*4.0+S1*a1^3*py*4.0+a1^2*d1*pz*4.0-a2^2*d1*pz*4.0-a3^2*d1*pz*4.0-C1*a1*a2^2*px*4.0-C1*a1*a3^2*px*4.0-C1*S1*px^3*py*4.0+C1*a1*d1^2*px*4.0+C1*a1*px*py^2*1.2e1+C1*a1*px*pz^2*4.0+S1*a1*a2^2*py*4.0-S1*a1*a3^2*py*4.0+S1*a1*d1^2*py*4.0+S1*a1*px^2*py*4.0+S1*a1*py*pz^2*4.0-C1*S1^3*px*py^3*4.0+C1*S1^3*px^3*py*4.0-C1^3*a1*px*py^2*1.2e1-S1^3*a1*a2^2*py*8.0+C1^2*d1*px^2*pz*4.0-C1^2*d1*py^2*pz*4.0-S1^3*a1*px^2*py*4.0-C1*a1*d1*px*pz*8.0-S1*a1*d1*py*pz*8.0-C1*S1*a1^2*px*py*1.2e1-C1*S1*a2^2*px*py*4.0+C1*S1*a3^2*px*py*4.0-C1*S1*d1^2*px*py*4.0-C1*S1*px*py*pz^2*4.0-C1^2*S1*a1*a2^2*py*8.0+C1^2*S1*a1*px^2*py*8.0+C1*S1^3*a2^2*px*py*8.0+C1^3*S1*a2^2*px*py*8.0+C1*S1*d1*px*py*pz*8.0)-a1^2*1i-a2^2*1i+a3^2*1i-d1^2*1i-py^2*1i-pz^2*1i-C1^2*px^2*1i+C1^2*py^2*1i+C1*a1*px*2.0i+S1*a1*py*2.0i-C1*S1*px*py*2.0i)-angle(-a2*(a1*-1i-d1+pz+C1*px*1i+S1^3*py*1i+C1^2*S1*py*1i));
                    end
                    S2 = sin(q(2));
                    C2 = cos(q(2));

                    if sol(3) == 1
                        q(3) = angle(a3*1i-sqrt(d1*pz*-2.0+a1^2+a2^2-a3^2+d1^2+py^2+pz^2+C1^2*px^2-C1^2*py^2+C2*a1*a2*2.0-C1*a1*px*2.0+S2*a2*d1*2.0-S1*a1*py*2.0-S2*a2*pz*2.0-C1*C2*a2*px*2.0-C2*S1*a2*py*2.0+C1*S1*px*py*2.0))-angle(a2*-1i-C2*a1*1i-C2*d1+C2*pz+S2*a1-S2*d1*1i+S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py);
                    else
                        q(3) = -angle(a2*-1i-C2*a1*1i-C2*d1+C2*pz+S2*a1-S2*d1*1i+S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i+sqrt(d1*pz*-2.0+a1^2+a2^2-a3^2+d1^2+py^2+pz^2+C1^2*px^2-C1^2*py^2+C2*a1*a2*2.0-C1*a1*px*2.0+S2*a2*d1*2.0-S1*a1*py*2.0-S2*a2*pz*2.0-C1*C2*a2*px*2.0-C2*S1*a2*py*2.0+C1*S1*px*py*2.0));
                    end
                end

                theta(1:3) = q;

            case 'rrp'
                % RRP (Stanford arm like)

                px = T(1,4); py = T(2,4); pz = T(3,4);
                d1 = L(1).d;
                d2 = L(2).d;

                %%% autogenerated code
                if L(1).alpha < 0
                    if sol(1) == 1
                        q(1) = -angle(-px+py*1i)+angle(d2*1i-sqrt(-d2^2+px^2+py^2));
                    else
                        q(1) = angle(d2*1i+sqrt(-d2^2+px^2+py^2))-angle(-px+py*1i);
                    end
                    S1 = sin(q(1));
                    C1 = cos(q(1));

                    if sol(2) == 1
                        q(2) = angle(d1-pz-C1*px*1i-S1*py*1i);
                    else
                        q(2) = angle(-d1+pz+C1*px*1i+S1*py*1i);
                    end
                    S2 = sin(q(2));
                    C2 = cos(q(2));

                    q(3) = -C2*d1+C2*pz+C1*S2*px+S1*S2*py;

                else
                    if sol(1) == 1
                        q(1) = -angle(px-py*1i)+angle(d2*1i-sqrt(-d2^2+px^2+py^2));
                    else
                        q(1) = -angle(px-py*1i)+angle(d2*1i+sqrt(-d2^2+px^2+py^2));
                    end
                    S1 = sin(q(1));
                    C1 = cos(q(1));

                    if sol(2) == 1
                        q(2) = angle(-d1+pz-C1*px*1i-S1*py*1i);
                    else
                        q(2) = angle(d1-pz+C1*px*1i+S1*py*1i);
                    end
                    S2 = sin(q(2));
                    C2 = cos(q(2));

                    q(3) = -C2*d1+C2*pz-C1*S2*px-S1*S2*py;
                end
                theta(1:3) = q;

            case 'kr5'
                %Given function will calculate inverse kinematics for KUKA KR5 robot

                % Equations are calculated and implemented by
                % Gautam Sinha
                % Autobirdz Systems Pvt. Ltd.
                % SIDBI Office,
                % Indian Institute of Technology Kanpur, Kanpur, Uttar Pradesh
                % 208016
                % India
                %email- gautam.sinha705@gmail.com

                % get the a1, a2 and a3-- link lenghts for link no 1,2,3
                L = robot.links;
                a1 = L(1).a;
                a2 = L(2).a;
                a3 = L(3).a;

                % Check wether wrist is spherical or not
                if ~robot.isspherical()
                    error('wrist is not spherical')
                end

                % get d1,d2,d3,d4---- Link offsets for link no 1,2,3,4
                d1 = L(1).d;
                d2 = L(2).d;
                d3 = L(3).d;
                d4 = L(4).d;

                % Get the parameters from transformation matrix
                Ox = T(1,2);
                Oy = T(2,2);
                Oz = T(3,2);

                Ax = T(1,3);
                Ay = T(2,3);
                Az = T(3,3);

                Px = T(1,4);
                Py = T(2,4);
                Pz = T(3,4);

                % Set the parameters n1, n2 and n3 to get required configuration from
                % solution
                n1 = -1;   % 'l'
                n2 = -1;   % 'u'
                n4 = -1;   % 'n'
                if ~isempty(strfind(configuration, 'l'))
                    n1 = -1;
                end
                if ~isempty(strfind(configuration, 'r'))
                    n1 = 1;
                end
                if ~isempty(strfind(configuration, 'u'))
                    if n1 == 1
                        n2 = 1;
                    else
                        n2 = -1;
                    end
                end
                if ~isempty(strfind(configuration, 'd'))
                    if n1 == 1
                        n2 = -1;
                    else
                        n2 = 1;
                    end
                end
                if ~isempty(strfind(configuration, 'n'))
                    n4 = 1;
                end
                if ~isempty(strfind(configuration, 'f'))
                    n4 = -1;
                end

                % Calculation for theta(1)
                r=sqrt(Px^2+Py^2);

                if (n1 == 1)
                    theta(1)= atan2(Py,Px) + asin((d2-d3)/r);
                else
                    theta(1)= atan2(Py,Px)+ pi - asin((d2-d3)/r);
                end

                % Calculation for theta(2)
                X= Px*cos(theta(1)) + Py*sin(theta(1)) - a1;
                r=sqrt(X^2 + (Pz-d1)^2);
                Psi = acos((a2^2-d4^2-a3^2+X^2+(Pz-d1)^2)/(2.0*a2*r));

                if ~isreal(Psi)
                    warning('RTB:ikine6s:notreachable', 'point not reachable');
                    theta = [NaN NaN NaN NaN NaN NaN];
                    return
                end

                theta(2) = atan2((Pz-d1),X) + n2*Psi;

                % Calculation for theta(3)
                Nu = cos(theta(2))*X + sin(theta(2))*(Pz-d1) - a2;
                Du = sin(theta(2))*X - cos(theta(2))*(Pz-d1);
                theta(3) = atan2(a3,d4) - atan2(Nu, Du);

                % Calculation for theta(4)
                Y = cos(theta(1))*Ax + sin(theta(1))*Ay;
                M2 = sin(theta(1))*Ax - cos(theta(1))*Ay ;
                M1 =  ( cos(theta(2)-theta(3)) )*Y + ( sin(theta(2)-theta(3)) )*Az;
                theta(4) = atan2(n4*M2,n4*M1);

                % Calculation for theta(5)
                Nu =  -cos(theta(4))*M1 - M2*sin(theta(4));
                M3 =  -Az*( cos(theta(2)-theta(3)) ) + Y*( sin(theta(2)-theta(3)) );
                theta(5) = atan2(Nu,M3);

                % Calculation for theta(6)
                Z = cos(theta(1))*Ox + sin(theta(1))*Oy;
                L2 = sin(theta(1))*Ox - cos(theta(1))*Oy;
                L1 = Z*( cos(theta(2)-theta(3) )) + Oz*( sin(theta(2)-theta(3)));
                L3 = Z*( sin(theta(2)-theta(3) )) - Oz*( cos(theta(2)-theta(3)));
                A1 = L1*cos(theta(4)) + L2*sin(theta(4));
                A3 = L1*sin(theta(4)) - L2*cos(theta(4));
                Nu =  -A1*cos(theta(5)) - L3*sin(theta(5));
                Du =  -A3;
                theta(6) = atan2(Nu,Du);

            otherwise
                error('RTB:ikine6s:badarg', 'Unknown solution type [%s]', robot.ikineType);
        end

        if ~isempty(theta)
            % Solve for the wrist rotation

            % we need to account for some random translations between the first and last 3
            % joints (d4) and also d6,a6,alpha6 in the final frame.

            T13 = robot.A(1:3, theta(1:3));  % transform of first 3 joints

            % T = T13 * Tz(d4) * R * Tz(d6) Tx(a5)
            Td4 = SE3(0, 0, L(4).d);      % Tz(d4)
            Tt = SE3(L(6).a, 0, L(6).d) * SE3.Rx(L(6).alpha);  % Tz(d6) Tx(a5) Rx(alpha6)

            R = inv(Td4) * inv(T13) * SE3(T) * inv(Tt);

            % the spherical wrist implements Euler angles

            if sol(3) == 1
                theta(4:6) = tr2eul(R, 'flip');
            else
                theta(4:6) = tr2eul(R);

            end
            if L(4).alpha > 0
                theta(5) = -theta(5);
            end

            % remove the link offset angles
            for j=1:robot.n   %#ok<*AGROW>
                theta(j) = theta(j) - L(j).offset;
            end

            % stack the rows
            thetavec(k,:) = theta;
        else
            warning('RTB:ikine6s:notreachable', 'point not reachable');
            thetavec(k,:) = [NaN NaN NaN NaN NaN NaN];
        end
    end
end

% predicates to determine which kinematic solution to use
function s = is_simple(L)
    alpha = [-pi/2 0 pi/2];
    s =     all([L(2:3).d] == 0) && ...
        (all([L(1:3).alpha] == alpha) || all([L(1:3).alpha] == -alpha)) && ...
        all([L(1:3).isrevolute] == 1) && ...
        (L(1).a == 0);
end

function s = is_offset(L)
    alpha = [-pi/2 0 pi/2];
    s =     (all([L(1:3).alpha] == alpha) || all([L(1:3).alpha] == -alpha)) && ...
        all([L(1:3).isrevolute] == 1);
end

function s = is_rrp(L)
    alpha = [-pi/2 pi/2 0];
    s =     all([L(2:3).a] == 0) && ...
        (all([L(1:3).alpha] == alpha) || all([L(1:3).alpha] == -alpha)) && ...
        all([L(1:3).isrevolute] == [1 1 0]);
end

function s = is_puma(L)
    alpha = [pi/2 0 -pi/2];
    s =     (L(2).d == 0) && (L(1).a == 0) && ...
        (L(3).d ~= 0) && (L(3).a ~= 0) && ...
        all([L(1:3).alpha] == alpha) && ...
        all([L(1:3).isrevolute] == 1);
end


function Q = vbpcamv(Y, d, varargin)
% Modified RPPCA by Jaakko Luttinen
% Uses separate outlier models for each observed signal.
% p(y|W,x,mu,u,tau) = N(y | W*x+mu, diag(1/u*1/tau))
% Uses hierarchical models and variational learning.
%
% If you have troubles with bad minima, you may want to try changing the
% time when the algorithm starts to update the hyperparameters
% (startupdatehyper) and/or to rotate (startrotate).

% Last updated 13th February 2009, Jaakko Luttinen

opts = struct( ...
    'init',          [],...
    'prior', [], ...
    'hiermean', false, ...
    'rotate', true, ...
    'commonnoise', true, ...
    'startrotate', 1, ...
    'startupdatehyper', 1, ...
    'autosavetime', 0,...
    'autosavefile', 'vbpcamv_autosave',...
    'testset', [], ...
    'fixw', false, ...
    'maxiters', 1000, ...
    'convergence', eps);

[ opts, errmsg, wrnmsg ] = argschk( opts, varargin{:} );
if ~isempty(errmsg), error( errmsg ), end
if ~isempty(wrnmsg), warning( wrnmsg ), end

[m,n] = size(Y);

% Missing values
if issparse(Y)
  Obs = Y~=0; %spones(Y);
else
  Obs = ~isnan(Y);
end
[ObsI, ObsJ] = find(Obs);

%%%%%%%%%%%%%%%%%%%
%% INITIALIZATION

nmv = sum(Obs,2);

% NAMING CONVENTION:
% variables are named without '_': e.g. a->tau->mu ==> ataumu
% but the values of posterior distribution parameters are named with one
% '_': e.g. a->tau->mu ==> a_taumu
% So, priors (or hyperparameter variables) have no '_'. Understand the
% difference!
% But for clarity mu_W is just W and mu_X is just X and so on. That is,
% plain variable name (W,X,mu,tau,logtau,U,logU) refers to posterior
% expectation if there is no naming conflict.

% Mean (mu)
% $$$ mumumu = 0;     % prior for hierarchical stuff
% $$$ taumumu = 1e-5; % prior for hierarchical stuff
v_mumu = 0;
ataumu = 1e-3;  % prior for hierarchical stuff
btaumu = 1e-3;  % prior for hierarchical stuff
mumu = 0;       % init posterior or prior if no hierarchical mean
taumu = 1e-6;   % init posterior or prior if no hierarchical mean
v_mu = 1e0*ones(m,1); % init posterior
mu = zeros(m,1);      % init posterior

% Loading matrix (W)
%aw = 1e-16; % prior
%bw = 1e-13; % prior
aw = 1e-10; % prior
bw = 1e-5; % prior
a_w = aw * ones(1,d); % init size
b_w = bw * ones(1,d); % init size
w = 1e-6 * ones(1,d);    % init posterior
logw = nan * ones(1,d);  % init size
W = orth(randn(m,d));          % init posterior
CovW = 1e-0*repmat(eye(d),[1,1,m]); % init posterior
varW = nan*ones(m,d);              % init size

% Observation noise
atau = 1e-4; % prior
btau = 1e-4; % prior
a_tau = nan * ones(m,1);
b_tau = nan * ones(m,1);
tau = 1e2 * ones(m,1); % init posterior (THIS SEEMS TO BE IMPORTANT!!!)
%tau = 1./varmv(Y,2) * (m^2)/((m-d)^2)   % adhoc init posterior
logtau = nan * ones(m,1); % init size

% Outlier parameters
% $$$ nu = 10 * ones(m,1); % variable init
% $$$ a_U = nan * ones(m,n); % init size
% $$$ b_U = nan * ones(m,n); % init size
U = ones(m,n);     % init posterior
% $$$ logU = nan * zeros(m,n); % init size

% Initialize sizes
X = nan * zeros(d,n); % init size
Sv = cell(n,1);
Sv(:) = {nan*zeros(d,d)};
CovX = nan*zeros(d,d,n);
XX = nan*zeros(d,d,n);
UXX = nan*zeros(d,d,m);

% Prior information
if isstruct(opts.prior)
  if isfield(opts.prior, 'mumumu')
    mumumu(1) = opts.prior.mumumu
  end
  if isfield(opts.prior, 'taumumu')
    taumumu(1) = opts.prior.taumumu
  end
  if isfield(opts.prior, 'ataumu')
    ataumu(1) = opts.prior.ataumu
  end
  if isfield(opts.prior, 'btaumu')
    btaumu(1) = opts.prior.btaumu
  end
  if isfield(opts.prior, 'aw')
    aw(1) = opts.prior.aw
  end
  if isfield(opts.prior, 'bw')
    bw(1) = opts.prior.bw
  end
  if isfield(opts.prior, 'atau')
    atau(1) = opts.prior.atau
  end
  if isfield(opts.prior, 'btau')
    btau(1) = opts.prior.btau
  end
end

% Use given initialization (size matching is checked by using ':' )
if isstruct(opts.init)
  if isfield(opts.init, 'W')
    W(:,:) = opts.init.W;
    fprintf('Using old W.\n');
  end
  if isfield(opts.init, 'CovW')
    CovW(:,:,:) = opts.init.CovW;
    fprintf('Using old CovW.\n');
  end
  if isfield(opts.init, 'w')
    w(:) = opts.init.w
    fprintf('Using old w.\n');
  end
  if isfield(opts.init, 'X')
    X(:,:) = opts.init.X;
    fprintf('Using old X.\n');
  end
  if isfield(opts.init, 'CovX')
    CovX(:,:,:) = opts.init.CovX;
    fprintf('Using old CovX.\n');
  end
  if isfield(opts.init, 'Sv')
    Sv(:) = opts.init.Sv;
    fprintf('Using old Sv.\n');
  end
  if isfield(opts.init, 'mu')
    mu(:) = opts.init.mu;
    fprintf('Using old mu.\n');
  end
  if isfield(opts.init, 'v_mu')
    v_mu(:) = opts.init.v_mu;
    fprintf('Using old v_mu.\n');
  end
  if isfield(opts.init, 'mumu')
    mumu(1) = opts.init.mumu;
    fprintf('Using old mumu.\n');
  end
  if isfield(opts.init, 'taumu')
    taumu(1) = opts.init.taumu;
    fprintf('Using old taumu.\n');
  end
  if isfield(opts.init, 'tau')
    tau(:) = opts.init.tau;
    fprintf('Using old tau.\n');
  end
  if isfield(opts.init, 'nu')
    nu(:) = opts.init.nu;
    fprintf('Using old nu.\n');
  end
  if isfield(opts.init, 'U')
    U(:,:) = opts.init.U;
    fprintf('Using old U.\n');
  end
end

WW = zeros(d,d,m);
for i=1:m
  WW(:,:,i) = W(i,:)'*W(i,:) + CovW(:,:,i);
end

% Log-likelihood
logP = -Inf;

Im = eye(m);
Id = eye(d);

lastsave = now;

N = 1:n;
vM = 1:m;
m_mv = sum(Obs,1);
n_mv = sum(Obs,2);
nm_mv = sum(Obs(:)); % number of observations

log2pi = log(2*pi);

oldcosts = -inf;

% Monitor the overall time of the iteration
starttime = cputime;
duration = nan*zeros(opts.maxiters,1);
rmse = nan*zeros(opts.maxiters,1);
rmse_test = nan*zeros(opts.maxiters,1);

cost = nan*zeros(opts.maxiters,1);
for k=1:opts.maxiters

  itertime = cputime;

  % This is used to monitor the rotation of the subspace after each step
  W_old = W;

  % Update X
  for j=1:n
    imv = Obs(:,j);
    UTau = diag(U(imv,j).*tau(imv));
    UWWTau = 0;
    for i=vM(imv)
      UWWTau = UWWTau + U(i,j)*tau(i) * WW(:,:,i);
    end
    CovX(:,:,j) = inv(UWWTau + Id);
    X(:,j) = CovX(:,:,j) * W(imv,:)' * UTau * (Y(imv,j)-mu(imv));
    XX(:,:,j) = CovX(:,:,j) + X(:,j)*X(:,j)';
    Sv{j} = CovX(:,:,j); % only for backward compatibility..
  end

  for i=1:m
    jmv = Obs(i,:);

    % Update sum_j(<u><xx>)
    UXX(:,:,i) = 0;
    for j=N(jmv) % sum over observed time instances
      UXX(:,:,i) = UXX(:,:,i) + U(i,j)*XX(:,:,j);%(CovX(:,:,j) + X(:,j)*X(:,j)');
    end

    % Update W
    CovW(:,:,i) = inv(UXX(:,:,i)*tau(i) + diag(w));
    W(i,:) = (U(i,jmv).*(Y(i,jmv)-mu(i))*X(:,jmv)') * CovW(:,:,i) * tau(i);
    WW(:,:,i) = W(i,:)'*W(i,:) + CovW(:,:,i);

    % Update mu
    v_mu(i) = 1 / (taumu + sum(U(i,jmv))*tau(i));
    mu(i) = v_mu(i) * (taumu*mumu + U(i,jmv)*(Y(i,jmv)-W(i,:)*X(:,jmv))'*tau(i));

  end

  % Rotate
  % THINK/TODO: WHAT WOULD BE A GOOD TIME TO START ROTATING??
  % (rotating too early may lead to bad local minima and not rotating
  % makes converging very slow) should one measure how "converged" the
  % loglikelihood is and then decide whether to start rotate?
  if opts.rotate && k>=opts.startrotate
% $$$     before = trace( sum(CovX,3) * sum(CovW,3) )
    [W,CovW,X,Sv,CovX,mu] = orthogonalize(W,CovW,X,Sv,CovX,mu,opts.fixw,tau,Obs);
% $$$     after = trace( sum(CovX,3) * sum(CovW,3) )
%    [W,CovW,X,Sv,CovX,mu] = orthogonalize_TEST(W,CovW,X,Sv,CovX,mu,tau(1)); false
%    [W,CovW,X,CovX,mu] = RotateToPCA(W,CovW,X,CovX,mu);
  end

  for i=1:m
    jmv = Obs(i,:); % observed

    WW(:,:,i) = W(i,:)'*W(i,:) + CovW(:,:,i);
    varW(i,:) = diag(CovW(:,:,i));

    % Update sum_j(<u><xx>)
    UXX(:,:,i) = 0;
    for j=N(jmv) % sum over observed time instances
      UXX(:,:,i) = UXX(:,:,i) + U(i,j)*(CovX(:,:,j) + X(:,j)*X(:,j)');
    end

    % Update tau
    UWXXW = sum(sum( WW(:,:,i) .* UXX(:,:,i) ));
    a_tau(i) = atau + 0.5*sum(jmv);
    b_tau(i) = btau ...
        + 0.5 * (UWXXW + ...
                 sum(-2*W(i,:)*X(:,jmv)*(U(i,jmv).*(Y(i,jmv)-mu(i)))') + ...
                 sum(U(i,jmv).*(Y(i,jmv)-mu(i)).^2) + ...
                 sum(U(i,jmv)*v_mu(i)));

  end

  % "AD HOC"(?): Use common variance
  if opts.commonnoise
    a_tau(:) = sum(a_tau-atau) + atau;
    b_tau(:) = sum(b_tau-btau) + btau;
  end
  tau = a_tau./b_tau;
  logtau = psi(a_tau) - log(b_tau);
  % TODO: DEBUGGING: USE ML ESTIMATE
% $$$   logtau = log(tau);

  % Update hyperparameters for mu
  if ~opts.hiermean
    logtaumu = log(taumu);
  else
    a_taumu = ataumu + 0.5*m;
    b_taumu = btaumu + 0.5*sum( (mu-mumu).^2 + v_mu + v_mumu);
    taumu = a_taumu ./ b_taumu; % posterior mean of accuracy hyperparameter
    logtaumu = psi(a_taumu) - log(b_taumu);
  end

  % Update hyperparameter for W (you can try to start updating these
  % hyperparameters only after some iteration if some problems seem to
  % appear.)
  if ~opts.fixw
    if k >= opts.startupdatehyper
      a_w(:) = aw + 0.5*m;
      b_w(:) = bw + 0.5*sum(W.^2 + varW, 1);
    end
    w = a_w ./ b_w; % posterior mean of precision hyperparameter
    logw = psi(a_w) - log(b_w);
  else
    logw = log(w);
  end

  % Change of the principal subspace (in radians)
  angle = subspace(W,W_old);

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Calculate lower bound of the log-likelihood

  % Cost from Y
  cost_Y = 0;
  for j=1:n
    imv = Obs(:,j);
    for i=vM(imv)
      mean_muY = W(i,:)*X(:,j)+mu(i);
      var_muY = W(i,:)*CovX(:,:,j)*W(i,:)' + X(:,j)'*CovW(:,:,i)*X(:,j) ...
                + sum(sum( CovX(:,:,j).*CovW(:,:,i) )) + v_mu(i);
      tauY = tau(i);
      logtauY = logtau(i);
      cost_Y = cost_Y + cost_gaussian(Y(i,j),[], mean_muY,var_muY, ...
                                      tauY,logtauY);
    end
  end

  % Cost from W
  cost_W = 0;
  for i=1:m
    cost_W = cost_W + cost_gaussian(W(i,:)',CovW(:,:,i), 0,0, w',logw');
  end

  % Cost from X
  cost_X = 0;
  for j=1:n
    cost_X = cost_X + cost_gaussian(X(:,j),CovX(:,:,j), 0,0, 1,0);
  end

  % Cost from mu
  cost_mu = cost_gaussian(mu,diag(v_mu), mumu,v_mumu, taumu,logtaumu);

  % Cost from tau
  if opts.commonnoise
    cost_tau = cost_gamma(tau(1),logtau(1),a_tau(1),b_tau(1),atau,btau);
  else
    cost_tau = sum(cost_gamma(tau,logtau,a_tau,b_tau,atau,btau));
  end

  % Cost from w
  if ~opts.fixw
    cost_w = sum(cost_gamma(w,logw,a_w,b_w,aw,bw));
  else
    cost_w = 0;
  end

  if opts.hiermean
    cost_taumu = cost_gamma(taumu,logtaumu,a_taumu,b_taumu,ataumu,btaumu);
  end

  oldlogP = logP;

  % Lower bound of loglikelihood
  logP = cost_Y + cost_W + cost_X + cost_mu + cost_tau + cost_w;
  %logP = cost_Y;
  costs = [cost_Y; cost_W; cost_X; cost_mu; cost_tau; cost_w];

  cost(k) = logP;

  % Show progress
  time = cputime - itertime;
  if k > 1
    duration(k) = duration(k-1) + time;
  else
    duration(1) = time;
  end
  fprintf('Step %d: loglike=%e, angle=%e  (%f seconds)\n', ...
          k, logP, angle, time);

  % Debugging..
  if logP < oldlogP
%    diff_costs = costs - oldcosts
%    error('Log-likelihood bound not improved! Bug in code or likelihood?');
    warning('Log-likelihood bound decreased! Bug in code or likelihood?');
  end
  oldcosts = costs;

  % RMSE
  Yh = W*X + repmat(mu, 1,n);
  err = Y-Yh;
  err = err(~isnan(err));
  rmse(k) = sqrt( mean(err(:).^2) );
  if ~isempty(opts.testset)
    err = opts.testset-Yh;
    err = err(~isnan(err));
    rmse_test(k) = sqrt( mean(err(:).^2) );
  end

  % Check whether to save the results
  if (now-lastsave)*3600*24 >= opts.autosavetime && opts.autosavetime > 0
    fprintf('Saving to %s...', opts.autosavefile);
    loglikelihood = logP;
    if opts.hiermean
      save(opts.autosavefile, 'W','CovW','w','a_w','b_w','X','CovX','Sv', ...
           'mu','v_mu','mumu','v_mumu','taumu','a_taumu','b_taumu','tau', ...
           'a_tau','b_tau','U','a_U','b_U','nu','loglikelihood');
    else
      save(opts.autosavefile, 'W','CovW','w','a_w','b_w','X','CovX','Sv', ...
           'mu','v_mu','tau', 'a_tau','b_tau', 'loglikelihood', 'cost', ...
           'duration', 'rmse', 'rmse_test');
    end
    lastsave = now;
    fprintf(' done.\n');
  end

% $$$   if angle < 1e-14
% $$$     fprintf('Stopping iteration: only minor change in subspace\n');
% $$$     break
% $$$   end
  improvement = abs((logP - oldlogP) / logP);
  if improvement < opts.convergence && k > opts.startupdatehyper
    fprintf('Stopping iteration: only minor improvement in loglikelihood\n');
    break
  end

end

fprintf('Saving to %s...', opts.autosavefile);
loglikelihood = logP;
if opts.hiermean
  save(opts.autosavefile, 'W','CovW','w','a_w','b_w','X','CovX','Sv', ...
       'mu','v_mu','mumu','v_mumu','taumu','a_taumu','b_taumu','tau', ...
       'a_tau','b_tau','U','a_U','b_U','nu','loglikelihood');
else
  save(opts.autosavefile, 'W','CovW','w','a_w','b_w','X','CovX','Sv', ...
       'mu','v_mu','tau', 'a_tau','b_tau', 'loglikelihood', 'cost', ...
       'duration', 'rmse', 'rmse_test');
end
fprintf(' done.\n');

% Results as a struct if desired
if true %nargout == 1
  results.W = W;
  results.CovW = CovW;
  results.w = w;
  results.a_w = a_w;
  results.b_w = b_w;
  results.X = X;
  results.CovX = CovX;
  results.Sv = Sv;
  results.mu = mu;
  results.v_mu = v_mu;
  if opts.hiermean
    results.mumu = mumu;
    results.v_mumu = v_mumu;
    results.taumu = taumu;
    results.a_taumu = a_taumu;
    results.b_taumu = b_taumu;
  end
  results.tau = tau;
  results.a_tau = a_tau;
  results.b_tau = b_tau;
% $$$   results.U = U;
% $$$   results.a_U = a_U;
% $$$   results.b_U = b_U;
% $$$   results.nu = nu;
  results.loglikelihood = logP;
  results.cost = cost;
  results.time = duration;
  Q = results;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function c = cost_gaussian(x,Covx,mu,Covmu,tau,logtau)
% c = cost_gaussian(x,Covx,mu,Covmu,tau,logtau)
%
% Calculates the difference between <log p(X)> - <log q(X)>
% where expectation is over q(X).
%
% A lower bound for a Gaussian:
% p(X) = N(MU,1/TAU)
% q(X) = N(x,Covx)
%
% Rest of the parameters are defined as:
% q(MU) = N(mu,Covmu)
% <TAU> = tau
% <log TAU> = logtau
%
% If Covx==[], then the term <log q(X)> is not calculated.
% This is useful when X is, e.g., observations.

c = 0;

% Cost from q-posterior
if ~isempty(Covx)
  entropy = entropy_gaussian(Covx);
  c = entropy;
else
  Covx = 0;
end

% Cost from prior
err2 = ((x-mu).^2 + diag(Covx) + diag(Covmu));
c = c + 0.5 * sum( -log(2*pi) + logtau - tau .* err2 );

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function c = cost_gamma(x,logx,apost,bpost,aprior,bprior)
% A lower bound for a Gamma distributed X:
% p(X) = G(aprior,bprior)
% q(X) = G(apost,bpost)
% <X> = x
% <log X> = logx

% Cost from prior
c = aprior*log(bprior) - gammaln(aprior) + (aprior-1)*logx - bprior*x;
% Cost from q-posterior
c = c + entropy_gamma(apost,bpost);

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

function H = entropy_gaussian(Cov)
% Cov is covariance matrix
d = size(Cov,1);

log2pi = log(2*pi);
%H = 0;

%for j=1:size(Cov,3)
L = chol(Cov);
H = d/2*log2pi + 0.5*2*logdettri(L) + d/2;
%end

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

function H = entropy_gamma(a,b)
% a is shape, b is inverse scale

H = a - log(b) + gammaln(a) - (a-1).*psi(a);
%H = sum(H(:));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [ A, Av, S, Sv, mu ] = ...
    RotateToPCA( A, Av, S, Sv, mu);

n1 = size(A,1);
n2 = size(S,2);

% TODO: Take into account the prior for Mu, A???
mS = mean(S,2);
dMu = A*mS;
S = S - repmat(mS,1,n2);
mu = mu + dMu;

covS = S*S';
for j = 1:n2
  covS = covS + Sv(:,:,j);
end

R = 1;

% w.r.t. S
covS = covS / n2;
[VS,D] = eig(covS);
RA = VS*sqrt(D);
A = A*RA;
covA = A'*A;
for i = 1:n1
  Av(:,:,i) = RA'*Av(:,:,i)*RA;
  covA = covA + Av(:,:,i);
end
R = diag(1./sqrt(diag(D)))*VS';

% $$$ % w.r.t. A
% $$$ covA = covA / n1;
% $$$ [VA,DA] = eig(covA);
% $$$ [DA,I] = sort( -diag(DA) );
% $$$ DA = -DA;
% $$$ VA = VA(:,I);
% $$$ A = A*VA;
% $$$ for i = 1:n1
% $$$   Av(:,:,i) = VA'*Av(:,:,i)*VA;
% $$$ end
% $$$ R = VA'*R;

S = R*S;
for j = 1:length(Sv)
    Sv(:,:,j) = R*Sv(:,:,j)*R';
end

return

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

function [W,CovW,X,Sv,CovX,mu] = orthogonalize(W,CovW,X,Sv,CovX,mu,fixw,tau,Obs)
[m,c] = size(W);
n = size(X,2);

WW = W'*W + sum(CovW,3);

% $$$ old_recon = W*X + repmat(mu,1,n);

% Use ML zero mean (mu should be updated after this function!!) ..
if false
  disp('Using complicated but exact mean translation');
  tau = reshape(tau,[1,1,m]);
  nom = 0;
  denom = 0;
  I = eye(c);
  for j=1:n
    imv = Obs(:,j);
    tmp = I + sum(bsxfun(@times,tau(1,1,imv),CovW(:,:,imv)),3);
    nom = nom + tmp * X(:,j);
    denom = denom + tmp;
  end
  dmu = denom \ nom;
else
  dmu = mean(X,2);
end
% .. or my analytic zero mean with proper fix to mu:
% $$$  dmu = inv(n*eye(c)+taumu*WW) * (sum(X,2) + taumu*W'*(mumu-mu));

% Move bias
X = X - repmat(dmu,1,n);
mu = mu + W*dmu;
% $$$ nobiasmove = true

Qx = 1;

% Whiten w.r.t. X
XX = X*X' + sum(CovX,3);
if fixw
  [Vx,Dx,tmp] = svd(XX/(n-m)); % USE THIS IF FIXED w ??
else
  [Vx,Dx,tmp] = svd(XX/n);
end
Qx = diag(1./sqrt(diag(Dx))) * Vx';
Qw = Vx*sqrt(Dx);
W = W * Qw;
for i=1:size(CovW,3)
  CovW(:,:,i) = Qw'*CovW(:,:,i)*Qw;
end
X = Qx * X;
for j = 1:size(CovX,3)
  Sv{j} = Qx*Sv{j}*Qx';
  CovX(:,:,j) = Qx*CovX(:,:,j)*Qx';
end

% Check that XX is really whitened! (because of numerical issues!!)
XX = X*X' + sum(CovX,3);
if fixw
  [Vx,Dx,tmp] = svd(XX/(n-m)); % USE THIS IF FIXED w ??
else
  [Vx,Dx,tmp] = svd(XX/n);
end
if Dx(1) > 1.1
  warning('Needs to whiten X again. See vbpcamv->orthogonalize');
  % Whiten w.r.t. X AGAIN
  Qx = diag(1./sqrt(diag(Dx))) * Vx';
  Qw = Vx*sqrt(Dx);
  W = W * Qw;
  for i=1:size(CovW,3)
    CovW(:,:,i) = Qw'*CovW(:,:,i)*Qw;
  end
  X = Qx * X;
  for j = 1:size(CovX,3)
    Sv{j} = Qx*Sv{j}*Qx';
    CovX(:,:,j) = Qx*CovX(:,:,j)*Qx';
  end
end

Qx = 1;
% Diagonalize w.r.t. W
WW = W'*W + sum(CovW,3);
[Vw,Dw,tmp] = svd(WW);
%[Dw,I] = sort(diag(Dw), 'descend');
%Vw = Vw(:,I);
Qx = Vw' * Qx;
Qw = Vw;
W = W * Qw;
for i=1:size(CovW,3)
  CovW(:,:,i) = Qw'*CovW(:,:,i)*Qw;
end
X = Qx * X;
for j = 1:size(CovX,3)
  Sv{j} = Qx*Sv{j}*Qx';
  CovX(:,:,j) = Qx*CovX(:,:,j)*Qx';
end

% $$$ xx = X*X' + sum(CovX,3)
% $$$ ww = W'*W + sum(CovW,3)

% $$$ new_recon = W*X + repmat(mu,1,n);
% $$$
% $$$ mse = mean((old_recon(:) - new_recon(:)).^2)

return
% $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ function [W,CovW,X,Sv,CovX,mu] = orthogonalize_TEST(W,CovW,X,Sv,CovX,mu,tau)
% $$$ [m,c] = size(W);
% $$$ n = size(X,2);
% $$$
% $$$ WW = W'*W + sum(CovW,3);
% $$$
% $$$ old_recon = W*X + repmat(mu,1,n);
% $$$
% $$$ % Use ML zero mean (mu should be updated after this function!!) ..
% $$$ % $$$ dmu = mean(X,2);
% $$$ % $$$ tau*sum(CovW,3)
% $$$ dmu = inv(n*eye(c)+tau*n*sum(CovW,3)) * (sum((eye(c)+tau*sum(CovW,3))*X, 2));
% $$$ %(dmu2-dmu)./dmu
% $$$ % .. or my analytic zero mean with proper fix to mu:
% $$$ % $$$  dmu = inv(n*eye(c)+taumu*WW) * (sum(X,2) + taumu*W'*(mumu-mu));
% $$$
% $$$ % Move bias
% $$$ X = X - repmat(dmu,1,n);
% $$$ mu = mu + W*dmu;
% $$$
% $$$ Qx = 1;
% $$$
% $$$ % Whiten w.r.t. X
% $$$ XX = X*X' + sum(CovX,3);
% $$$ [Vx,Dx] = eig(XX/n);
% $$$ %[Vx,Dx] = eig(XX/(n+m));
% $$$ %[Vx,Dx] = eig(XX/(n-m));
% $$$ Qx = diag(1./sqrt(diag(Dx))) * Vx';
% $$$ Qw = Vx*sqrt(Dx);
% $$$ W = W * Qw;
% $$$ for i=1:size(CovW,3)
% $$$   CovW(:,:,i) = Qw'*CovW(:,:,i)*Qw;
% $$$ end
% $$$
% $$$ % Whiten w.r.t. W
% $$$ WW = W'*W + sum(CovW,3);
% $$$ [Vw,Dw] = eig(WW);
% $$$ [Dw,I] = sort(diag(Dw), 'descend');
% $$$ Vw = Vw(:,I);
% $$$ Qx = Vw' * Qx;
% $$$ Qw = Vw;
% $$$ W = W * Qw;
% $$$ for i=1:size(CovW,3)
% $$$   CovW(:,:,i) = Qw'*CovW(:,:,i)*Qw;
% $$$ end
% $$$
% $$$ X = Qx * X;
% $$$ for j = 1:size(CovX,3)
% $$$   Sv{j} = Qx*Sv{j}*Qx';
% $$$   CovX(:,:,j) = Qx*CovX(:,:,j)*Qx';
% $$$ end
% $$$
% $$$ new_recon = W*X + repmat(mu,1,n);
% $$$
% $$$ mse = mean((old_recon(:) - new_recon(:)).^2);
% $$$
% $$$ return
% $$$


function EmergencyTrajectory = calcEmergencyVelocityProfile_veldep(TargetTrajectory, ...
    drag_coefficient, roh_air, vehiclemass_kg, ...
    P_VDC_AccLimits_v_mps, P_VDC_AccLimits_ax_mps2, P_VDC_AccLimits_ay_mps2)

% Authors:       Alexander Wischnewski
%
% Description:
%   calculates a brake velocity profile to a given target trajectory to generate a reasonable
%   emergency backup

% Inputs:
%   TargetTrajectory    Trajectory to be checked (see tests for format)
%   drag_coefficient    Vehicle drag coefficient
%   roh_air             air density
%   vehiclemass_kg      vehicle mass
%   P_VDC_AccLimits_v_mps:  Acceleration limit map - velocity interpolation points
%   P_VDC_AccLimits_ax_mps2: Acceleration limit map - long. acc. limits
%   P_VDC_AccLimits_ay_mps2: Acceleration limit map - lat. acc. limits
%
% Outputs:
%   EmergencyTrajectory True if trajectory complies with all specs

% copy target trajectory
EmergencyTrajectory = TargetTrajectory;
% only if the trajectory is a driving trajectory
if(EmergencyTrajectory.v_mps(1) > 1)
    % calculate new velocity profile starting with the fifth element to ensure that the trajectory
    % does not begin prior to switching on it as this might cause serious issues
    for i = 6:1:50
        % distance to next point
        dS = double(EmergencyTrajectory.s_loc_m(i) - EmergencyTrajectory.s_loc_m(i-1));
        % update acceleration limits
        EmergencyTrajectory.ax_lim_mps2(i-1) = interp1(P_VDC_AccLimits_v_mps, P_VDC_AccLimits_ax_mps2, EmergencyTrajectory.v_mps(i-1));
        EmergencyTrajectory.ay_lim_mps2(i-1) = interp1(P_VDC_AccLimits_v_mps, P_VDC_AccLimits_ay_mps2, EmergencyTrajectory.v_mps(i-1));
        % calculate allowed longitudinal deceleration
        ax_mps2 = -(1 - abs(double(EmergencyTrajectory.kappa_radpm(i-1))*double(EmergencyTrajectory.v_mps(i-1)).^2/EmergencyTrajectory.ay_lim_mps2(i-1)))*EmergencyTrajectory.ax_lim_mps2(i-1);
        % add acceleration from drag
        ax_mps2 = ax_mps2 - 0.5*drag_coefficient*roh_air*EmergencyTrajectory.v_mps(i-1)^2/vehiclemass_kg;
        % calculate time needed to travel there using pq formula
        p = 2*EmergencyTrajectory.v_mps(i-1)/ax_mps2;
        q = -2*dS/ax_mps2;
        % check if the solution is still valid
        if((p/2)^2 > q && EmergencyTrajectory.v_mps(i-1) > 0.2)
            v_next = (-p/2 - sqrt((p/2)^2 - q))*ax_mps2 + EmergencyTrajectory.v_mps(i-1);
        else
            v_next = 0;
        end
        % set calculated speed and start next step with this speed
        EmergencyTrajectory.v_mps(i) = single(v_next);
        EmergencyTrajectory.ax_mps2(i-1) = ax_mps2;
    end
    % set final acceleration to value of before
    % use a lot less as this might lead to issues with the safety checks otherwise
    % speed should be zero here anyway
    EmergencyTrajectory.ax_mps2(50) = 0.5*EmergencyTrajectory.ax_mps2(49);
else
    % set speeds and acceleration to zero if the initial planning speed is too slow for driving
    EmergencyTrajectory.v_mps = zeros(50, 1);
    EmergencyTrajectory.ax_mps2 = zeros(50, 1);
end


function [pnt, tri] = triangulate_seg(seg, npnt, origin)

% TRIANGULATE_SEG constructs a triangulation of the outer surface of a
% segmented volume. It starts at the center of the volume and projects the
% vertices of an evenly triangulated sphere onto the outer surface. The
% resulting surface is star-shaped from the origin of the sphere.
%
% Use as
%   [pnt, tri] = triangulate_seg(seg, npnt, origin)
%
% Input arguments:
%  seg    = 3D-matrix (boolean) containing segmented volume. If not boolean
%           seg = seg(~=0);
%  npnt   = requested number of vertices
%  origin = 1x3 vector specifying the location of the origin of the sphere
%           in voxel indices. This argument is optional. If undefined, the
%           origin of the sphere will be in the centre of the volume.
%
% Output arguments:
%  pnt = Nx3 matrix of vertex locations
%  tri = Mx3 matrix of triangles
%
% Seg will be checked for holes, and filled if necessary. Also, seg will be
% checked to consist of a single boolean blob. If not, only the outer surface
% of the largest will be triangulated. SPM is used for both the filling and
% checking for multiple blobs.
%
% See also KSPHERE

% Copyright (C) 2005-2012, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% impose it to be boolean
seg = (seg~=0);
dim = size(seg);
len = ceil(sqrt(sum(dim.^2))/2);

if ~any(seg(:))
  ft_error('the segmentation is empty')
end

% define the origin if it is not provided in the input arguments
if nargin<3
  origin(1) = dim(1)/2;
  origin(2) = dim(2)/2;
  origin(3) = dim(3)/2;
end

% ensure that the seg consists of only one filled blob.
% if not filled: throw a warning and fill
% if more than one blob: throw a warning and use the biggest

% look for holes
seg = volumefillholes(seg);

% ensure that SPM is available, needed for spm_bwlabel
ft_hastoolbox('spm8up', 3) || ft_hastoolbox('spm2', 1);

% look for >1 blob
[lab, num] = spm_bwlabel(double(seg), 26);
if num>1,
  ft_warning('the segmented volume consists of more than one compartment, using only the biggest one for the segmentation');

  for k = 1:num
    n(k) = sum(lab(:)==k);
  end
  [m,ix] = max(n);
  seg(lab~=ix) = false;
end

% start with a unit sphere with evenly distributed vertices
[pnt, tri] = mesh_sphere(npnt, 'ksphere');

ishollow = false;

for i=1:npnt
  % construct a sampled line from the center of the volume outward into the direction of the vertex
  lin = (0:0.5:len)' * pnt(i,:);
  lin(:,1) = lin(:,1) + origin(1);
  lin(:,2) = lin(:,2) + origin(2);
  lin(:,3) = lin(:,3) + origin(3);
  % round the sampled line towards the nearest voxel indices, which allows
  % a quick nearest-neighbour interpolation/lookup
  lin = round(lin);
  % exclude indices that do not lie within the volume
  sel = lin(:,1)<1 | lin(:,1)>dim(1) | ...
        lin(:,2)<1 | lin(:,2)>dim(2) | ...
        lin(:,3)<1 | lin(:,3)>dim(3);
  lin = lin(~sel,:);
  sel = sub2ind(dim, lin(:,1), lin(:,2), lin(:,3));

  % interpolate the segmented volume along the sampled line
  int = seg(sel);

  % the value along the line is expected to be 1 at first and then drop to 0
  % anything else suggests that the segmentation is hollow
  ishollow = any(diff(int)==1);

  % find the last sample along the line that is inside the segmentation
  sel = find(int, 1, 'last');
  % this is a problem if sel is empty. If so, use the edge of the volume
  if ~isempty(sel)
    % take the last point inside and average with the first point outside
    pnt(i,:) = lin(sel,:);
  else
    % take the edge
    pnt(i,:) = lin(end,:);
  end
end

if ishollow
  % this should not have hapened, especially not after filling the holes
  ft_warning('the segmentation is not star-shaped, please check the surface mesh');
end

% undo the shift of the origin from where the projection is done
% pnt(:,1) = pnt(:,1) - origin(1);
% pnt(:,2) = pnt(:,2) - origin(2);
% pnt(:,3) = pnt(:,3) - origin(3);

% fast unconditional re-implementation of the standard MATLAB function
function [s] = sub2ind(dim, i, j, k)
s = i + (j-1)*dim(1) + (k-1)*dim(1)*dim(2);


%% Import from VPSC
%
%%
% <https://public.lanl.gov/lebenso/ VPSC> is a crystal plasticity code
% originally written by Ricardo Lebensohn and Carlos Tome from Los Alamos
% National Laboratory - USA.
%
% Original code can be requested to lebenso@lanl.gov
%
% https://public.lanl.gov/lebenso/
%
%% Import the orientations generated by VPSC
%
% Running a simulation in VPSC ussually results in an output file
% |TEX_PH1.OUT| which contains multiple sets of orientations for different
% strain levels. As these files does not contain any information on the
% crystal symmetry we need to specify it first

cs = crystalSymmetry('222', [4.762 10.225 5.994],'mineral', 'olivine');

%%
% In the next step the orientations are imported and converted into a list
% of <SO3Fun.SO3Fun.html ODFs> using the command <ODF.load.html |ODF.load|>.

% put in here the path to the VPSC output files
path2file = [mtexDataPath filesep 'VPSC'];

odf = ODF.load([path2file filesep 'TEX_PH1.OUT'],'halfwidth',10*degree,'CS',cs)

%%
% The individuel ODFs can be accessed by |odf{id}|

% lets plot the second ODF
plotSection(odf{2},'sigma','figSize','normal')

%%
% The information about the strain are stored as additional properties
% within each ODF variable

odf{1}.opt

%% Compare pole figures during deformation
%
% Next we examine the evaluation of the ODF during the deformation by
% plotting strain depended pole figures.

% define some crystal directions
h = Miller({1,0,0},{0,1,0},{0,0,1},cs,'uvw');

% generate some figure
fig = newMtexFigure('layout',[4,3],'figSize','huge');
subSet = 1:4;

% plot pole figures for different strain steps
for n = subSet
  nextAxis
  plotPDF(odf{n},h,'lower','contourf','doNotDraw');
  ylabel(fig.children(end-2),['\epsilon = ',xnum2str(odf{n}.opt.strain)]);
end
setColorRange('equal')
mtexColorbar

%% Visualize slip system activity
%
% Alongside with the orientation data VPSC also outputs a file
% |ACT_PH1.OUT| which contains the activity of the different slip systems
% during the deformation. Lets read this file as a table

ACT = readtable([path2file filesep 'ACT_PH1.OUT'],'FileType','text')

%%
% and plot the slip activity with respect to the strain for the different
% modes

% loop though the columns MODE1 ... MOD11
close all
for n = 3: size(ACT,2)

  % perform the plotting
  plot(ACT.STRAIN, table2array(ACT(:,n)),'linewidth',2,...
    'DisplayName',['Slip mode ',num2str(n-2)])
  hold on;
end
hold off

% some styling
xlabel('Strain');
ylabel('Slip activity');
legend('show','location','NorthEastOutside');

set(gca,'Ylim',[-0.005 1])
set(gcf,'MenuBar','none','units','normalized','position',[0.25 0.25 0.5 0.5]);

%for only one mode plot, e.g.,mode 3: cs = csapi(STRAIN,MODE{3});fnplt(cs,3,'color','b');hold off;


function [params] = sv_calcB(vResults)
% function [vResults] = sv_calcB(vResults)
% -------------------------------------
% Calculation of b-value uncertainty from given mValueGrid_ determined with
% sv_calcMc
%
%
% Input parameters:
% vResults
%
% Output parameters:
%
%
% J. Woessner; woessner@seismo.ifg.ethz.ch
% last update: 14.04.03

% Initialize

% Create Indices to catalog
[vResults.caNodeIndices] = ex_CreateIndexCatalog(vResults.mCatalog, vResults.mPolygon, vResults.bMap, vResults.nGriddingMode,...
    vResults.nNumberEvents, vResults.fRadius, vResults.fSizeRectHorizontal, vResults.fSizeRectDepth);

% Determine time period of catalog
vResults.fTminCat = min(vResults.mCatalog(:,3));
vResults.fTmaxCat = max(vResults.mCatalog(:,3));
% Adjust to decimal years
fTimePeriod =vResults.fTimePeriod/365;

% Init result matrix
mValueGrid_ = [];

% Loop over all grid nodes
for nNode_ = 1:length(vResults.mPolygon(:,1))
    nNode_
    % Create node catalog
    mNodeCatalog_ = vResults.mCatalog(vResults.caNodeIndices{nNode_}, :);
    % Select time period for mNodeCatalog_
    vSel = (vResults.fTstart <= mNodeCatalog_(:,3) & mNodeCatalog_(:,3) < vResults.fTstart+fTimePeriod);
    mNodeCatalog_ = mNodeCatalog_(vSel,:);
    % Check for constant number of events calculations
    if (vResults.nGriddingMode == 0)
        [mNodeCatalog_] = ex_MaxRadius(vResults.mCatalog, vResults.mPolygon, nNode_, vResults.caNodeIndices, vResults.fMaxRadius, vResults.nNumberEvents, vResults.bMap);
    end
    [nX,nY] = size(mNodeCatalog_);
    if (nX < vResults.nMinimumNumber)
        mValueGrid_= [mValueGrid_; NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN];
    else
        try
            % b-value for median Mc(EMR)
            vSel1=(mNodeCatalog_(:,6) >= vResults.mValueGrid(nNode_,6));
            mCat1=mNodeCatalog_(vSel1,:);
            [fMeanMag1, fBValue1, fStdDev1, fAValue1] =  calc_bmemag(mCat1, vResults.fBinning);
            % b-value for 16 percentile Mc
            vSel2=(mNodeCatalog_(:,6) >= vResults.mValueGrid(nNode_,16));
            mCat2=mNodeCatalog_(vSel2,:);
            [fMeanMag2, fBValue2, fStdDev2, fAValue2] =  calc_bmemag(mCat2, vResults.fBinning);
            % b-value for 84 percentile Mc
            vSel3=(mNodeCatalog_(:,6) >= vResults.mValueGrid(nNode_,17));
            mCat3=mNodeCatalog_(vSel3,:);
            [fMeanMag3, fBValue3, fStdDev3, fAValue3] =  calc_bmemag(mCat3, vResults.fBinning);
            % b-value check for Mc(EMR)
            vSel4=(mNodeCatalog_(:,6) >= vResults.mValueGrid(nNode_,5));
            mCat4=mNodeCatalog_(vSel4,:);
            [fMeanMag4, fBValue4, fStdDev4, fAValue4] =  calc_bmemag(mCat4, vResults.fBinning);
            mValueGrid_= [mValueGrid_; fBValue1 fStdDev1 fAValue1 fBValue2 fStdDev2 fAValue2...
                    fBValue3 fStdDev3 fAValue3 fMeanMag4 fBValue4 fStdDev4 fAValue4];
        catch
            mValueGrid_= [mValueGrid_; NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN];
        end
    end; % End of if on length(mNodeCatalog_(:,1)
end; % for nNode
save(['Bval_result_Time' num2str(vResults.fTstart) '_Nmin_' num2str(vResults.nMinimumNumber) '.mat'], 'mValueGrid_');