function [vertices2, faces2] = subdivideMesh(vertices, faces, n)
%SUBDIVIDEMESH Subdivides each face of the mesh
%
%   [V2 F2] = subdivideMesh(V, F, N)
%   Subdivides the mesh specified by (V,F) such that each face F is divided
%   into N^2 smaller faces.
%
%   Example
%     [v f] = createOctahedron;
%     figure; drawMesh(v, f); view(3);
%     [v2 f2] = subdivideMesh(v, f, 4);
%     figure; drawMesh(v2, f2); view(3)
%
%   See also
%     meshes3d, drawMesh
%

% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2013-08-22,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2013 INRA - Cepia Software Platform.

%% Initialisations

if ~isnumeric(faces) || size(faces, 2) ~= 3
    error('Requires a triangular mesh');
end

% compute the edge array
% edgeVertexIndices = computeMeshEdges(faces);
edges = meshEdges(faces);
nEdges = size(edges, 1);

% index of faces around each edge
% edgeFaceIndices = meshEdgeFaces(vertices, edges, faces);

% index of edges around each face
faceEdgeIndices = meshFaceEdges(vertices, edges, faces);

%% Create new vertices on edges

% several interpolated positions
t = linspace(0, 1, n + 1)';
coef2 = t(2:end-1);
coef1 = 1 - t(2:end-1);

% initialise the array of new vertices
vertices2 = vertices;

% keep an array containing index of new vertices for each original edge
edgeNewVertexIndices = zeros(nEdges, n-1);

% create new vertices on each edge
for iEdge = 1:nEdges
    % extract each extremity as a point
    v1 = vertices(edges(iEdge, 1), :);
    v2 = vertices(edges(iEdge, 2), :);

    % compute new points
    newPoints = coef1 * v1 + coef2 * v2;

    % add new vertices, and keep their indices
    edgeNewVertexIndices(iEdge,:) = size(vertices2, 1) + (1:n-1);
    vertices2 = [vertices2 ; newPoints]; %#ok<AGROW>
end

%% Process each face

faces2 = zeros(0, 3);

nFaces = size(faces, 1);
for iFace = 1:nFaces
    % compute index of each corner vertex
    face = faces(iFace, :);
    iv1 = face(1);
    iv2 = face(2);
    iv3 = face(3);

    % compute index of each edge
    faceEdges = faceEdgeIndices{iFace};
    ie1 = faceEdges(1);
    ie2 = faceEdges(2);
    ie3 = faceEdges(3);

    % indices of new vertices on edges
    edge1NewVertexIndices = edgeNewVertexIndices(ie1, :);
    edge2NewVertexIndices = edgeNewVertexIndices(ie2, :);
    edge3NewVertexIndices = edgeNewVertexIndices(ie3, :);

    % keep vertex 1 as reference for edges 1 and 3
    if edges(ie1, 1) ~= iv1
        edge1NewVertexIndices = edge1NewVertexIndices(end:-1:1);
    end
    if edges(ie3, 1) ~= iv1
        edge3NewVertexIndices = edge3NewVertexIndices(end:-1:1);
    end

    % create the first new face, on 'top' of the original face
    topVertexInds = [edge1NewVertexIndices(1) edge3NewVertexIndices(1)];
    newFace = [iv1 topVertexInds];
    faces2 = [faces2; newFace]; %#ok<AGROW>

    % iterate over middle strips
    for iStrip = 2:n-1
        % index of extreme vertices of current row
        ivr1 = edge1NewVertexIndices(iStrip);
        ivr2 = edge3NewVertexIndices(iStrip);

        % extreme vertices as points
        v1 = vertices2(ivr1, :);
        v2 = vertices2(ivr2, :);

        % create additional vertices within the bottom row of the strip
        t = linspace(0, 1, iStrip+1)';
        coef2 = t(2:end-1);
        coef1 = 1 - t(2:end-1);
        newPoints = coef1 * v1 + coef2 * v2;

        % compute indices of new vertices in result array
        newInds = size(vertices2, 1) + (1:iStrip-1);
        botVertexInds = [ivr1 newInds ivr2];

        % add new vertices
        vertices2 = [vertices2 ; newPoints]; %#ok<AGROW>

        % create top faces of current strip
        for k = 1:iStrip-1
            newFace = [topVertexInds(k) botVertexInds(k+1) topVertexInds(k+1)];
            faces2 = [faces2; newFace]; %#ok<AGROW>
        end

        % create bottom faces of current strip
        for k = 1:iStrip
            newFace = [topVertexInds(k) botVertexInds(k) botVertexInds(k+1)];
            faces2 = [faces2; newFace]; %#ok<AGROW>
        end

        % bottom vertices of current strip are top vertices of next strip
        topVertexInds = botVertexInds;
    end

    % for edge 2, keep vertex 2 of the current face as reference
    if edges(ie2, 1) ~= iv2
        edge2NewVertexIndices = edge2NewVertexIndices(end:-1:1);
    end

    % consider new vertices together with extremities
    botVertexInds = [iv2 edge2NewVertexIndices iv3];

    % create top faces for last strip
    for k = 1:n-1
        newFace = [topVertexInds(k) botVertexInds(k+1) topVertexInds(k+1)];
        faces2 = [faces2; newFace]; %#ok<AGROW>
    end

    % create bottom faces for last strip
    for k = 1:n
        newFace = [topVertexInds(k) botVertexInds(k) botVertexInds(k+1)];
        faces2 = [faces2; newFace]; %#ok<AGROW>
    end

end


function [ x, y ] = sswap ( n, x, incx, y, incy )

%*****************************************************************************80
%
%% SSWAP interchanges two vectors.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 May 2006
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979.
%
%    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
%    Basic Linear Algebra Subprograms for Fortran Usage,
%    Algorithm 539,
%    ACM Transactions on Mathematical Software,
%    Volume 5, Number 3, September 1979, pages 308-323.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vectors.
%
%    Input, real X(*), one of the vectors to swap.
%
%    Input, integer INCX, the increment between successive entries of X.
%
%    Input, real Y(*), one of the vectors to swap.
%
%    Input, integer INCY, the increment between successive elements of Y.
%
%    Output, real X(*), the swapped vector.
%
%    Output, real Y(*), the swapped vector.
%
  if ( n <= 0 )

  elseif ( incx == 1 & incy == 1 )

    m = mod ( n, 3 );

    for i = 1 : m
      temp = x(i);
      x(i) = y(i);
      y(i) = temp;
    end

    for i = m+1 : 3 : n

      temp = x(i);
      x(i) = y(i);
      y(i) = temp;

      temp = x(i+1);
      x(i+1) = y(i+1);
      y(i+1) = temp;

      temp = x(i+2);
      x(i+2) = y(i+2);
      y(i+2) = temp;

    end

  else

    if ( 0 <= incx )
      ix = 1;
    else
      ix = ( - n + 1 ) * incx + 1;
    end

    if ( 0 <= incy )
      iy = 1;
    else
      iy = ( - n + 1 ) * incy + 1;
    end

    for i = 1 : n
      temp = x(ix);
      x(ix) = y(iy);
      y(iy) = temp;
      ix = ix + incx;
      iy = iy + incy;
    end

  end

  return
end


  function [phantom, params] = rect_im(ig, params, varargin)
%|function [phantom, params] = rect_im(ig, params, options)
%|
%| generate rectangle phantom image from parameters:
%|	[x_center y_center x_width y_width angle_degrees amplitude]
%|
%| in
%|	ig			image_geom() object
%|	params			rect parameters, if empty, use the default
%|				params = rect_im_default_parameters(xfov, yfov)
%|
%| options
%|	'oversample'	int	oversampling factor, for grayscale boundaries
%|	'hu_scale'	float	use 1000 to scale shepp-logan to HU (default: 1)
%|	'fov'		float	default ig.fov
%|
%| out
%|	phantom		[nx ny]	image
%|
%| 2008-08-04, Yong Long, adapted from ellipse_im()
%| Copyright 2006-2-2, Jeff Fessler, University of Michigan

if nargin == 1 && streq(ig, 'test'), rect_im_test, return, end
if nargin < 1, ir_usage, end

arg.oversample = 1;
arg.replace = 0;
arg.hu_scale = 1;
arg.fov = [];
arg.chat = false;
arg = vararg_pair(arg, varargin);
if isempty(arg.fov), arg.fov = ig.fov; end

if ~isvar('params') || isempty(params)
	params = rect_im_default_parameters(arg.fov, arg.fov);
end

params(:,6) = params(:,6) * arg.hu_scale;

do_fast = params(:,5) == 0; % non-rotated ones are faster

if arg.chat
	printm('%d of %d fast', sum(do_fast), numel(do_fast))
end

if any(do_fast)
	phantom = rect_im_fast(params(do_fast,:), ...
		ig.nx, ig.ny, ig.dx, ig.dy, ig.offset_x, ig.offset_y, ...
		arg.replace);
else
	phantom = 0;
end

if any(~do_fast)
	phantom = phantom + rect_im_slow(params(~do_fast,:), ...
		ig.nx, ig.ny, ig.dx, ig.dy, ig.offset_x, ig.offset_y, ...
		arg.oversample, arg.replace);
end

% rect_im_fast()
% fast version for non-rotated rectangles
% using exact integration over each pixel so over-sampling is irrelevant
function phantom = rect_im_fast(params, ...
	nx, ny, dx, dy, offset_x, offset_y, replace)

if size(params,2) ~= 6
	error 'bad rect parameter vector size'
end

phantom = zeros(nx, ny);

wx = (nx-1)/2 + offset_x;
wy = (ny-1)/2 + offset_y;
x1 = ([0:nx-1]' - wx) * dx; % col
y1 = ([0:ny-1] - wy) * dy; % row

fun = @(x1, x2, wx) ... % integrated rect(x/wx) function from x1 to x2
	max(min(x2, wx/2) - max(x1, -wx/2), 0);

%ticker reset
ne = nrow(params);
for ie = 1:ne
%	ticker(mfilename, ie, ne)

	rect = params(ie, :);
	cx = rect(1);	wx = rect(3);
	cy = rect(2);	wy = rect(4);
	theta = deg2rad(rect(5));
	if theta ~= 0, fail 'theta=0 required', end
	x = x1 - cx;
	y = y1 - cy;
	tx = fun(x-abs(dx)/2, x+abs(dx)/2, wx) / abs(dx);
	ty = fun(y-abs(dy)/2, y+abs(dy)/2, wy) / abs(dy);
	tmp = single(tx) * single(ty); % outer product (separable)
	if replace
		phantom(tmp > 0) = rect(6);
	else
		phantom = phantom + rect(6) * tmp;
	end
end

% rect_im_slow()
% slower version that handles rotation too and uses over-sampling
function phantom = rect_im_slow(params, ...
	nx, ny, dx, dy, offset_x, offset_y, over, replace)

if size(params,2) ~= 6
	error 'bad rect parameter vector size'
end

phantom = zeros(nx*over, ny*over);

wx = (nx*over-1)/2 + offset_x*over;
wy = (ny*over-1)/2 + offset_y*over;
xx = ([0:nx*over-1] - wx) * dx / over;
yy = ([0:ny*over-1] - wy) * dy / over;
[xx yy] = ndgrid(xx, yy);

ticker reset
ne = nrow(params);
for ie = 1:ne
	ticker(mfilename, ie, ne)

	rect = params(ie, :);
	cx = rect(1);	wx = rect(3);
	cy = rect(2);	wy = rect(4);
	theta = deg2rad(rect(5));
	x = cos(theta) * (xx-cx) + sin(theta) * (yy-cy);
	y = -sin(theta) * (xx-cx) + cos(theta) * (yy-cy);
	tmp = abs(x / wx) < 1/2 & abs(y / wy) < 1/2;
	if replace
		phantom(tmp > 0) = rect(6);
	else
		phantom = phantom + rect(6) * tmp;
	end
end

phantom = downsample2(phantom, over);

%
% default params for rectangles
% the first four columns are unitless for fov=64
%
function params = rect_im_default_parameters(xfov, yfov)
f = 1/64;
params = [ ...
	0	0	50	50	0	1
	10	-16	25	16	0	-0.5
	-13	15	13	13	1*45	1
...
	-18	0	1	1	0	1
	-12	0	1	1	0	1
	-6	0	1	1	0	1
	0	0	1	1	0	1
	6	0	1	1	0	1
	12	0	1	1	0	1
	18	0	1	1	0	1
];

params(:,[1 3]) = params(:,[1 3]) * xfov / 64;
params(:,[2 4]) = params(:,[2 4]) * yfov / 64;

% rect_im_test()
function rect_im_test
ig = image_geom('nx', 2^8, 'ny', 2^8, 'fov', 100);
pic = rect_im(ig, [[0.5 0 3 20]*ig.dx 0 1], 'oversample', 3);
jf_equal(unique(pic), [0;1])
im(pic)
pic = rect_im(ig, [], 'oversample', 3, 'chat', im);
%unique(pic)
im(ig.x, ig.y, pic, 'default rects'), cbar


function value = r8vec_amax ( n, a )

%*****************************************************************************80
%
%% R8VEC_AMAX returns the maximum absolute value in an R8VEC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the array.
%
%    Input, real A(N), the array.
%
%    Output, real VALUE, the value of the entry
%    of largest magnitude.
%
  value = max ( abs ( a(1:n) ) );

  return
end


% MP_SOLVE_MAXWELL_SRC_2D: Solve the 2d Maxwell source problem with a B-spline discretization, in a multipatch domain.
%
% Example to solve the problem
%
%    curl ( epsilon(x) curl (u)) + mu u = f      in Omega
%              (epsilon(x) curl(u)) x n = g      on Gamma_N
%                                 u x n = h x n  on Gamma_D
%
% where the domain \Omega is formed by several patches of the form F((0,1)^n).
%
% USAGE:
%
%  [geometry, msh, space, u] = mp_solve_maxwell_src (problem_data, method_data)
%
% INPUT:
%
%  problem_data: a structure with data of the problem. It contains the fields:
%    - geo_name:     name of the file containing the geometry
%    - nmnn_sides:   sides with Neumann boundary condition (may be empty)
%    - drchlt_sides: sides with Dirichlet boundary condition
%    - c_elec_perm:  electric permittivity (epsilon in the equation)
%    - c_magn_perm:  magnetic permeability (mu in the equation)
%
%  method_data : a structure with discretization data. Its fields are:
%    - degree:     degree of the spline functions.
%    - regularity: continuity of the spline functions.
%    - nsub:       number of subelements with respect to the geometry mesh
%                   (nsub=1 leaves the mesh unchanged)
%    - nquad:      number of points for Gaussian quadrature rule
%
% OUTPUT:
%
%  geometry:  array of geometry structures (see mp_geo_load)
%  msh:       multipatch mesh, consisting of several Cartesian meshes (see msh_multipatch)
%  space:     multipatch space, formed by several tensor product spaces plus connectivity and orientation (see sp_multipatch)
%  u:         the computed degrees of freedom
%
% Copyright (C) 2010-2015 Rafael Vazquez
%
%    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/>.

function [geometry, msh, space, u] = mp_solve_maxwell_src (problem_data, method_data)

% Extract the fields from the data structures into local variables
data_names = fieldnames (problem_data);
for iopt  = 1:numel (data_names)
  eval ([data_names{iopt} '= problem_data.(data_names{iopt});']);
end
data_names = fieldnames (method_data);
for iopt  = 1:numel (data_names)
  eval ([data_names{iopt} '= method_data.(data_names{iopt});']);
end

% Construct geometry structure
[geometry, boundaries, interfaces, ~, boundary_interfaces] = mp_geo_load (geo_name);
npatch = numel (geometry);

msh = cell (1, npatch);
sp = cell (1, npatch);
for iptc = 1:npatch
  [knots, zeta] = ...
         kntrefine (geometry(iptc).nurbs.knots, nsub-1, degree, regularity);
  [knots_hcurl, degree_hcurl] = knt_derham (knots, degree);

% Construct msh structure
  rule      = msh_gauss_nodes (nquad);
  [qn, qw]  = msh_set_quad_nodes (zeta, rule);
  msh{iptc} = msh_cartesian (zeta, qn, qw, geometry(iptc));

% Construct space structure
  scalar_spaces = cell (msh{iptc}.ndim, 1);
  for idim = 1:msh{iptc}.ndim
    scalar_spaces{idim} = sp_bspline (knots_hcurl{idim}, degree_hcurl{idim}, msh{iptc});
  end
  sp{iptc} = sp_vector (scalar_spaces, msh{iptc}, 'curl-preserving');
  clear scalar_spaces
end

msh = msh_multipatch (msh, boundaries);
space = sp_multipatch (sp, msh, interfaces, boundary_interfaces);
clear sp

stiff_mat = op_curlu_curlv_mp (space, space, msh, c_stiff);
mass_mat = op_u_v_mp (space, space, msh, c_mass);
rhs = op_f_v_mp (space, msh, f);

% Apply Neumann boundary conditions
Nbnd = cumsum ([0, boundaries.nsides]);
for iref = nmnn_sides
  iref_patch_list = Nbnd(iref)+1:Nbnd(iref+1);
  gref = @(varargin) g(varargin{:},iref);
  rhs_nmnn = op_f_v_mp (space.boundary, msh.boundary, gref, iref_patch_list);
  rhs(space.boundary.dofs) = rhs(space.boundary.dofs) + rhs_nmnn .* space.boundary.boundary_orientation.';
end

% Apply Dirichlet boundary conditions
u = zeros (space.ndof, 1);
[u_drchlt, drchlt_dofs] = sp_drchlt_l2_proj (space, msh, h, drchlt_sides);
u(drchlt_dofs) = u_drchlt;
int_dofs = setdiff (1:space.ndof, drchlt_dofs);

% Solve the linear system
rhs(int_dofs) = rhs(int_dofs) - stiff_mat(int_dofs, drchlt_dofs)*u_drchlt ...
                              - mass_mat(int_dofs, drchlt_dofs)*u_drchlt;

u(int_dofs) = (stiff_mat(int_dofs, int_dofs) + ...
                mass_mat(int_dofs, int_dofs)) \ rhs(int_dofs);

end

%!demo
%! ex_maxwell_src_Lshaped_mp


function order = lyness_order ( rule )

%*****************************************************************************80
%
%% LYNESS_ORDER returns the order of a Lyness quadrature rule.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    James Lyness, Dennis Jespersen,
%    Moderate Degree Symmetric Quadrature Rules for the Triangle,
%    Journal of the Institute of Mathematics and its Applications,
%    Volume 15, Number 1, February 1975, pages 19-32.
%
%  Parameters:
%
%    Input, integer RULE, the index of the rule.
%
%    Output, integer ORDER, the order of the rule.
%
  if ( rule == 0 )
    order = 1;
  elseif ( rule == 1 )
    order = 3;
  elseif ( rule == 2 )
    order = 4;
  elseif ( rule == 3 )
    order = 4;
  elseif ( rule == 4 )
    order = 7;
  elseif ( rule == 5 )
    order = 6;
  elseif ( rule == 6 )
    order = 10;
  elseif ( rule == 7 )
    order = 9;
  elseif ( rule == 8 )
    order = 7;
  elseif ( rule == 9 )
    order = 10;
  elseif ( rule == 10 )
    order = 12;
  elseif ( rule == 11 )
    order = 16;
  elseif ( rule == 12 )
    order = 13;
  elseif ( rule == 13 )
    order = 13;
  elseif ( rule == 14 )
    order = 16;
  elseif ( rule == 15 )
    order = 16;
  elseif ( rule == 16 )
    order = 21;
  elseif ( rule == 17 )
    order = 16;
  elseif ( rule == 18 )
    order = 19;
  elseif ( rule == 19 )
    order = 22;
  elseif ( rule == 20 )
    order = 27;
  elseif ( rule == 21 )
    order = 28;
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LYNESS_ORDER - Fatal error!\n' );
    fprintf ( 1, '  Unrecognized rule index.\n' );
    error ( 'LYNESS - Fatal error!' );
  end

  return
end


function op = linop_subsample( sz, omega, SYMMETRIC )
%LINOP_SUBSAMPLE Subsampling linear operator.
%OP = LINOP_SUBSAMPLE( SZ, OMEGA )
%   vector and matrix subsampling. Depending on SZ and OMEGA,
%   this can do row-sampling (e.g. a partial FFT)
%   or it can sample specific entries of a matrix (e.g. matrix completion)
%   To specify matrix subsampling, set sz = { [n1,n2], [length(omega),1] }
%       where the input matrix has size n1 x n2
%
%OP = LINOP_SUBSAMPLE( OMEGA )
%   works if OMEGA is a sparse matrix, whose nonzero entries
%   specify the entries to sample.
%
%OP = LINOP_SUBSAMPLE( ..., SYMMETRIC )
%   If SYMMETRIC is true, then
%   forces the domain to be the space of symmetric matrices
%
% Example with FFT:
%   FFT = linop_handles([N,N], @(x)fft(x)/sqrt(N), @(x)sqrt(N)*real(ifft(x)) ,'R2C');
%   and to make a row sub-sampled FFT (M <= N):
%   rows = randperm(M);
%   A   = linop_compose( linop_subsample([M,N], rows ), FFT );

% Designed to be used with a fft or dct
% Do we need a separate oversampling version?
% Also, make linop_DCT and linop_FFT ?
%   The reason we might want this is that for linop_FFT,
%   people will probably use idct as the transpose -- this is not
%   correct, due to scaling

% Help documentation: TBD
%    Constructs a TFOCS-compatible linear operator from separate function
%    handles that compute the forward and adjoint operations. The first
%    argument, SZ, gives the size of the linear operator; and the forward
%    and adjoint handles are AF and AT, respectively.
%
%    If the inputs and outputs are simple vectors, then SZ can take the
%    standard Matlab form [N,M], where N is the input length and M is the
%    output length. If the input or output is a matrix or array, then SZ
%    should take the form { S_in, S_out }, where S_in is the size of the
%    input and S_out is the size of the output.
%

error(nargchk(1,3,nargin));
if nargin < 3
    SYMMETRIC = false;
end
if nargin == 1
    omega = sz;
    sz = { size(omega), [nnz(omega),1] };
elseif nargin ==2 && issparse(sz)
    SYMMETRIC = omega;
    omega = sz;
    sz = { size(omega), [nnz(omega),1] };
end

if numel( sz ) ~= 2,
    error( 'Size must have two elements.' );
elseif isnumeric( sz ),
    sz = { [sz(2),1], [sz(1),1] };
elseif ~isa( sz, 'cell' ),
    error( 'Invalid operator size specification.' );
end

dom = sz{1};
n1 = dom(1); n2 = dom(2);
ran = sz{2};

% There are several possibilities.  Let x be a point in the domain
%{
    x is a vector.  Then omega should be a vector.
                    We return x(omega), resp.

    x is a matrix.
        omega is a vector
            This is ambiguous: does the user want x(omega,:) or x(omega)?
        omega is a matrix with 2 columns, then assume it is [I,J]
            We convert it to linear indices.
        omega is a general matrix, more than 2 columns
            Not sure what the user means; report an error.
        omega is a sparse matrix
            We find it's entries, and use those

%}

if n2 == 1
    % x is a vector, not a matrix.  Simple.
%     op = @(x,mode) linop_subsample_vector( sz, omega, x, mode );
    % Allow it to vectorize along rows:
    op = @(x,mode) linop_subsample_row( sz, omega, x, mode );
else
    % trickier case.
    if issparse(omega)
        ind = find(omega);
        [I,J] = ind2sub( sz{1}, ind );
%         op = @(x,mode) linop_subsample_matrix( sz, ind, I, J, SYMMETRIC, x, mode );
    elseif isvector(omega)
        ind     = omega;
        [I,J]   = ind2sub( sz{1}, ind );
%         op = @(x,mode) linop_subsample_matrix( sz, omega, SYMMETRIC,x, mode );
    elseif size(omega,2) == 2
        ind = sub2ind( sz{1}, omega(:,1), omega(:,2) );
        I   = omega(:,1);
        J   = omega(:,2);
%         op = @(x,mode) linop_subsample_matrix( sz, ind, SYMMETRIC,x, mode );
    else
        error('omega is not an acceptable size; perhaps you meant it to be sparse?');
    end
    % Make sure 'ind' is a column vector
    ind     = ind(:);
    op = @(x,mode) linop_subsample_matrix( sz, ind, I, J, SYMMETRIC, x, mode );

end

function y = linop_subsample_vector(sz, omega, x, mode )
switch mode,
    case 0, y = sz;
    case 1,
        if ~isequal( sz{1}, size(x) ), error('input wrong size for vector subsampling'); end
        y = x(omega);
    case 2,
        y = zeros( sz{1} );
        y(omega) = x;
end
function y = linop_subsample_row(sz, omega, x, mode )
switch mode,
    case 0, y = sz;
    case 1,
        if ~isequal( sz{1}(1), size(x,1) ), error('input wrong size for vector subsampling'); end
        y = x(omega,:);
    case 2,
        y = zeros( sz{1} );
        y = zeros( sz{1}(1), size(x,2) );
        y(omega,:) = x;
end
function y = linop_subsample_matrix(sz, omega, indI, indJ,SYMMETRIC, x, mode )
switch mode,
    case 0, y = sz;
    case 1
        S = [];
        S.type = '()';
        S.subs = {omega};
        y = subsref(x,S);
    case 2,
        dom = sz{1}; n1 = dom(1); n2 = dom(2);
        y = sparse( indI, indJ, x, n1, n2 );
        if SYMMETRIC
            % in future, might update this
            % e.g. force omega to only refer to lower part of matrx,
            % and then do the update y = y + tril(y,-1)'
            y = (y+y')/2;
        end
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function a = wishart_unit_sample ( m, df )

%*****************************************************************************80
%
%% WISHART_UNIT_SAMPLE samples the unit Wishart distribution.
%
%  Discussion:
%
%    This function requires functions from the PDFLIB and RNGLIB libraries.
%
%    The "initialize()" function from RNGLIB must be called before using
%    this function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 October 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Patrick Odell, Alan Feiveson,
%    A numerical procedure to generate a sample covariance matrix,
%    Journal of the American Statistical Association,
%    Volume 61, Number 313, March 1966, pages 199-203.
%
%    Stanley Sawyer,
%    Wishart Distributions and Inverse-Wishart Sampling,
%    Washington University,
%    30 April 2007, 12 pages.
%
%  Parameters:
%
%    Input, integer M, the order of the matrix.
%
%    Input, integer DF, the number of degrees of freedom.
%    M <= DF.
%
%    Output, real A(M,M), the sample matrix from the unit Wishart distribution.
%
  if ( df < m )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'WISHART_UNIT_SAMPLE - Fatal error!\n' );
    fprintf ( 1, '  DF = %d < M = %d.\n', df, m );
    error ( 'WISHART_UNIT_SAMPLE - Fatal error!\n' );
  end

  c = zeros ( m, m );

  for i = 1 : m
    df_chi = df - i + 1;
    c(i,i) = sqrt ( r8_chi_sample ( df_chi ) );
    for j = i + 1 : m
      c(i,j) = r8_normal_01_sample ( );
    end
  end

  a = c' * c;

  return
end


function jac = jacmodhs52(p)
  m=5;

  jac(1, 1:m)=[4.0, -1.0, 0.0, 0.0, 0.0];
  jac(2, 1:m)=[0.0, 1.0, 1.0, 0.0, 0.0];
  jac(3, 1:m)=[0.0, 0.0, 0.0, 1.0, 0.0];
  jac(4, 1:m)=[0.0, 0.0, 0.0, 0.0, 1.0];


function s = i4_to_s_zero ( intval, s_len )

%*****************************************************************************80
%
%% I4_TO_S_ZERO converts an I4 to a string, with zero padding.
%
%  Discussion:
%
%    An I4 is an integer.
%
%  Example:
%
%    Assume that S is 6 characters long:
%
%    INTVAL  S
%
%         1  000001
%        -1  -00001
%         0  000000
%      1952  001952
%    123456  123456
%   1234567  ******  <-- Not enough room%
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 December 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer INTVAL, an integer to be converted.
%
%    Input, integer S_LEN, the length of the string to be used.
%
%    Output, string S, the representation of the integer.
%    The integer will be right justified, and zero padded.
%    If there is not enough space, the string will be filled with stars.
%
  s = [];

  if ( s_len <= 0 )
    return
  end

  ilo = 1;
%
%  Make a copy of the integer.
%
  ival = intval;
%
%  Handle the negative sign.
%
  if ( ival < 0 )

    if ( s_len <= 1 )
      s(1) = '*';
      s = char ( s );
      return
    end

    ival = - ival;
    s(1) = '-';
    ilo = 2;

  end
%
%  Working from right to left, strip off the digits of the integer
%  and place them into S(ILO:S_LEN).
%
  ipos = s_len;

  while ( ival ~= 0 | ipos == s_len )

    idig = mod ( ival, 10 );
    ival = floor ( ival / 10 );

    if ( ipos < ilo )
      s(1:s_len) = '*';
      s = char ( s );
      return
    end

    c = digit_to_ch ( idig );

    s(ipos) = c;
    ipos = ipos - 1;

  end
%
%  Fill the empties with zeroes.
%
  s(ilo:ipos) = '0';
  s = char ( s );

  return
end


%function [varargout] = date_(varargin)
function     [dt_]  = ...
    date_(x, time, strfreq, t0, t1, options)
%    date_(x, time, freq, ndfy, ndfm, ndly, ndlm, turnphase, phase, cycle, thresh, nrep, complete)
% st,trinary,notentp,dura_,ampl_,cumm_,excc_,durcv_,amplcv_,exccv_
%***********************************************************************************************/
%*                                                                                */
%*  Modified BBQ program                                                                       */
%*                                                                                */
%*  Computes turning points and imposes restrictions in one step                               */
%*                                                                                */
%*    NB: rule is peak greater than turnphase on either side,                                  */
%*             trough less than turnphase on either side.           (can be modified)          */
%*    Restriction: Phase quarters/months, Cycle  quarters/months Alternating Troughs and Peaks,*/
%*                  if two peaks(troughs) in a row chooses highest(lowest), Trough must be     */
%*                  lower then preceeding peak, and No turning point within phase length       */
%*                  of end points                                                              */
%*                                                                                */
%*                                                                                */
%*                                                                                */
%*    Date: 15th November 2005                                                                  */
%*     Author: James Engel - Code modified from Adrian Pagan and Don Harding's BBQ code     */
%*
%*     further adjustments  made  by  F. Canova
%***********************************************************************************************/
%

ndfy = t0(1); % year start
ndfm = t0(2); % month/quarter start,
ndly = t1(1); % year start,
ndlm = t1(2); % month/quarter start,

if length(x) ~= length(time)
    error('data (x) and time must have the same length');
end

if strcmp(strfreq,'q') == 1
    freq =1;
    % default settings cycle parameters q data
    turnphase   = 2;
    phase       = 2;        % censoring rules
    cycle       = 5;        % lenght  of cycle

    if ndfm>4 || ndlm>4
        error('Something went wrong. Frequency is quarterly; therefore, starting and ending quarters cannot be larger than 4!');
    end

elseif strcmp(strfreq,'m') == 1
    freq =2;
    % default settings cycle parameters m data
    turnphase   = 6;
    phase       = 6;        % censoring rules
    cycle       = 15;        % lenght  of cycle

else
    error('strfreq must be ''q'' (quarterly) or ''m'' (monthly)');
end

notentp=0;

if freq==1 % q
    nd= 4*(ndly-1-ndfy) + (5-ndfm) + (ndlm);     % Number of data points %
    stepsize = 1/4;
elseif freq==2 % m
    nd= 12*(ndly-1-ndfy) + (13-ndfm) + (ndlm);
    stepsize = 1/12;
end

thresh      = 10.4;     % bypasses phase and cycle restriction if peak to trough is > than thresh
nrep        = 1;        % 1 if analyze real data
complete    = 1;        % if= 1- use complete cycles,if =0 -use incomplete cycles (excess still on complete cycle)

if nargin > 5
    if isfield(options,'turnphase')==1
        turnphase = options.turnphase;
    end
    % censoring rules
    if isfield(options,'phase')==1
        phase = options.phase;
    end
    % lenght of cycle
    if isfield(options,'cycle')==1
        cycle = options.cycle;
    end
    % bypasses phase and cycle restriction if peak to trough is > than thresh
    if isfield(options,'thresh')==1
        thresh = options.thresh;
    end
    % 1 if analyze real data
    if isfield(options,'nrep')==1
        nrep = options.nrep;
    end
    % if= 1- use complete cycles,if =0 -use incomplete cycles (excess still on complete cycle)
    if isfield(options,'complete')==1
        complete = options.complete;
    end

end

pdcv=0;tdcv=0;pacv=0;tacv=0;pdm=0;pdma=0;tdm=0;tdma=0;pdcm=0;tdcm=0;
pdem=0;tdem=0;tdema=0;pdema=0;epcv=0;etcv=0;

[bcp5, bct5, nbp, nbt] = rawall(x(1:nd),turnphase,nd,phase,cycle,thresh);   % calculates turning points with restrictions %

if nbp+nbt<=2
    notentp=notentp+1;
else

    %ntr=nbt;npk=nbp;

    nr  = [nbt;nbp];
    nv  = max(nr);

    %               pdc=zeros(nv,1);tdc=zeros(nv,1);
    %               pda=zeros(nv,1);tda=zeros(nv,1);td=zeros(nv,1);pd=zeros(nv,1);
    pdc = zeros(nv,1);
    tdc = zeros(nv,1); % pde=zeros(nv,1);tde=zeros(nv,1);
    %               tdea=zeros(nv,1);pdea=zeros(nv,1);

    % calculate peak to trough durations & amps
    % p st&s for peaks,t for troughs,p gives cntractions,t expansions
    % code is pd,td is durations; pda,tda is amps; pdc,tdc is cum move pde, tde exces
    % excess is measured differently to avoid case that amps is close to zero in part cycle so that denom can become neg.so use cum movements as denom
    % now vs triangle in early paper

    if bcp5(1,1) < bct5(1,1)      % Peaks are first

        nr  = [nbt;nbp];
        r   = nbt;
        pd  = bct5(1:r,1)-bcp5(1:r,1);
        pda = x(bct5(1:r,1))-x(bcp5(1:r,1));
        k   = 1;

        while k<=r
            %pdc(k)=sumc(x(bcp5(k,1):bct5(k,1),1)-x(bcp5(k,1),1));
            pdc(k)=sum(x(bcp5(k,1):bct5(k,1),1)-x(bcp5(k,1),1));
            k=k+1;
        end
    else                      % troughs are First

        r   = nbt-1;
        pd  = bct5(2:r+1,1)-bcp5(1:r,1);
        pda = x(bct5(2:r+1,1))-x(bcp5(1:r,1));

        k=1;
        while k<=r
            %pdc(k)=sumc(x(bcp5(k,1):bct5(k+1,1),1)-x(bcp5(k,1),1));
            pdc(k)=sum(x(bcp5(k,1):bct5(k+1,1),1)-x(bcp5(k,1),1));
            k=k+1;
        end

        %r1 = r;

    end

    % calculate trough to peak durations & amplitudes

    if bct5(1,1) < bcp5(1,1)        %  Troughs are first
        r   = nbp;
        td  = bcp5(1:r,1)-bct5(1:r,1);
        tda = x(bcp5(1:r,1))-x(bct5(1:r,1));
        k   = 1;
        while k<=r
            %tdc(k)=sumc(x(bct5(k,1):bcp5(k,1),1)-x(bct5(k,1),1));
            tdc(k)=sum(x(bct5(k,1):bcp5(k,1),1)-x(bct5(k,1),1));
            k=k+1;
        end

    else                      % peaks are First
        r   = nbp-1;
        td  = bcp5(2:r+1,1)-bct5(1:r,1);
        tda = x(bcp5(2:r+1,1))-x(bct5(1:r,1));

        k=1;
        while k<=r

            %tdc(k)=sumc(x(bct5(k,1):bcp5(k+1,1),1)-x(bct5(k,1),1));
            tdc(k)=sum(x(bct5(k,1):bcp5(k+1,1),1)-x(bct5(k,1),1));
            k=k+1;
        end

    end
    pdc=pdc(1:size(pd,1));
    tdc=tdc(1:size(td,1));

    % compute excesses
    %excess is percentage of triangle area
    za      = (pd.*pda)/2;
    pde     = 100*(pdc-za-.5*pda)./za;

    pdea    = 100*(pdc-((pd.*pda)/2))./za;
    za      = (td.*tda)/2;
    tde     = 100*(tdc-za-.5*tda)./za;

    tdea    = 100*(tdc-((td.*tda)/2))./za;

    %***********************************************************************************************/

    if complete == 0    % switch: 1-only use complete cycles,0-use incomplete cycles (excess still on complete cycle)%

        bct5u   = bct5;
        bcp5u   = bcp5;

        if bcp5(1,1) < bct5(1,1)     % modifies code to include incomplete cycles %

            bct5u  = [1;bct5];

        elseif 	bct5(1,1) < bcp5(1,1)

            bcp5u = [1;bcp5];

        end

        nbtu = size(bct5u,1);
        nbpu = size(bcp5u,1);

        if bcp5u(nbpu,1) < bct5u(nbtu,1)    % modifies code to include incomplete cycles %

            bcp5u  = [bcp5u;nd];

        elseif 	bct5u(nbt,1) < bcp5u(nbp,1)

            bct5u = [bct5u;nd];

        end

        ntr     = size(bct5u,1);
        npk     = size(bcp5u,1);
        % nr      = [ntr;npk];
        % nv=max(nr);

        %        pdc=zeros(nv,1);tdc=zeros(nv,1);
        %        pda=zeros(nv,1);tda=zeros(nv,1);td=zeros(nv,1);pd=zeros(nv,1);
        %        pdc=zeros(nv,1);tdc=zeros(nv,1);

        if bcp5u(1,1) < bct5u(1,1)       % Peaks are first

            %nr  = [ntr;npk];
            r   = ntr;
            pd  = bct5u(1:r,1)-bcp5u(1:r,1);
            pda = x(bct5u(1:r,1))-x(bcp5u(1:r,1));
            k   = 1;
            while k<=r
                %pdc(k)=sumc(x(bcp5u(k,1):bct5u(k,1),1)-x(bcp5u(k,1),1));
                pdc(k)=sum(x(bcp5u(k,1):bct5u(k,1),1)-x(bcp5u(k,1),1));
                k=k+1;
            end
        else                      % troughs are First
            r   = ntr-1;
            pd  = bct5u(2:r+1,1)-bcp5u(1:r,1);
            pda = x(bct5u(2:r+1,1))-x(bcp5u(1:r,1));
            k   = 1;
            while k<=r
                pdc(k)=sum(x(bcp5u(k,1):bct5u(k+1,1),1)-x(bcp5u(k,1),1));
                %pdc(k)=sumc(x(bcp5u(k,1):bct5u(k+1,1),1)-x(bcp5u(k,1),1));
                k=k+1;
            end

%             r1=r;

        end

        % calculate trough to peak durations & amplitudes

        if bct5u(1,1) < bcp5u(1,1)       %  Troughs are first

            r   = npk;
            td  = bcp5u(1:r,1)-bct5u(1:r,1);
            tda = x(bcp5u(1:r,1))-x(bct5u(1:r,1));
            k   = 1;
            while k<=r
                %tdc(k)=sumc(x(bct5u(k,1):bcp5u(k,1),1)-x(bct5u(k,1),1));
                tdc(k)=sum(x(bct5u(k,1):bcp5u(k,1),1)-x(bct5u(k,1),1));
                k=k+1;
            end

        else                     % peaks are First
            r   = npk-1;
            td  = bcp5u(2:r+1,1)-bct5u(1:r,1);
            tda = x(bcp5u(2:r+1,1))-x(bct5u(1:r,1));

            k=1;
            while k<=r
                %tdc(k)=sumc(x(bct5u(k,1):bcp5u(k+1,1),1)-x(bct5u(k,1),1));
                tdc(k)=sum(x(bct5u(k,1):bcp5u(k+1,1),1)-x(bct5u(k,1),1));
                k=k+1;
            end

        end
        pdc=pdc(1:size(pd,1));
        tdc=tdc(1:size(td,1));

    end

    %***********************************************************************************************/

    % cumulate.when nrep=1 then gives raw data,otherwsise sums over monte carlo amounts

    pdcv    = pdcv + stdc(pd)/meanc(pd);       % compute cv's
    tdcv    = tdcv + stdc(td)/meanc(td);
    pacv    = pacv + stdc(pda)/meanc(pda);
    tacv    = tacv + stdc(tda)/meanc(tda);
    epcv    = epcv + stdc(pde)/meanc(pde);  %  cv of xss
    etcv    = etcv + stdc(tde)/meanc(tde);

    pdm     = pdm+meanc(pd);          % durations
    pdma    = pdma+meanc(pda);

    tdm     = tdm+meanc(td);          % durations
    tdma    = tdma+meanc(tda);

    pdcm    = pdcm+meanc(pdc);       % cumulative
    tdcm    = tdcm+meanc(tdc);

    % pdem=pdem+meanc(pde);
    tdem    = tdem+meanc(tde);       % xss
    tdema   = tdema+meanc(tdea);
    pdem    = pdem+meanc(pde);
    pdema   = pdema+meanc(pdea);

end

if nrep==1&&(nbp+nbt>2)

    %nbp and nbt=number of peaks and number of troughs %

    disp('peaks at')
    fprintf('%.2f\n', time(bcp5))
    %disp(bcp5(1:nbp))

    disp('troughs at')
    fprintf('%.2f\n', time(bct5))
    %disp(bct5(1:nbt))

    % total  number of  turning  points
    ttp=length(bcp5)+length(bct5);

    % collecting  turning  points in  trinary  indicator
    trinary=zeros(length(x),1);
    trinary(bcp5)=1;
    trinary(bct5)=-1;

end

if nrep==1&&(nbp+nbt>2)
    %remove below if want states ...
    %st=zeros(nd,1);

    [st] = states(bcp5,bct5,nbp,nbt,nd);
    dt_.st(:,nrep) = st;

    %determine obs in which states have been completed%...
    na  = min([bct5(1);bcp5(1)])';
    nb  = max([bct5(nbt);bcp5(nbp)])';
    %nb-na+1;
    z   = [x(na:nb) st(na:nb)];

    %disp('dated series')
    %z1= [time(time_start+1:time_end)' x st];
    %format  short  g
    %round(z1, 2)
    %%fprintf('%.2f\n', '%.2f\n', '%.2f\n', z1)

end
nrep1   = nrep-notentp;

% Compute final statistics
% duration contractions/duration expansions
dt_.dura = [pdm/nrep1 tdm/nrep1];
% amplitudes contractions/amplitude expansions
dt_.ampl  = 100*[pdma/nrep1 tdma/nrep1];
% cumulative contractions/cumulative expansions
dt_.cumm  = 100*[pdcm/nrep1 tdcm/nrep1];
% excess movements percent of triangle area
% contractions/expansions
dt_.excc  = [pdem/nrep1 tdem/nrep1];
% cv of durations contractions/expansions
dt_.durcv = [pdcv/nrep1 tdcv/nrep1];
% cv of amplitude contractions/expansions
dt_.amplcv = [pacv/nrep1 tacv/nrep1];
% cv of excess movements contractions/expansions
dt_.exccv = [epcv/nrep1 etcv/nrep1];
%
dt_.trinary = trinary;
%
dt_.notentp = notentp;

function m=meanc(x)
% get mean of each column
m=mean(x);
if size(m,2)>1
   m=m';
end

function m=stdc(x)
% get standard deviation of each column
m=std(x);
if size(m,2)>1
   m=m';
end


function [jointTorques_star, LDot_tilde, L_tilde, intL_tilde, xiDes, f_des, xiDot_star] = momentumJerkControlYoga(intLTilde_angular, nu, J_CoM, JL, JR, xCoM, w_H_l_sole, w_H_r_sole, L, M, h, JDotL_nu, JDotR_nu, x_dx_ddx_dddx_CoM_des, ...
                                                                                gainsPCOM, gainsDCOM, LEFT_RIGHT_FOOT_IN_CONTACT, qj, qj_dqj_ddqj_des, impedances, dampings, ...
                                                                                contactForces_est, xi_star, xi, contactForces_star, Config, Gain, Reg)

    % MOMENTUMJERKCONTROL implements a momentum-based jerk controller.
    %
    % REFERENCES: IEEE-RAL 2018, "Momentum Control of an Underactuated Flying Humanoid Robot";
    %

    %% ------------Initialization----------------

    % parameters
    ndof            = size(M(7:end,7:end),1);

    % ROBUSTNESS w.r.t. modeling errors (inertial parameters)
    if Config.testRobustness

        M           = M * Config.robustnessFactor;
        h           = h * Config.robustnessFactor;
    end

    % get the robot total mass, and gravity forces, and feet positions
    m               = M(1,1);
    f_grav          = m*Config.GRAVITY_ACC*[0; 0; 1; zeros(3,1)];
    posLeftFoot     = w_H_l_sole(1:3,4);
    posRightFoot    = w_H_r_sole(1:3,4);

    % compute momentum references
    [LDDot_des, LDot_des, L_des, intL_des] = computeMomentumReferences(x_dx_ddx_dddx_CoM_des, m);

    %% %%%%%%%%%%%%%%% COMPUTE THE MOMENTUM ACCELERATION %%%%%%%%%%%%%%% %%
    %
    % WHILE BALANCING: the momentum acceleration is defined as follows:
    %
    %   LDDot = Ac * Beta* xiDot + AcDot * contactForces. (1)
    %
    % The contact forces derivative have been parametrized as follows:
    %
    %   contactForcesDot = Beta * xiDot
    %
    % xiDot is then used as control input. Beta is an invertible matrix.
    % It is therefore necessary to compute AcDot and Ac.

    % compute matrix Ac for both feet
    r_left          = posLeftFoot  - xCoM;
    r_right         = posRightFoot - xCoM;

    Ac_leftFoot     = [eye(3),           zeros(3);
                       wbc.skew(r_left), eye(3)];

    Ac_rightFoot    = [eye(3),            zeros(3);
                       wbc.skew(r_right), eye(3)];

    % compute matrix Beta_left and Beta_right
    Beta_left       = computeParametrizationGradient(xi(1:6),   Config);
    Beta_right      = computeParametrizationGradient(xi(7:end), Config);

    % multiplier of contact forces derivatives
    Ac              = [Ac_leftFoot .* LEFT_RIGHT_FOOT_IN_CONTACT(1), ...
                       Ac_rightFoot .* LEFT_RIGHT_FOOT_IN_CONTACT(2)];
    Ac_Beta         = [Ac_leftFoot * Beta_left .* LEFT_RIGHT_FOOT_IN_CONTACT(1), ...
                       Ac_rightFoot * Beta_right .* LEFT_RIGHT_FOOT_IN_CONTACT(2)];

    % compute the time derivative of r_left and r_right
    nu_leftFoot     = JL * nu;
    nu_rightFoot    = JR * nu;
    v_CoM           = J_CoM(1:3,:) * nu;
    rDot_left       = nu_leftFoot(1:3)  - v_CoM;
    rDot_right      = nu_rightFoot(1:3) - v_CoM;

    % compute AcDot
    AcDot_leftFoot  = [zeros(3),             zeros(3);
                       wbc.skew(rDot_left),  zeros(3)];

    AcDot_rightFoot = [zeros(3),             zeros(3);
                       wbc.skew(rDot_right), zeros(3)];

    AcDot           = [AcDot_leftFoot .* LEFT_RIGHT_FOOT_IN_CONTACT(1), ...
                       AcDot_rightFoot .* LEFT_RIGHT_FOOT_IN_CONTACT(2)];

    %% %%%%%%%%%%%%%%% MOMENTUM CONTROLLER DEFINITION  %%%%%%%%%%%%%%%%% %%
    %
    % The control input of Eq. (1) is:
    %
    %  u1 = xiDot
    %
    % For the purpose of this documentation, it suffices to know that the
    % robot momentum converges to the desired values if the following
    % equation is verified:
    %
    %  ATilde * u1 + deltaTilde = 0 (2)
    %
    % where: ATilde and deltaTilde must be properly calculated.
    % For details on the controller implementation, see the RAL 2018 paper
    % at: https://ieeexplore.ieee.org/document/7997895.

    % Compute the momentum error derivative/integral
    LDot_estimated  = Ac * [contactForces_est(1:6) .* LEFT_RIGHT_FOOT_IN_CONTACT(1); ...
                            contactForces_est(7:end) .* LEFT_RIGHT_FOOT_IN_CONTACT(2)] - f_grav;
    LDot_tilde      = LDot_estimated - LDot_des;
    L_tilde         = L - L_des;
    intL_tilde      = [(m * xCoM - intL_des(1:3)); intLTilde_angular];

    % Get the gains matrices
    KP_momentum     = blkdiag(diag(gainsPCOM), Gain.KP_AngularMomentum .*eye(3));
    KD_momentum     = blkdiag(diag(gainsDCOM), Gain.KD_AngularMomentum .*eye(3));

    % CONTROL LAW: momentum-based control with Lyapunov stability (IEEE-RAL 2018)
    KTilde          = KP_momentum + inv(Gain.KO_momentum) + KD_momentum;
    deltaTilde      = KTilde * L_tilde + (KD_momentum + eye(6)) * LDot_tilde + ...
                      KP_momentum * intL_tilde - LDDot_des + AcDot * contactForces_est;

    % Primary task control input
    pinvAc_Beta       = [wbc.pinvDamped(Ac_leftFoot * Beta_left,1e-5) .* (1-LEFT_RIGHT_FOOT_IN_CONTACT(2)); zeros(6)] + ...
                        [zeros(6); wbc.pinvDamped(Ac_rightFoot * Beta_right,1e-5) .* (1-LEFT_RIGHT_FOOT_IN_CONTACT(1))] + ...
                         wbc.pinvDamped(Ac_Beta,1e-5).* LEFT_RIGHT_FOOT_IN_CONTACT(1).* LEFT_RIGHT_FOOT_IN_CONTACT(2);

    xiDot_primaryTask = -pinvAc_Beta * deltaTilde;

    % Null space projector (forces)
    Null_Ac_Beta      = (eye(12) - pinvAc_Beta*Ac_Beta)*LEFT_RIGHT_FOOT_IN_CONTACT(1)*LEFT_RIGHT_FOOT_IN_CONTACT(2);

    %% %%%%%%%%%%%%%%%%% TORQUE CONTROLLER DEFINITION  %%%%%%%%%%%%%%%%% %%

    % Get the required matrices
    Jc                = [JL .*LEFT_RIGHT_FOOT_IN_CONTACT(1); JR .*LEFT_RIGHT_FOOT_IN_CONTACT(2)];
    JcDot_nu          = [JDotL_nu .*LEFT_RIGHT_FOOT_IN_CONTACT(1); JDotR_nu .*LEFT_RIGHT_FOOT_IN_CONTACT(2)];
    B                 = [zeros(6,ndof); eye(ndof)];
    Lambda            = Jc/M*B;
    pinvLambda        = wbc.pinvDamped(Lambda, Reg.pinvDamp);
    NLambda           = eye(ndof) - pinvLambda*Lambda;

    % terms required for calculating u_0
    qjDot             = nu(7:end);
    KP_torqueControl  = diag(impedances);
    KD_torqueControl  = diag(dampings);
    jointPos_err      = qj    - qj_dqj_ddqj_des(:,1);
    jointVel_err      = qjDot - qj_dqj_ddqj_des(:,2);

    % consitency with the current gain tuning
    USE_MBAR          = false;
    USE_ACC           = 0;

    if USE_MBAR

        MBar          = (M(7:end,7:end) - M(7:end,1:6)/M(1:6,1:6)*M(1:6,7:end)); %#ok<UNRCH>
    else
        MBar          = eye(ndof);
    end

    u_0               = MBar*(USE_ACC*qj_dqj_ddqj_des(:,3) - KD_torqueControl * jointVel_err - KP_torqueControl * jointPos_err);

    % terms required for calculating tau_0
    tau_0             = h(7:end) -M(7:end,1:6)/M(1:6,1:6)*h(1:6) ...
                       -transpose(Jc(:,7:end))*contactForces_est + ...
                        M(7:end,1:6)/M(1:6,1:6)*transpose(Jc(:,1:6))*contactForces_est + u_0;

    jointTorques_star = pinvLambda*(Jc/M*(h-Jc'*contactForces_star) -JcDot_nu) + NLambda*tau_0;

    %% %%%%%%%%%%%%%%%%%%%%% NULL SPACE OF MOMENTUM %%%%%%%%%%%%%%%%%%%% %%

    % Now, rewrite the joint torques equations as:
    %
    %    tau = Sigma * f_star + gamma_full
    %
    % where we isolated the terms multiplied by the forces in the
    % joint torques equations. Finally we compute the time derivative of tau:
    %
    %    tauDot = SigmaDot * f_star + Sigma * fDot_star + gamma_fullDot
    %    tauDot ~ Sigma * fDot_star (approximated) + gamma_redDot
    %
    % Then, consider the following Lyapunov function candidate:
    %
    %    V    = 1/2*|tau|^2
    %    VDot = tau^T*tauDot
    %
    % It is clear that choosing tauDot = -K_tau*tau will ensures the
    % convergence of the joint torques to zero.
    %

    % theoretically, the forces inside tau_0 are the measured ones, not the
    % desired. Therefore only the derivative of the desired forces should
    % be used to minimize the joint torques, however I am not really sure
    % about this.
    DEACTIVATE_NULL_SPACE_FORCES = 0;

    Sigma               = -pinvLambda*(Jc/M*Jc') -DEACTIVATE_NULL_SPACE_FORCES.*NLambda*(transpose(Jc(:,7:end)) - M(7:end,1:6)/M(1:6,1:6)*transpose(Jc(:,1:6)));
    Sigma_Beta          =  Sigma * Null_Ac_Beta;
    gamma_redDot        =  Sigma * xiDot_primaryTask -Gain.useVelFeedbackForMinTorques*NLambda*MBar*KP_torqueControl*jointVel_err;
    K_tau               =  Gain.K_tau;

    % Secondary task
    pinvSigma_Beta      =  wbc.pinvDamped(Sigma_Beta, Reg.pinvDamp);
    xiDot_secondaryTask = -pinvSigma_Beta*(K_tau*jointTorques_star + gamma_redDot);

    % FINAL control input (Parametrized)
    pinvAc              = [wbc.pinvDamped(Ac_leftFoot,1e-5) .* (1-LEFT_RIGHT_FOOT_IN_CONTACT(2)); zeros(6)] + ...
                          [zeros(6); wbc.pinvDamped(Ac_rightFoot,1e-5) .* (1-LEFT_RIGHT_FOOT_IN_CONTACT(1))] + ...
                           wbc.pinvDamped(Ac,1e-5).* LEFT_RIGHT_FOOT_IN_CONTACT(1).* LEFT_RIGHT_FOOT_IN_CONTACT(2);

    f_des               = pinvAc*(LDot_des + f_grav);

    if LEFT_RIGHT_FOOT_IN_CONTACT(1) > 0.5

        xi_des_left = fromForcesToParametrization(f_des(1:6),Config);
    else
        xi_des_left = zeros(6,1);
    end
    if LEFT_RIGHT_FOOT_IN_CONTACT(2) > 0.5

        xi_des_right = fromForcesToParametrization(f_des(7:end),Config);
    else
        xi_des_right = zeros(6,1);
    end

    xiDes               = [xi_des_left;xi_des_right];
    Ke                  = Gain.Ke;
    xiDot_star          = xiDot_primaryTask + Null_Ac_Beta*xiDot_secondaryTask -Ke*(xi_star-[xiDes(1:6)*LEFT_RIGHT_FOOT_IN_CONTACT(1);xiDes(7:end)*LEFT_RIGHT_FOOT_IN_CONTACT(2)]);
end


% Copyright (c) 2015, Francesco Perrone
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in
%       the documentation and/or other materials provided with the distribution
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.

function [WF,WFtower,t,dy,dz,Y,Z,Zbottom,Ztower] = turbulent_wind_field_generator(U0,I0,Seed,HubHt,Ny,Nz,Ly,Lz,dt,T,xLu,xLv,xLw,Lc,a,shearExp)

%% FREQUENY/TIME PREALLOCATION & INITIALISATION
T = round(T + (Ly/U0));
Nt = Ny*Nz; % Total number of grid points
N = find_steps_number(T,dt); % Number of samples (in frequency and time domain)
Fs = N/T;
df = Fs/N; % Frequency sample
fp = df:df:0.5*Fs; % Positive frequency array [Hz]
dT = T/N; % Wind file time step

%% DEFINITION OF SIMULATION GRID

[Y,Z,dy,dz] = define_grid_coordinates(Ly,Lz,Ny,Nz,HubHt); % Generate (Y,Z) coordinates of grid points and spacing on y-/z-axis
Zunique = unique(Z);
Zbottom = min(Zunique(:));
Ztower = (Zbottom):-dz:0;
% Ytower = zeros(size(Ztower));
Ntower = numel(Ztower);
Uvert = repmat(U0.*((Zunique/HubHt).^shearExp),1,Ny)'; % Exponential vertical wind profile
Uvert_tower = U0.*((Ztower/HubHt).^shearExp);
Uvert1 = Uvert(:);
Distance = abs(sqrt(bsxfun(@plus,sum([Y(:) Z(:)].^2,2),sum([Y(:) Z(:)].^2,2)') - 2*([Y(:) Z(:)]*[Y(:) Z(:)]'))); % Distance matrix

%% POWER SPECTRA

r = 5/3; % Kaimal spectrum exponent
sigma_u = (I0*0.01*U0);
sigma_v = 0.8*sigma_u;
sigma_w = 0.5*sigma_u;
var_u = sigma_u^2;
var_v = sigma_v^2;
var_w = sigma_w^2;

Suu_p = ((4*var_u*(xLu/U0))./((1 + 6*fp*(xLu/U0)).^r))*0.5; % Positive amplitude spectrum (U-component)
Svv_p = ((4*var_v*(xLv/U0))./((1 + 6*fp*(xLv/U0)).^r))*0.5; % Positive amplitude spectrum (V-component)
Sww_p = ((4*var_w*(xLw/U0))./((1 + 6*fp*(xLw/U0)).^r))*0.5; % Positive amplitude spectrum (W-component)

Su = sqrt(Suu_p*N*Fs);
Sv = sqrt(Svv_p*N*Fs);
Sw = sqrt(Sww_p*N*Fs);

%% CALCULATION OF FFT TERMS

rng(Seed,'twister'); % Start Mersenne twister

fft_uu_p = zeros(Nt,N*0.5);
fft_vv_p = zeros(Nt,N*0.5);
fft_ww_p = zeros(Nt,N*0.5);

nn_u = exp(1i*2*pi*rand(Nt,numel(fp)));
nn_v = exp(1i*2*pi*rand(Nt,numel(fp)));
nn_w = exp(1i*2*pi*rand(Nt,numel(fp)));

for idx_chol = 1:numel(fp)
    Coh_uu = chol(exp(-a.*Distance.*sqrt(((fp(idx_chol)./U0).^2) + ((0.12./Lc).^2))),'lower'); %Coherence function U-component (according to IEC 61400-1 Ed.3)
    fft_uu_p(:,idx_chol) = (Coh_uu*Su(idx_chol))*nn_u(:,idx_chol);%chol(Coh_uu,'lower')
    fft_vv_p(:,idx_chol) = (Coh_uu*Sv(idx_chol))*nn_v(:,idx_chol);%chol(Coh_vv_ww,'lower')
    fft_ww_p(:,idx_chol) = (Coh_uu*Sw(idx_chol))*nn_w(:,idx_chol);%chol(Coh_vv_ww,'lower')
end

% Prepare FFT terms for each time series by mirroring the positive side
% about the frequency axis
fft_uu = [zeros(Nt,1) fft_uu_p fliplr(conj(fft_uu_p(:,1:end-1)))];
fft_vv = [zeros(Nt,1) fft_vv_p fliplr(conj(fft_vv_p(:,1:end-1)))];
fft_ww = [zeros(Nt,1) fft_ww_p fliplr(conj(fft_ww_p(:,1:end-1)))];

fft_uu(:,N*0.5 + 1) = real(fft_uu(:,N*0.5 + 1));
fft_vv(:,N*0.5 + 1) = real(fft_vv(:,N*0.5 + 1));
fft_ww(:,N*0.5 + 1) = real(fft_ww(:,N*0.5 + 1));

%% GENERATE TIME SERIES

Ucomp = (ifft((fft_uu),[],2));
Vcomp = (ifft((fft_vv),[],2));
Wcomp = (ifft((fft_ww),[],2));

%% SCALE TIME SERIES

stdU = (std(Ucomp,[],2));
stdV = (std(Vcomp,[],2));
stdW = (std(Wcomp,[],2));

idx_hub = ((Nz+1)*Ny)*0.5 - (Ny-1)*0.5;

SF = [sigma_u./stdU(idx_hub) , sigma_v./stdV(idx_hub) , sigma_w./stdW(idx_hub)];

Ucomp1 = arrayfun(@(i) Ucomp(i,:)*SF(1),1:Nt,'UniformOutput',false);
Ucomp1 = vertcat(Ucomp1{:});
Vcomp1 = arrayfun(@(i) Vcomp(i,:)*SF(2),1:Nt,'UniformOutput',false);
Vcomp1 = vertcat(Vcomp1{:});
Wcomp1 = arrayfun(@(i) Wcomp(i,:)*SF(3),1:Nt,'UniformOutput',false);
Wcomp1 = vertcat(Wcomp1{:});

t = 0:dT:(N-1)*dT;

WF = zeros(N,Nz,Ny,3);
WFtower = zeros(N,numel(Ztower),3);
it = 0;
for iz = 1:Nz
    for iy = 1:Ny
        it = it + 1;
        WF(:,iz,iy,1) = Ucomp1(it,:) + Uvert1(it);
        WF(:,iz,iy,2) = Vcomp1(it,:);
        WF(:,iz,iy,3) = Wcomp1(it,:);
    end
end

for iz = 1:Ntower
        WFtower(:,iz,1) = (WF(:,1,.5*(Ny+1),1) - mean(WF(:,1,.5*(Ny+1),1))) + Uvert_tower(iz);
        WFtower(:,iz,2) = WF(:,1,.5*(Ny+1),2);
        WFtower(:,iz,3) = WF(:,1,.5*(Ny+1),3);
end

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_tx200l(robot, T)
%   Solves the inverse kinematic problem for the STAUBLI tx200l robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC_tx200l returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   robot=load_robot('staubli', 'tx200l');
%   q = [0 0 0 0 0 0];
%   T = directkinematic(robot, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(robot, T);
%   check that all of them are feasible solutions!
%   and every Ti equals T
%   for i=1:8,
%        Ti = directkinematic(robot, qinv(:,i))
%   end
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Carlos Pardo Pla
%
% 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 q = inversekinematic_tx200l(robot, T)

%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry at the reference for this robot
L6=abs(d(6));

A1 = a(1);

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
W = T(1:3,3);

% Pm: wrist position
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
q1=atan2(Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);

q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);

q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%Arrange solutions, there are 8 possible solutions so far.
% if q1 is a solution, q1* = q1 + pi is also a solution.
% For each (q1, q1*) there are two possible solutions
% for q2 and q3 (namely, elbow up and elbow up solutions)
% So far, we have 4 possible solutions. Howefer, for each triplet (theta1, theta2, theta3),
% there exist two more possible solutions for the last three joints, generally
% called wrist up and wrist down solutions. For this reason,
%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%leave only the real part of the solutions
q=real(q);

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));

% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    qtemp = solve_spherical_wrist_tx200l(robot, q(:,i), T, 1); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solve_spherical_wrist_tx200l(robot, q(:,i), T, -1); %wrist down
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%See geometry
L2=abs(a(2));
L3=abs(d(4));

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(-p1(2), p1(1));
gamma =(acos((L2^2+r^2-L3^2)/(2*r*L2)));

if ~isreal(gamma)
    disp('WARNING:inversekinematic_tx200l: the point is not reachable for this configuration, imaginary solutions');
    %gamma = real(gamma);
end

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3
q2(1) = pi/2 - beta - gamma; %elbow up
q2(2) = pi/2 - beta + gamma; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%See geometry
L2=abs(a(2));
L3=abs(d(4));

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

eta =(acos((L2^2 + L3^2 - r^2)/(2*L2*L3)));

if ~isreal(eta)
   disp('WARNING:inversekinematic_tx200l: the point is not reachable for this configuration, imaginary solutions');
   %eta = real(eta);
end

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = pi - eta;
q3(2) = eta - pi;

% Solve the special case of this spherical wrist
% For wrists that whose reference systems have been placed as in the
% ABB IRB 140--> use solve_spherical_wrist2
% For wrists with the same orientation as in the KUKA KR30_jet
%--> use solve_spherical_wrist
function q = solve_spherical_wrist_tx200l(robot, q, T, wrist)

% T is the noa matrix defining the position/orientation of the end
% effector's reference system
vx6=T(1:3,1);
vz5=T(1:3,3); % The vector a z6=T(1:3,3) is coincident with z5

% Obtain the position and orientation of the system 3
% using the already computed joints q1, q2 and q3
T01=dh(robot, q, 1);
T12=dh(robot, q, 2);
T23=dh(robot, q, 3);
T03=T01*T12*T23;

vx3=T03(1:3,1);
vy3=T03(1:3,2);
vz3=T03(1:3,3);

% find z4 normal to the plane formed by z3 and a
vz4=cross(vz3, vz5);	% end effector's vector a: T(1:3,3)

% in case of degenerate solution,
% when vz3 and vz6 are parallel--> then z4=0 0 0, choose q(4)=0 as solution
if norm(vz4) <= 0.00000001
    if wrist == 1 %wrist up
        q(4)=0;
    else
        q(4)=-pi; %wrist down
    end
else
    %this is the normal and most frequent solution
    cosq4=wrist*dot(vy3,vz4);
    sinq4=wrist*dot(-vx3,vz4);
    q(4)=atan2(sinq4, cosq4);
end
%propagate the value of q(4) to compute the system 4
T34=dh(robot, q, 4);
T04=T03*T34;
vx4=T04(1:3,1);
vy4=T04(1:3,2);

% solve for q5
cosq5=dot(-vy4,vz5);
sinq5=dot(vx4,vz5);
q(5)=atan2(sinq5, cosq5);

%propagate now q(5) to compute T05
T45=dh(robot, q, 5);
T05=T04*T45;
vx5=T05(1:3,1);
vy5=T05(1:3,2);

% solve for q6
cosq6=dot(vx6,vx5);
sinq6=dot(vx6,vy5);
q(6)=atan2(sinq6, cosq6);


 function centers = lloyd_max_hist(data, centers, MM, tol, max_iter, chat)
%function centers = lloyd_max_hist(data, centers, MM, tol, max_iter, chat)
%|
%| "improved" version of the lloyd-max algorithm for scalar quantizer design
%| that saves computation by histogramming the data first.
%| Before using this, try using highrate_centers() first!
%|
%| in
%|	data	[N 1]	training data
%|	centers	[K 1]	initial guess of centroids (codebook)
%|	MM		# of histogram bins: K < M < N
%| out
%|	centers [K 1]	final centroids
%|
%| Copyright 2004-7-1, Jeff Fessler, University of Michigan

if nargin == 1 && streq(data, 'test'), lloyd_max_hist_test, return, end
if nargin < 2, ir_usage, end
if nargin < 3, MM = 0; end
if nargin < 4, tol = 1e-3; end
if nargin < 5, max_iter = 40; end
if nargin < 6, chat = 0; end

data = data(:);
Norig = length(data);
if MM
	if ~isreal(data)
		if length(MM) == 1
			MM = [MM MM];
		end
		[wt data] = hist_equal([real(data) imag(data)], MM);
		[dr di] = ndgrid(data(:,1), data(:,2));
		data = dr + 1i * di;
	else
		[wt data] = hist(data, MM);
	end
	data = data(:);
	wt = wt(:);
	% eliminate data bins with 0 wt
	data = data(wt ~= 0);
	wt = wt(wt ~= 0);
else
	wt = ones(size(data));
end

K = length(centers);
if length(unique(centers)) ~= K
	error 'initial centers are not unique'
end
redundant = 0;
if K > length(data)
	if K > Norig
		warning(sprintf('#centroids %d > #data %d!?', K, Norig))
	else
		printf('Warn: #centroids %d > #unique(data) %d.', K, length(data))
	end
	redundant = 1;
end

tol = tol * max(abs(data));
iter = 1;
change = inf;
while iter <= max_iter && change > tol
	if ~redundant && (length(unique(centers)) ~= K)
		warning 'centers became not unique'
	end
	index = quant1_index(data, centers);
	old = centers;
	for kk=1:K
		ik = index == kk;
		if sum(ik)
			if ~sum(wt(ik))
				warning 'bug?'
				keyboard
			end
			centers(kk) = sum(data(ik) .* wt(ik)) ./ sum(wt(ik));
		else
			centers(kk) = NaN;
		end
	end

	% assign any unused centers to the data point(s) furthest from centroids
	if redundant
		centers(isnan(centers)) = 0;
	elseif any(isnan(centers))
		printm('fixing unused centers %d', sum(isnan(centers)))
		while any(isnan(centers))
			cgood = col(centers(~isnan(centers)));
			index = quant1_index(data, cgood);
			dhat = cgood(index);
			iworst = imax(abs(data - dhat));
			knan = find(isnan(centers));
			centers(knan(1)) = data(iworst);
		end
	end
%	disp([iter centers])
	change = max(abs(centers - old));
	iter = iter + 1;
end
if iter == max_iter + 1
	warning 'max %d iterations reached'
end
if chat
	printf('%s: %d iterations', mfilename, iter)
end

% quant1_index()
% find index of nearest centroid.
% this version works even for complex data / centroids
%
function index = quant1_index(x, centers)
[dummy index] = min(abs(outer_sum(x, -centers)), [], 2);
%breaks = (centers(2:end) + centers(1:end-1)) / 2;
%index0 = 1 + sum(outer_sum(x, -breaks) > 0, 2);
%minmax(index-index0)

% quant1_rms()
% rms error between data and its quantized version (complex ok)
%
function rms = quant1_rms(x, centers)
index = quant1_index(x, centers);
rms = sqrt(mean(abs(x(:) - col(centers(index))).^2));

% lloyd_max_hist_test
% self test: compare this approach to matlab's lloyds routine
function lloyd_max_hist_test
rng(0)
x = [10*randn(10^4,1); 50 + 15*randn(10^4,1)];
%x = [10*rand(10^4,1); 50 + 15*rand(10^4,1)];

L = 5;
%c0 = 10*linspace(-1,1,L);
pn = jf_protected_names;
c0 = pn.prctile(x, 100*([1:L]-0.5)/L);

[nx cx] = hist(x, 100);

ch = highrate_centers(x, L);

if exist('lloyds') == 2
	tic
	[p1 c1] = lloyds(x, c0);
	t1 = toc;
else
	p1 = nan(L,1);
	c1 = nan(L,1);
	t1 = nan;
end

tic
c2 = lloyd_max_hist(x, c0, 50);
t2 = toc;

o = ones(L,1);
if im
	plot(cx, nx, '-', c0, 80*o, 'yo', ch, 60*o, 'yx', ...
		c1, 40*o, 'ys', c2, 20*o, 'y^')
	legend('hist', 'c0', 'highrate', 'lloyds', 'hist')
end

tic
c3 = lloyd_max_hist(x, c0);
t3 = toc;

if exist('quantiz') == 2
	[dum1 dum2 distor] = quantiz(x, p1, c1);
else
	distor = nan;
end

rms0 = quant1_rms(x, c0);
rms1 = quant1_rms(x, c1);
rms2 = quant1_rms(x, c2);
rms3 = quant1_rms(x, c3);
rmsh = quant1_rms(x, ch);
printf('lloyds matlab time=%g rms=%g distor=%g', t1, rms1, sqrt(distor))
printf('lloyd_max_hist time=%g rms=%g', t2, rms2)
printf('lloyd_max (no hist) time=%g rms=%g', t3, rms3)
printf('rms0=%g rmsh=%g', rms0, rmsh)
if im
	disp([c0, c1, c2, c3])
end


function [xi,yi,curvatures] = maxr(a)
%MAXR   Find interpolated maximizer(s) and max value(s)
%       for (each column of) a.
%
%               [xi,yi,curvatures] = maxr(a)
%
%       Calls max() followed by qint() for quadratic interpolation.
%
   [m,n] = size(a);
   if m==1, a=a'; t=m; m=n; n=t; end;
   [y,x] = max(a);
   xi=x;    % vector of maximizer locations, one per col of a
   yi=y;    % vector of maximum values, one per column of a
   if nargout>2, curvatures = zeros(1,n); end
   for j=1:n,   % loop over columns
     if x(j)>1, % only support interior maxima
       if x(j)<m,
         [xdelta,yij,cj] = qint(a(x(j)-1,j),y(j),a(x(j)+1,j));
         xi(j) = x(j) + xdelta;
         if nargout>2, curvatures(j) = cj; end
         if (nargout>1), yi(j) = yij; end
       end;
     end;
   end;


function E = evaluate_weighted_energy(uv, im, occ, bfhsz, mfsz, sigma_i)

% edge region: weighted median filtering, the weights are determined by
%  spatial distance, intensity distance, occlusion state
% smooth region:

%   Author: Deqing Sun, Department of Computer Science, Brown University
%   Contact: dqsun@cs.brown.edu
%   $Date: 2009$
%   $Revision $
%
% Copyright 2009-2010, Brown University, Providence, RI. USA
%
%                          All Rights Reserved
%
% All commercial use of this software, whether direct or indirect, is
% strictly prohibited including, without limitation, incorporation into in
% a commercial product, use in a commercial service, or production of other
% artifacts for commercial purposes.
%
% Permission to use, copy, modify, and distribute this software and its
% documentation for research purposes is hereby granted without fee,
% provided that the above copyright notice appears in all copies and that
% both that copyright notice and this permission notice appear in
% supporting documentation, and that the name of the author and Brown
% University not be used in advertising or publicity pertaining to
% distribution of the software without specific, written prior permission.
%
% For commercial uses contact the Technology Venture Office of Brown University
%
% THE AUTHOR AND BROWN UNIVERSITY DISCLAIM ALL WARRANTIES WITH REGARD TO
% THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
% FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL THE AUTHOR OR
% BROWN UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
% DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR
% PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
% ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF
% THIS SOFTWARE.

sigma_x = 7;   %  spatial distance (7)

dilate_sz = 5*[1 1];  % dilation window size for flow edge region [5 5]

sz = size(im);
sz = sz(1:2);

if nargin < 3
    occ = ones(sz);
end;

if nargin < 4
    bfhsz = 10; % half window size
end;

%mfsz = [7 7]; % for test

if nargin < 5
    uvo = uv;
else
%     uvo(:,:,1) = medfilt2(uv(:,:,1), mfsz, 'symmetric');
%     uvo(:,:,2) = medfilt2(uv(:,:,2), mfsz, 'symmetric');
end;

if nargin < 6
    sigma_i = 5; %3.5; % intensity distance (10) 5 better than 10 and 20
end;

% % % WMF first then MF
% % uvo = uv;

e1 = edge(uv(:,:,1), 'sobel');
e2 = edge(uv(:,:,2), 'sobel');
e  = e1|e2;
mask = imdilate(e, ones(dilate_sz) );

% nearest 4 neighbors smaller than [3 3]
% tmp = [ 0 1 0; 1 1 1; 0 1 0];
% mask = imdilate(e, tmp );

% mask = e; % no dilation

% Update non-edge regions first
% uv(repmat(mask, [1 1 2])) = uvo(repmat(mask, [1 1 2]));

% Uncomment below to apply WMF to all regions
% mask = ones(size(im));

% Select boundary regions
[indx_row, indx_col] = find(mask ==1); %

% [H W] = size(im);
pad_u  = padarray(uv(:,:,1), bfhsz*[1 1], 'symmetric', 'both');
pad_v  = padarray(uv(:,:,2), bfhsz*[1 1], 'symmetric', 'both');
pad_im = padarray(im, bfhsz*[1 1], 'symmetric', 'both');
pad_occ= padarray(occ, bfhsz*[1 1], 'symmetric', 'both');

% fprintf('%d\n', length(indx_row));

% Divide into several groups for memory reasons ~70,000 causes out of memory

Indx_Row = indx_row;
Indx_Col = indx_col;
N        = length(Indx_Row); % number of elements to process
n        = 4e4;              % number of elements per batch
nB       = ceil(N/n);

E = 0;

for ib = 1:nB;
    istart = (ib-1)*n + 1;
    iend   = min(ib*n, N);
    indx_row = Indx_Row(istart:iend);
    indx_col = Indx_Col(istart:iend);

    neighbors_u = zeros((bfhsz*2+1)^2, length(indx_row));
    neighbors_v = neighbors_u;
    weights     = neighbors_u;

    for i = 1:length(indx_row)

        % crop window
        r1 = indx_row(i);
        r2 = indx_row(i) + 2*bfhsz;
        c1 = indx_col(i);
        c2 = indx_col(i) + 2*bfhsz;

        rc = indx_row(i) + bfhsz; % row  center
        cc = indx_col(i) + bfhsz; % column center
        ic = pad_im(rc, cc, :);       % intensity of the center pixel

        tmp_u = pad_u(r1:r2, c1:c2);
        tmp_v = pad_v(r1:r2, c1:c2);
        tmp_i = pad_im(r1:r2, c1:c2, :);

        % spatial weight
        [C R] = meshgrid(c1:c2, r1:r2);
        w = exp( -((C-cc).^2+(R-rc).^2)/2/sigma_x^2 );
        % Uncomment below: no spatial weight for test
        % w = ones(size(w));

        % intensity weight; comment below to disable the term
        w = w.* mean( exp(- (tmp_i - repmat(ic, [size(tmp_i,1) size(tmp_i, 2) 1])).^2/2/sigma_i^2), 3);

        % occluded weight;  comment below to disable the term
        w = w.*pad_occ(r1:r2, c1:c2);

        % Normalize
        w = w/sum(w(:));

%         neighbors_u(:,i) = tmp_u(:);
%         neighbors_v(:,i) = tmp_v(:);
%         weights(:,i)     = w(:);

        E = E + sum( abs(tmp_u(:) - pad_u(rc, cc)).* w(:));
        E = E + sum( abs(tmp_v(:) - pad_v(rc, cc)).* w(:));

    end;

end;

% for 5x5 equal weight

% Select non-boundary regions
[indx_row, indx_col] = find(mask ~=1);
bfhsz = floor(mfsz(1)/2);

pad_u  = padarray(uv(:,:,1), bfhsz*[1 1], 'symmetric', 'both');
pad_v  = padarray(uv(:,:,2), bfhsz*[1 1], 'symmetric', 'both');
pad_im = padarray(im, bfhsz*[1 1], 'symmetric', 'both');
pad_occ= padarray(occ, bfhsz*[1 1], 'symmetric', 'both');

% fprintf('%d\n', length(indx_row));

% Divide into several groups for memory reasons ~70,000 causes out of memory

Indx_Row = indx_row;
Indx_Col = indx_col;
N        = length(Indx_Row); % number of elements to process
n        = 4e4;              % number of elements per batch
nB       = ceil(N/n);

for ib = 1:nB;
    istart = (ib-1)*n + 1;
    iend   = min(ib*n, N);
    indx_row = Indx_Row(istart:iend);
    indx_col = Indx_Col(istart:iend);

    neighbors_u = zeros((bfhsz*2+1)^2, length(indx_row));
    neighbors_v = neighbors_u;
    weights     = neighbors_u;

    for i = 1:length(indx_row)

        % crop window
        r1 = indx_row(i);
        r2 = indx_row(i) + 2*bfhsz;
        c1 = indx_col(i);
        c2 = indx_col(i) + 2*bfhsz;

        rc = indx_row(i) + bfhsz; % row  center
        cc = indx_col(i) + bfhsz; % column center
        ic = pad_im(rc, cc, :);       % intensity of the center pixel

        tmp_u = pad_u(r1:r2, c1:c2);
        tmp_v = pad_v(r1:r2, c1:c2);
        tmp_i = pad_im(r1:r2, c1:c2, :);

        % spatial weight
        [C R] = meshgrid(c1:c2, r1:r2);
        w = exp( -((C-cc).^2+(R-rc).^2)/2/sigma_x^2 );
        % Uncomment below: no spatial weight for test
        w = ones(size(w));

%         % intensity weight; comment below to disable the term
%         w = w.* mean( exp(- (tmp_i - repmat(ic, [size(tmp_i,1) size(tmp_i, 2) 1])).^2/2/sigma_i^2), 3);
%
%         % occluded weight;  comment below to disable the term
%         w = w.*pad_occ(r1:r2, c1:c2);

        % Normalize
        w = w/sum(w(:));

%         neighbors_u(:,i) = tmp_u(:);
%         neighbors_v(:,i) = tmp_v(:);
%         weights(:,i)     = w(:);

        E = E + sum( abs(tmp_u(:) - pad_u(rc, cc)).* w(:));
        E = E + sum( abs(tmp_v(:) - pad_v(rc, cc)).* w(:));

    end;

end;


function give_Me_Jelly()

L = 3;           % Length of Computational Domain
ds = L/(2*600);  % Makes Lagrangian Spaced based on 1024x1024 finest grid

frac = 0.5;

% Makes Geometry for EQUILIBRIUM State %
%
a1 = 0.63;  %Semi-major axis length for position ONE (Expanded State)
b1 = 0.45;  %Semi-minor axis length for position ONE (Expanded State)
a1 = frac*a1; b1 = frac*b1;
a1
b1
[X1 Y1 Ninfo arcL] = make_Geometry_Position_ONE(ds,a1,b1);

% Makes Geometry for OVER-CONTRACTED State for RIGHT side %
%
a2 = 0.325; %Semi-major axis length for position TWO (Contracted State)
b2 = 0.6;   %GUESS: Semi-minor axis length for position TWO (Contracted State)
a2 = frac*a2; b2 = frac*b2;
[X2 Y2 b2] = make_Geometry_Position_TWO(a2,b2,b1,Ninfo,arcL);

% Makes Geometry for LESS-CONTRACTED State for LEFT side %
%
a3 = 0.4;   %Semi-major axis length for position TWO (Contracted State)
b3 = 0.7;   %GUESS: Semi-minor axis length for position TWO (Contracted State)
a3 = frac*a3; b3 = frac*b3;
a3
b3
[X3 Y3 b3] = make_Geometry_Position_TWO(a3,b3,b1,Ninfo,arcL);

% Makes Geometry for OVER-EXPANDED State for LEFT SIDE %
%
a4 = 0.75;  %Semi-major axis length for position TWO (Contracted State)
b4 = 0.3;   %GUESS: Semi-minor axis length for position TWO (Contracted State)
a4 = frac*a4; b4 = frac*b4;
[X4 Y4 b4] = make_Geometry_Position_TWO(a4,b4,b1,Ninfo,arcL);

% Translate Points from [-L/2,L/2]x[-L/2,L/2] -> [0,L]x[0,L]
X1 = X1 + L/2; Y1 = Y1 + L/2;
X2 = X2 + L/2; Y2 = Y2 + L/2;
X3 = X3 + L/2; Y3 = Y3 + L/2;
X4 = X4 + L/2; Y4 = Y4 + L/2;

nArm = (length(X1)-1)/2;

% Plots Geometry for Both Phases %
val = frac*0.75;
plot(X1,Y1,'*',X2(1:nArm),Y2(1:nArm),'r*',X3(nArm+2:end),Y3(nArm+2:end),'g*',X4(nArm+2:end),Y4(nArm+2:end),'m*'); hold on;
axis([0 L 0 L]);
legend('1','2','3','4');

figure(2)
plot(X2,Y2,'r*'); hold on;
plot(X1,Y1,'b*'); hold on;

% Prints Useful Info about Geometry %
fprintf('\n\n');
fprintf('Number of Total Geometry Pts: %d\n',Ninfo(2));
fprintf('Number of Geometry Pts in EACH Arm: %d\n',Ninfo(1));
fprintf('Arc length of bell (one side): %d\n',arcL);
fprintf('\n');
fprintf('(X1,Y1): Equilibrium State\n');
fprintf('(X2,Y2): OVER-Contracted State for RIGHT Side\n');
fprintf('(X3,Y3): Contracted State for LEFT Side (slightly less than right side)\n');
fprintf('(X4,Y4): OVER-EXPANDED State for LEFT Side\n');
fprintf('\n');
fprintf('Idea: \n');
fprintf('RHS contracts further than LHS, rests & returns to equil. state after LHS begins expanding\n');
fprintf('LHS contracts less than RHS, then expands further than equil. state and then returns to equil. state\n');
fprintf('\n');
fprintf('PUT FOLLOWING IN UPDATE_TARGET_POINTS.C:\n');
fprintf('numPts (# of Total Geometry Pts): %d\n',Ninfo(2));
fprintf('NL_Start (FIRST Point in LEFT Arm): %d\n\n\n',Ninfo(1)+1);

% Prints INPUT files %
print_Vertex_Pts(X1,Y1,Ninfo);
target_force = 5e6;
print_Target_Pts(target_force,Ninfo);

% Prints all vectors to .TXT files %
%print_Text_Files(Ninfo,X1,Y1,X2,Y2,X3,Y3,X4,Y4)

% Prints 2-Position Right/Left Side vectors to .TXT files %
print_2_Position_Text_File(Ninfo,X1,Y1,X2,Y2,X3,Y3);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Function: Prints 2 Position Phases:
%           XY(:,1) = original position x-Values
%           XY(:,3) = original position y-Values
%           XY(2:N+1,2) = 2nd position x-Values for RIGHT bell
%           XY(N+2:end,2) = 2nd position x-Values for LEFT bell
%           XY(2:N+1,4) = 2nd position y-Values for RIGHT bell
%           XY(N+2:end,4) = 2nd position y-Values for LEFT bell
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_2_Position_Text_File(Ninfo,X1,Y1,X2,Y2,X3,Y3)

N = Ninfo(1);    % Number of pts. in ONE arm
NTot = Ninfo(2); % Total # of Lag Pts. on Jelly Bell

% Initialization
XY = zeros(NTot,4);

% Construct Matrices For Storing Points
XY(:,1) = X1(1:NTot); XY(:,2) = X1(1:NTot);
XY(:,3) = Y1(1:NTot); XY(:,4) = Y1(1:NTot);

% 2nd Phase Right Arm
XY(2:N+1,2) = X2(2:N+1);
XY(2:N+1,4) = Y2(2:N+1);

% 2nd Phase Left Arm
XY(N+2:end,2) = X2(N+2:end);
XY(N+2:end,4) = Y2(N+2:end);

% PLOT TEST! %
%figure(3)
%plot(XY(:,1),XY(:,3),'k*'); hold on;
%plot(XY(:,2),XY(:,4),'g*'); hold on;

% PRINT x/y-Information %

fileID_XY = fopen('XY_2Pos.txt','w');

for i=1:length( XY(:,1) )
    fprintf(fileID_XY,'%1.16e %1.16e %1.16e %1.16e\n',XY(i,1),XY(i,2),XY(i,3),XY(i,4));
end

fclose(fileID_XY);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Function to PRINT all TEXT files for both PHASES
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Text_Files(Ninfo,X1,Y1,X2,Y2,X3,Y3,X4,Y4)

N = Ninfo(1); %Number of pts. in ONE arm

% x-Information %

fileID_x1 = fopen('x1.txt','w');
fprintf(fileID_x1,'%1.16e\n',X1);
fclose(fileID_x1);

fileID_x2 = fopen('x2.txt','w');
fprintf(fileID_x2,'%1.16e\n',X2);
fclose(fileID_x2);

fileID_x3 = fopen('x3.txt','w');
fprintf(fileID_x3,'%1.16e\n',X3);
fclose(fileID_x3);

fileID_x4 = fopen('x4.txt','w');
fprintf(fileID_x4,'%1.16e\n',X4);
fclose(fileID_x4);

% y-Information %

fileID_y1 = fopen('y1.txt','w');
fprintf(fileID_y1,'%1.16e\n',Y1);
fclose(fileID_y1);

fileID_y2 = fopen('y2.txt','w');
fprintf(fileID_y2,'%1.16e\n',Y2);
fclose(fileID_y2);

fileID_y3 = fopen('y3.txt','w');
fprintf(fileID_y3,'%1.16e\n',Y3);
fclose(fileID_y3);

fileID_y4 = fopen('y4.txt','w');
fprintf(fileID_y4,'%1.16e\n',Y4);
fclose(fileID_y4);

% xR1 = X1(2:2+N-1); % x-Pts. on RHS of center point for Position ONE
% xL1 = X1(2+N:end); % x-Pts. on LHS of center point for Position ONE
%
% xR2 = X2(2:2+N-1); % x-Pts. on RHS of center point for Position TWO
% xL2 = X2(2+N:end); % x-Pts. on LHS of center point for Position TWO
%
% yR1 = Y1(2:2+N-1); % y-Pts. on RHS of center point for Position ONE
% yL1 = Y1(2+N:end); % y-Pts. on RHS of center point for Position ONE
%
% yR2 = Y2(2:2+N-1); % y-Pts. on LHS of center point for Position TWO
% yL2 = Y2(2+N:end); % y-Pts. on LHS of center point for Position TWO
%
%
% fileID_xR1 = fopen('xR_1.txt','w');
% fprintf(fileID_xR1,'%1.16e\n',xR1);
% fclose(fileID_xR1);
%
% fileID_xR2 = fopen('xR_2.txt','w');
% fprintf(fileID_xR2,'%1.16e\n',xR2);
% fclose(fileID_xR2);
%
% fileID_xL1 = fopen('xL_1.txt','w');
% fprintf(fileID_xL1,'%1.16e\n',xL1);
% fclose(fileID_xL1);
%
% fileID_xL2 = fopen('xL_2.txt','w');
% fprintf(fileID_xL2,'%1.16e\n',xL2);
% fclose(fileID_xL2);
%
%
% fileID_yR1 = fopen('yR_1.txt','w');
% fprintf(fileID_yR1,'%1.16e\n',yR1);
% fclose(fileID_yR1);
%
% fileID_yR2 = fopen('yR_2.txt','w');
% fprintf(fileID_yR2,'%1.16e\n',yR2);
% fclose(fileID_yR2);
%
% fileID_yL1 = fopen('yL_1.txt','w');
% fprintf(fileID_yL1,'%1.16e\n',yL1);
% fclose(fileID_yL1);
%
% fileID_yL2 = fopen('yL_2.txt','w');
% fprintf(fileID_yL2,'%1.16e\n',yL2);
% fclose(fileID_yL2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Makes the Oblate Geometry (via ellipses)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [X1 Y1 Ninfo arcL] = make_Geometry_Position_ONE(ds,a,b)

%a: Semi-major axis (along horizontal)
%b: Semi-minor axis (along vertical)

%Finding elliptical arc length
h = ( (a-b)/(a+b) )^2;
arcL = (1/4)* pi*(a+b)*(1 + 3*h/ (10 + sqrt(4-3*h) ) ); % (1/4)th of the entire ELLIPTICAL ARC

n = 1;        %counting for array
X(n) = 0;     %first x-value at (0,-b)
Y(n) = -b;    %first y-value at (0,-b)
tprev = -pi/2;  %initial angle

tol = ds / 10; %error tolerance for root-finding algorithm

maxy = max(a,b);

    while tprev < 0

        n = n+1;
        tnext = tprev + ds/maxy; %Guess for next angle value

        if tprev < -pi/4
            tfar =  0.0;%abs(2*tprev);      %Far guess
        else
            tfar = pi/4;
        end

        %initiating guess for bisection-algorithm
        xn = a*cos(tnext);
        yn = b*sin(tnext);
        errSign = ( ds - sqrt( (xn-X(n-1))^2 + (yn-Y(n-1))^2 ) );
        err = abs(errSign);

        %Bisection algorithm to make points equally spaced
        while ( err > tol )

            if errSign < 0
                tfar = tnext;
                tnext = (tnext+tprev)/2;
            elseif errSign > 0
                tprev = tnext;
                tnext = (tnext+tfar)/2;
            end

            if tnext<tprev
                fprintf('NOT CONVERGING AT ANGLE %d\n',tprev)
                break;
            end

            xn = a*cos(tnext);
            yn = b*sin(tnext);
            errSign = ( ds - sqrt( (xn-X(n-1))^2 + (yn-Y(n-1))^2 ) );
            err = abs(errSign);
        end

        X(n) = xn;   %Store X-value
        Y(n) = yn;   %Store Y-value
        tprev = tnext; %Update previous angle

        %plot(xn,yn,'*'); hold on;

    end

 N = length(X);
 X(N+1) = a; %Add one more pt.
 Y(N+1) = 0; %Add one more pt.

 X1 = [X -X(2:end)]; %Reflects geometry
 Y1 = -[Y  Y(2:end)]; %Y-values for reflected X-value points

 Ninfo(1) = N;          % # of pts. along one arm (NOT counting center pt.)
 Ninfo(2) = length(X1); % Total # of pts. in geometry

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Makes the Oblate Geometry (via ellipses)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [X2 Y2 b] = make_Geometry_Position_TWO(a,bguess,b1,Ninfo,L)

%Find correct b to give same length arm
b = Newtons_Give_Me_Semi_Minor(a,bguess,L);

%Finding elliptical arc length
h = ( (a-b)/(a+b) )^2;
arcL = (1/4)* pi*(a+b)*(1 + 3*h/ (10 + sqrt(4-3*h) ) ); % (1/4)th of the entire ELLIPTICAL ARC

ds2 = arcL / Ninfo(1); %Ensures same number of pts on arm for position ONE and TWO

[X Y Ninfo] = make_Elliptic_Geometry_Position_TWO(ds2,a,b,Ninfo);

dV = b1-b;   %Makes sure first pt is the same as in position ONE

Y2 = -(Y-dV);%Minus sign to put jelly into right orientation
X2 = X;

%Ninfo
%plot(X2,Y2,'r*'); hold on;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Makes the Oblate Geometry (via ellipses)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function b = Newtons_Give_Me_Semi_Minor(a,bn,L)

tol = 1e-6;

bprev = 0.2*bn; %"Previous Guess" (left side of guess)
bnext = bn;     % First "Guess" at b
bfar =  1.5*bn; %"Far Guess" (right side of guess)

%Finding elliptical arc length
h = ( (a-bn)/(a+bn) )^2;
arcL = (1/4)* pi*(a+bn)*(1 + 3*h/ (10 + sqrt(4-3*h) ) ); % (1/4)th of the entire ELLIPTICAL ARC

errSign = L - arcL;
err = abs( errSign );

%Bisection algorithm to make points equally spaced
while ( err > tol )

    if errSign < 0
        bfar = bnext;
        bnext = (bnext+bprev)/2;
    elseif errSign > 0
        bprev = bnext;
        bnext = (bnext+bfar)/2;
    end

    %if bnext<bprev
    %    fprintf('NOT CONVERGING AT ANGLE %d\n',tprev)
    %    break;
    %end

    %Finding elliptical arc length
    h = ( (a-bnext)/(a+bnext) )^2;
    arcL = (1/4)* pi*(a+bnext)*(1 + 3*h/ (10 + sqrt(4-3*h) ) ); % (1/4)th of the entire ELLIPTICAL ARC

    errSign = ( L - arcL );
    err = abs(errSign);
end

b = bnext; %Assign value

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Makes the Oblate Geometry (via ellipses)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [X1 Y1 Ninfo] = make_Elliptic_Geometry_Position_TWO(ds,a,b,Ninfo)

%a: Semi-major axis (along horizontal)
%b: Semi-minor axis (along vertical)

n = 1;        %counting for array
X(n) = 0;     %first x-value at (0,-b)
Y(n) = -b;    %first y-value at (0,-b)
tprev = -pi/2;  %initial angle

tol = ds / 10; %error tolerance for root-finding algorithm

maxy = max(a,b);

    while n < Ninfo(1)+1

        n = n+1;
        tnext = tprev + ds/maxy; %Guess for next angle value

        if tprev < -pi/4
            tfar =  0.0;%abs(2*tprev);      %Far guess
        else
            tfar = pi/4;
        end

        %initiating guess for bisection-algorithm
        xn = a*cos(tnext);
        yn = b*sin(tnext);
        errSign = ( ds - sqrt( (xn-X(n-1))^2 + (yn-Y(n-1))^2 ) );
        err = abs(errSign);

        %Bisection algorithm to make points equally spaced
        while ( err > tol )

            if errSign < 0
                tfar = tnext;
                tnext = (tnext+tprev)/2;
            elseif errSign > 0
                tprev = tnext;
                tnext = (tnext+tfar)/2;
            end

            if tnext<tprev
                fprintf('NOT CONVERGING AT ANGLE %d\n',tprev)
                break;
            end

            xn = a*cos(tnext);
            yn = b*sin(tnext);
            errSign = ( ds - sqrt( (xn-X(n-1))^2 + (yn-Y(n-1))^2 ) );
            err = abs(errSign);
        end

        X(n) = xn;   %Store X-value
        Y(n) = yn;   %Store Y-value
        tprev = tnext; %Update previous angle

        %plot(xn,yn,'*'); hold on;

    end

 N = length(X)-1;
 %X(N+1) = a; %Add one more pt.
 %Y(N+1) = 0; %Add one more pt.

 %plot(X,Y,'*'); hold on;

 X1 = [X -X(2:end)]; %Reflects geometry
 Y1 = [Y  Y(2:end)]; %Y-values for reflected X-value points

 Ninfo(1) = N;          % # of pts. along one arm (NOT counting center pt.)
 Ninfo(2) = length(X1); % Total # of pts. in geometry

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Function to print VERTEX points
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Vertex_Pts(X,Y,Ninfo)

%Ninfo(1): # of pts. on one arm on Jellyfish
%Ninfo(2): # of total pts. in Jellyfish Geometry

Ntot = Ninfo(2);

vertex_fid = fopen('jellyfish.vertex', 'w');

% first line is the number of vertices in the file
fprintf(vertex_fid, '%d\n', Ntot);

for i=1:Ntot
    fprintf(vertex_fid, '%1.16e %1.16e\n', X(i), Y(i) );
end

fclose(vertex_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Function to print TARGET pts.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Target_Pts(target_force,Ninfo)

%Ninfo(1): # of pts. on one arm on Jellyfish
%Ninfo(2): # of total pts. in Jellyfish Geometry

Ntot = Ninfo(2);

% Write out the target point information
target_fid = fopen('jellyfish.target', 'w');

% First line is the number of target pts. in the file
fprintf(target_fid, '%d\n', Ntot);

for s=1:Ntot
        fprintf(target_fid, '%d %1.16e\n', s , target_force);
end

fclose(target_fid);


function ADEM_lorenz_entropy
% This demo shows how structure can be instilled from an environment. It
% uses an agent that optimises its recognition density over successive
% epochs of exposure to an environment. This environment causes the agent
% to flow on a Lorenz attractor with random perturbations. As the agent
% learns the causal regularities in its environment, it is better able to
% predict them and act to oppose the random effect. The result is that it
% is more robust to random forces and therefore exhibits states with lower
% entropy. This routine takes several minutes to run.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: ADEM_lorenz_entropy.m 4804 2012-07-26 13:14:18Z karl $

% generative process (environment)
%==========================================================================

% set up
%--------------------------------------------------------------------------
G(1).E.s = 1/4;                             % smoothness
G(1).E.n = 4;                               % embedding
G(1).E.d = 2;                               % embedding

% dynamics
%--------------------------------------------------------------------------
fG      = '[v + a; 0; 0] + [-P(1) P(1) 0; P(3) -1 -x(1); x(2) 0 P(2)]*x/64';
fM      = '[v    ; 0; 0] + [-P(1) P(1) 0; P(3) -1 -x(1); x(2) 0 P(2)]*x/64';

% parameters
%--------------------------------------------------------------------------
PG      = [10; -8/3; 32];                   % Target parameters
PM      = [10;    0;  0];                   % initial parameters

% level 1
%--------------------------------------------------------------------------
G(1).x  = [1; 1; 24];
G(1).f  = inline(fG ,'x','v','a','P');
G(1).g  = inline('x','x','v','a','P');
G(1).pE = PG;
G(1).V  = exp(8);                           % error precision
G(1).W  = exp(16);                          % error precision

% level 2
%--------------------------------------------------------------------------
G(2).a  = 0;                                % action variables
G(2).v  = 0;                                % inputs
G(2).V  = exp(16);
G       = spm_ADEM_M_set(G);

% % plot flow fields and equilibrium densities
% %==========================================================================
% spm_figure('GetWin','Figure 1');
%
% x{1}    = linspace(-20,20,32);
% x{2}    = linspace(-32,32,32);
% x{3}    = linspace(  10,40,8);
%
%
% % controlled flow (P0)
% %--------------------------------------------------------------------------
% subplot(3,2,1)
% spm_fp_display_density(G,x);
% xlabel('position','Fontsize',12)
% ylabel('velocity','Fontsize',12)
% title('controlled','Fontsize',16)

% recognition model: learn the controlled environmental dynamics
%==========================================================================

% make a naive model (M)
%--------------------------------------------------------------------------
M       = G;
M(1).f  = inline(fM ,'x','v','P');
M(1).g  = inline('x','x','v','P');
M(1).pE = PM;
M(1).pC = eye(3)*4;
M(1).V  = exp(8);
M(1).W  = exp(8);
M       = spm_DEM_M_set(M);

% teach naive model by exposing it to a controlled environment (G)
%==========================================================================

% length of realization
%--------------------------------------------------------------------------
n     = 256;
U     = sparse(n,1);

DEM.M = M;
DEM.G = G;
DEM.C = U;
DEM.U = U;

% optimise recognition model in epochs
%--------------------------------------------------------------------------
DEM.M(1).E.nE = 1;
H     = sparse(128,128);
for i = 1:32

        % random perturbations
        %------------------------------------------------------------------
        DEM.C       = spm_conv(randn(n,1)*8,8);

        % integrate and update priors
        %------------------------------------------------------------------
        DEM         = spm_ADEM(DEM);
        DEM.M(1).pE = DEM.qP.P{1};
        DEM.M(1).x  = DEM.qU.x{1}(:,end);
        DEM.M(2).a  = DEM.qU.a{2}(:,end);

        % save free-energy and conditional expectations
        %------------------------------------------------------------------
        qP(:,i) = DEM.qP.P{1};
        F(i)    = DEM.F;

        % display states at the beginning and end
        %------------------------------------------------------------------
        spm_figure('GetWin','Figure 1');

        if i < 8
            subplot(3,2,3)
            x = DEM.pU.v{1}(2,:);
            y = DEM.pU.v{1}(3,:);
            plot(x,y); hold on
            title('states before','FontSize',16)
            a = axis;

        elseif i > 24
            subplot(3,2,4)
            x = DEM.pU.v{1}(2,:);
            y = DEM.pU.v{1}(3,:);
            H = H + sparse(64 + fix(x)',64 + fix(y)',1,128,128);
            plot(x,y); hold on
            title('and after','FontSize',16)
            axis(a)

        end
    end

% graphics
%==========================================================================
spm_figure('GetWin','Figure 1');

% plot free-energy
%--------------------------------------------------------------------------
t   = 1:length(F);
subplot(3,1,1)
plot(t,-F)
xlabel('time (epochs)','FontSize',12)
title('Free-energy','FontSize',16)
axis square

% and underlying conditional expectations
%--------------------------------------------------------------------------
subplot(3,2,5)
plot(t,qP,t,kron(ones(1,length(t)),PG),'-.')
xlabel('time (epochs)','FontSize',12)
title('conditional parameters','FontSize',16)
axis square

subplot(3,2,6)
imagesc(H')
xlabel('first state','FontSize',12)
ylabel('second state','FontSize',12)
title('ensemble density','FontSize',16)
axis square xy


function h=framemul(f,Fa,Fs,s,varargin)
%FRAMEMUL  Frame multiplier
%   Usage:  h=framemul(f,Fa,Fs,s);
%
%   Input parameters:
%          Fa   : Analysis frame
%          Fs   : Synthesis frame
%          s    : Symbol
%          f    : Input signal
%
%   Output parameters:
%          h    : Output signal
%
%   `framemul(f,Fa,Fs,s)` applies the frame multiplier with symbol *s*
%   to the signal *f*. The frame *Fa* is used for analysis and the frame
%   *Fs* for synthesis.
%
%   Examples:
%   ---------
%
%   In the following example Gabor coefficients obtained through the DGT
%   of pink noise are multiplied by the symbol batmask before resynthesis.
%   The result of this operation is an output signal *h* that is constructed
%   through a Gabor expansion of the modified coefficients.:::
%
%      f = pinknoise(400);
%      a = 10;
%      M = 40;
%      [Fa, Fs] = framepair('dgt', 'gauss', 'dual', a, M);
%      s = framenative2coef(Fa, batmask);
%      fhat = framemul(f, Fa, Fs, s);
%      figure(1);
%      plotframe(Fa,frana(Fa,f),'clim',[-100,-20]);
%      figure(2);
%      plotframe(Fa,s,'lin');
%      figure(3);
%      plotframe(Fa,frana(Fa,fhat),'clim',[-100,-20]);
%
%   See also: iframemul, framemuladj

% Author: Peter L. Søndergaard

if nargin < 4
    error('%s: Too few input parameters.',upper(mfilename));
end;

if size(s,2)>1
    error(['%s: Symbol should be a column vecor i.e. ',...
           'in the common Frames framework coefficient format. ',...
           'See FRAMENATIVE2COEF and FRAMECOEF2NATIVE.' ],upper(mfilename));
end

% Check for compatibility
L1=framelength(Fa,size(f,1));
L2=framelengthcoef(Fs,size(s,1));
if L1~=L2
    error(['%s: The symbol and signal lengths are incompatible.'],upper(mfilename));
end;

% This is not *strictly* necessary, but we cannot check that the symbol
% is complex-valued in just the right way.
if Fa.realinput && ~isreal(s)
    error(['%s: For real-valued-input-only frames, the symbol must also ' ...
           'be real.'],upper(mfilename));
end;

h=frsyn(Fs,bsxfun(@times,frana(Fa,f),s));