function H=shadedErrorBar(x,y,errBar,lineProps,transparent)
% function H=shadedErrorBar(x,y,errBar,lineProps,transparent)
%
% Purpose
% Makes a 2-d line plot with a pretty shaded error bar made
% using patch. Error bar color is chosen automatically.
%
% Inputs
% x - vector of x values [optional, can be left empty]
% y - vector of y values or a matrix of n observations by m cases
%     where m has length(x);
% errBar - if a vector we draw symmetric errorbars. If it has a
%          size of [2,length(x)] then we draw asymmetric error bars
%          with row 1 being the upper bar and row 2 being the lower
%          bar. ** alternatively ** errBar can be a cellArray of
%          two function handles. The first defines which statistic
%          the line should be and the second defines the error
%          bar.
% lineProps - [optional,'-k' by default] defines the properties of
%             the data line. e.g.:
%             'or-', or {'-or','markerfacecolor',[1,0.2,0.2]}
% transparent - [optional, 0 by default] if ==1 the shaded error
%               bar is made transparent, which forces the renderer
%               to be openGl. However, if this is saved as .eps the
%               resulting file will contain a raster not a vector
%               image.
%
% Outputs
% H - a structure of handles to the generated plot objects.
%
%
% Examples
% y=randn(30,80); x=1:size(y,2);
% shadedErrorBar(x,mean(y,1),std(y),'g');
% shadedErrorBar(x,y,{@median,@std},{'r-o','markerfacecolor','r'});
% shadedErrorBar([],y,{@median,@std},{'r-o','markerfacecolor','r'});
%
% Overlay two transparent lines
% y=randn(30,80)*10; x=(1:size(y,2))-40;
% shadedErrorBar(x,y,{@mean,@std},'-r',1);
% hold on
% y=ones(30,1)*x; y=y+0.06*y.^2+randn(size(y))*10;
% shadedErrorBar(x,y,{@mean,@std},'-b',1);
% hold off
%
%
% Rob Campbell - November 2009

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Error checking
% error(nargchk(3,5,nargin))

%Process y using function handles if needed to make the error bar
%dynamically
if iscell(errBar) && ~isvector(y)
    fun1=errBar{1};
    fun2=errBar{2};
    errBar=fun2(y);
    y=fun1(y);
elseif ~iscell(errBar) && isvector(y)
    y=y(:)';
else
    error('2nd and 3rd input arguments are not compatible')
end

if isempty(x)
    x=1:length(y);
else
    x=x(:)';
end

if length(x) ~= length(y)
    error('inputs x and y are not of equal lengths')
end

%If only one error bar is specified then we will mirror it, turning it into
%both upper and lower bars.
if length(errBar)==length(errBar(:))
    errBar=repmat(errBar(:)',2,1);
else
    f=find(size(errBar)==2);
    if isempty(f), error('errBar has the wrong size'), end
    if f==2, errBar=errBar'; end
end

if length(x) ~= length(errBar)
    error('inputs x and y must have the same length as errBar')
end

%Set default options
defaultProps={'-k'};
if nargin<4 || isempty(lineProps)
    lineProps=defaultProps;
end
if ~iscell(lineProps)
    lineProps={lineProps};
end

if nargin<5 || ~isnumeric(transparent)
    transparent=0;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Plot the main line. We plot this first in order to extract the RGB values
% for the line colour. I am not aware of a function that does this.
H.mainLine=plot(x,y,lineProps{:});

% Work out the color of the shaded region and associated lines
% Using alpha requires the render to be openGL and so you can't
% save a vector image. On the other hand, you need alpha if you're
% overlaying lines. We therefore provide the option of choosing alpha
% or a de-saturated solid colour for the patch surface.

col=get(H.mainLine,'color');
edgeColor=col+(1-col)*0.55;
patchSaturation=0.15; %How de-saturated or transparent to make the patch
if transparent
    faceAlpha=patchSaturation;
    patchColor=col;
    set(gcf,'renderer','openGL')
else
    faceAlpha=1;
    patchColor=col+(1-col)*(1-patchSaturation);
    set(gcf,'renderer','painters')
end

%Calculate the y values at which we will place the error bars
uE=y+errBar(1,:);
lE=y-errBar(2,:);

%Add the error-bar plot elements
holdStatus=ishold;
if ~holdStatus, hold on,  end

%Make the cordinats for the patch
yP=[lE,fliplr(uE)];
xP=[x,fliplr(x)];

%remove any nans otherwise patch won't work
xP(isnan(yP))=[];
yP(isnan(yP))=[];

H.patch=patch(xP,yP,1,'facecolor',patchColor,...
              'edgecolor','none',...
              'facealpha',faceAlpha);

%Make nice edges around the patch.
H.edge(1)=plot(x,lE,'-','color',edgeColor);
H.edge(2)=plot(x,uE,'-','color',edgeColor);

%The main line is now covered by the patch object and was plotted first to
%extract the RGB value of the main plot line. I am not aware of an easy way
%to change the order of plot elements on the graph so we'll just remove it
%and put it back (yuk!)
delete(H.mainLine)
H.mainLine=plot(x,y,lineProps{:});

if ~holdStatus, hold off, end


%--- help for abstvar/variance_decomposition ---
%
%  Compute the variance decompsition of the VAR
%
%  ::
%
%     [vd,retcode] = variance_decomposition(self,params,Rfunc,shock_names,varargin);
%
%  Args:
%     self (var object):
%     params :(optional) parameter values
%     Rfunc (function handle): (optional) transform parameters into dynamics
%     shock_names : names of shocks
%     varargin
%
%  Returns:
%     :
%
%     - **vd** (struct): a struct containing the variance decomposition:
%
%        - **infinity** (struct):
%        - **conditional** (struct):
%
%     - **retcode** (integer): This passes through the retcode given by Rfunc. Currently (2018/07/19), this output always returns 0.
%
%
%    Other functions named variance_decomposition
%
%       generic/variance_decomposition
%


classdef Crop < dagnn.ElementWise
%CROP DagNN cropping layer.
%    This is a pecurial layer from FCN. It crops inputs{1} to
%    match the size of inputs{2} (starting with a base crop amount).
%    A future version

  properties
    crop = [0 0]
  end

  properties (Transient)
    inputSizes = {}
  end

  methods
    function crop = getAdaptedCrops(obj)
      cropv = obj.inputSizes{1}(1) - obj.inputSizes{2}(1) ;
      cropu = obj.inputSizes{1}(2) - obj.inputSizes{2}(2) ;
      cropv1 = max(0, cropv - obj.crop(1)) ;
      cropu1 = max(0, cropu - obj.crop(2)) ;
      crop = [cropv - cropv1, cropv1, cropu - cropu1, cropu1] ;
    end

    function outputs = forward(obj, inputs, params)
      obj.inputSizes = cellfun(@size, inputs, 'UniformOutput', false) ;
      adjCrop = obj.getAdaptedCrops() ;
      outputs{1} = vl_nncrop(inputs{1}, adjCrop) ;
    end

    function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)
      adjCrop = obj.getAdaptedCrops() ;
      derInputs{1} = vl_nncrop(inputs{1}, adjCrop, derOutputs{1}, obj.inputSize{1}) ;
      derInputs{2} = [] ;
      derParams = {} ;
    end

    function reset(obj)
      obj.inputSizes = {} ;
    end

    function outputSizes = getOutputSizes(obj, inputSizes)
      obj.inputSizes = inputSizes ;
      crop = obj.getAdaptedCrops() ;
      outputSizes{1} = inputSizes{1} - [crop(1)+crop(2), crop(3)+crop(4), 0, 0]
    end

    function rfs = getReceptiveFields(obj)
      rfs(1,1).size = [1 1] ;
      rfs(1,1).stride = [1 1] ;
      rfs(1,1).offset = 1 + obj.crop ;
      rfs(2,1).size = [] ;
      rfs(2,1).stride = [] ;
      rfs(2,1).offset = [] ;
    end

    function obj = Crop(varargin)
      obj.load(varargin) ;
    end
  end
end


load('../../word2vector_matlab/GoogleNews_words.mat');
load('../../word2vector_matlab/GoogleNews_vectors.mat');
load('caption_train.mat');
w_sum = cellfun(@(x) sum(x),w_names);
subset.is_appear = zeros(1,3000000,'single');

for i = 1:numel(caption_dic)
    disp(i);
    split_tline = strsplit(caption_dic{i},{'-',' ','.',',','(',')'},'CollapseDelimiters',true);
    for j=1:numel(split_tline)  % use sum to do hash
        word = split_tline{j};
        sub_index = find(w_sum == sum(word));
        if(isempty(sub_index))
            continue;
        end
        ind = cellfun(@(x) strcmp(x,word),w_names(sub_index));
        if(sum(ind(:))==0)
            continue;
        end
        index = sub_index(ind==1);
        %tmp = w_features(:,index);
        %sentence = sentence+tmp;
        subset.is_appear(index) = subset.is_appear(index) +1;
    end
end

sub = find(subset.is_appear>0);
subset.names = {w_names{sub}};
subset.features = w_features(:,sub);
save('CUHK-PEDES_dictionary.mat','subset');


function x = p36_start ( n )

%*****************************************************************************80
%
%% P36_START returns a starting point for optimization for problem 36.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables X.
%
%    Output, real X(N), a starting point for the optimization.
%
  x = [ 0.5, 1.0 ]';

  return
end


function snow = DetectSnow(dim,band_green,band_nir,band_bt,ndsi)
%DETECSNOW Summary of this function goes here
%   Detailed explanation goes here

% %     snow=zeros(dim,'uint8'); % Snow mask
    % It takes every snow pixels including snow pixel under thin clouds or icy clouds
    snow=ndsi>0.15&band_nir>1100&band_green>1000;
    if ~isempty(band_bt)
        snow=snow&band_bt<1000;
    end
%     snow(ids_snow)=1;
end


function testRB1
%testRB1: test the RBio toolbox.
%
% Example:
%   testRB1
%
% See also UFget, RBread, RBreade, testRB2.

% Copyright 2006, Timothy A. Davis

files = {
'bcsstk01.rb'
'farm.rb'
'lap_25.pse'
'lap_25.rb'
'west0479.rb'
'west0479.rua' } ;

for k = 1:length(files)
    file = files {k} ;
    fprintf ('%s : ', file) ;
    if (file (end) == 'e')
	[A Z] = RBreade (file) ;
    else
	[A Z] = RBread (file) ;
    end
    fprintf ('%s\n', RBtype (A)) ;
    RBwrite ('temp.rb', A, Z) ;
    [A2 Z2] = RBread ('temp.rb') ;
    if (~isequal (A, A2))
	fprintf ('test failed: %s (A differs)\n', file) ;
	error ('!') ;
    end
    if (~isequal (Z, Z2))
	fprintf ('test failed: %s (Z differs)\n', file) ;
	error ('!') ;
    end
end

load west0479
C = west0479 ;
RBwrite ('mywest', C, 'WEST0479 chemical eng. problem', 'west0479') ;
A = RBread ('mywest') ;
if (~isequal (A, C))
    error ('test failed: west0479 (MATLAB version)') ;
end
fprintf ('west0479 (MATLAB matrix) : %s\n', RBtype (A)) ;

if (~strcmp (RBtype (A), 'rua'))
    error ('test failed: RBtype(A)\n') ;
end
if (~strcmp (RBtype (spones (A)), 'pua'))
    error ('test failed: RBtype(spones(A))\n') ;
end
if (~strcmp (RBtype (2*spones (A)), 'iua'))
    error ('test failed: RBtype(2*spones(A))\n') ;
end
C = A+A' ;
if (~strcmp (RBtype (C), 'rsa'))
    error ('test failed: RBtype(A+A'')\n') ;
end

delete ('mywest') ;
delete ('temp.rb') ;
fprintf ('testRB1: all tests passed\n') ;


clear

if(isunix)
    executable = '"../../build/bin/FeatureExtraction"';
else
    executable = '"../../x64/Release/FeatureExtraction.exe"';
end

output = './processed_features/';

in_file = '../../samples/2015-10-15-15-14.avi';

command = sprintf('%s -f "%s" -out_dir "%s" -gaze -verbose', executable, in_file, output);

if(isunix)
    unix(command);
else
    dos(command);
end

%% Demonstrating reading the output files
[~, out_filename,~] = fileparts(in_file);
out_filename = sprintf('%s/%s.csv', output, out_filename);

% First read in the column names
tab = readtable(out_filename);
column_names = tab.Properties.VariableNames;

all_params  = dlmread(out_filename, ',', 1, 0);

gaze_angle_ids = cellfun(@(x) ~isempty(x) && x==1, strfind(column_names, 'gaze_angle_'));

gaze = all_params(:,gaze_angle_ids);

plot(gaze(:,1), 'DisplayName', 'Left - right');
hold on;
plot(gaze(:,2), 'DisplayName', 'Up - down');
xlabel('Frame') % x-axis label
ylabel('Angle in radians') % y-axis label
legend('show');
hold off;


function mnt = mnt_getNIRSMontage(varargin)
%MNT_GETNIRSMONTAGE - Get montage with NIRS channel positions
%
%Description:
% get montage with NIRS channel positions for a given set of sources and
% detectors. NIRS channels are placed half-way between a source and a
% detector on a spherical head model.
%
%Synopsis:
% MNT = nirs_getMontage(CLABSOURCE,CLABDETECTOR,<OPT>)
%       If you give channels labels of the source and detectors (should
%       correspond to EEG channel labels), montages for source and
%       detectors are produced, as well as montages for the
%       source-detector combinations.
%
% MNT = nirs_getMontage(MNT,<OPT>)
%       You can provide a montage struct wherein source and detector
%       montages are already given in .source and .detector fields.
%       In this case, only the source-detector combinations are determined.
%
%Input
% OPT: struct or property/value list of optional properties:
%  .File       - montage file used for channel positions
%  .ClabPolicy - specifies how the source-detector combinations are
%                labeled. 'Label' (default) concatenates the labels of
%                source and detector (eg. FzCz), 'Number' concatenates
%                the channel numbers (eg. 112)
%  .Connector  - specify how source and detectors labels are connected
%                (default ''). To have no connector, set to ''.
%  .Projection - determine projection of 3D coordinates to 2D
%                'orthogonal' or 'euclidean' (default)
%
%Output:
% MNT: NIRS montage
%  .x           - x coordinate of channel positions (2d projection)
%  .y           - y coordinate of channel positions (2d projection)
%  .pos_3d      - 3d positions on spherical head model
%  .clab        - channel labels
%  .angulardist - angular distances between sources and detectors on the
%                 spherical model head (in radians)
%  .sd          - source and detector index to which eg the channel belongs [1 12]
%  .source      - struct with montage for the sources
%  .detector    - struct with montage for the detectors
%
%See also: setElectrodeMontage, nirs_reduceMontage

% matthias.treder@tu-berlin.de 2011
% Markus Wenzel 2013 (adapted it to the new toolbox)
% Jan Mehnert February 2014 (ready for public BBCI toolbox) (jan@mehnert.org)

props= {'ClabPolicy'	'label'             'CHAR'
        'Projection'	'euclidean'         'CHAR'
        'PositionFcn'   @mntutil_posExt55   'FUNC'
        'Connector'     ''                  'CHAR'
       };

if nargin==0,
    mnt= props; return
end

if isstruct(varargin{1})
    mnt = varargin{1};
    opt= opt_proplistToStruct(varargin{2:end});
    misc_checkType(mnt, 'STRUCT'); %(redundant)
else
    clabSource = varargin{1};
    clabDetector = varargin{2};
    opt= opt_proplistToStruct(varargin{3:end});
    misc_checkType(clabSource, 'CHAR|CELL{CHAR}');
    misc_checkType(clabDetector, 'CHAR|CELL{CHAR}');
end

[opt, isdefault]= opt_setDefaults(opt, props);
opt_checkProplist(opt, props);

if ~exist('mnt','var')
  % Determine source and detector montages
  mnt = struct();
  mnt.source = mnt_setElectrodePositions(clabSource, 'PositionFcn', opt.PositionFcn);
  mnt.detector = mnt_setElectrodePositions(clabDetector, 'PositionFcn', opt.PositionFcn);
end

if any(isnan(mnt.source.pos_3d(:))) || any(isnan(mnt.detector.pos_3d(:)))
  warning(['Some source/detector positions are NaN. Note that the NIRS channels '...
    'involving these sources/detectors will also have NaN positions.\n '])
end

% Determine positions of source-detector combinations
mnt.x = [];
mnt.y = [];
mnt.pos_3d = [];
mnt.clab = {};
mnt.angulardist = [];

chanIdx = 1;
for ss=1:numel(mnt.source.clab)
  for dd=1:numel(mnt.detector.clab)
    ps = mnt.source.pos_3d(:,ss);
    pd = mnt.detector.pos_3d(:,dd);
    % 3d position of new channel (half-way between source and detector)
    % Set to norm=1 so that it's located on the head surface
    newpos = (ps + pd) / norm(ps+pd);
    % Angular distance in radians = angle between the two vectors1
    costheta = dot(ps,pd);
    theta = acos(costheta);
    % label
    if strcmp(opt.ClabPolicy,'label')
      label = [mnt.source.clab{ss} opt.Connector mnt.detector.clab{dd}];
    elseif strcmp(opt.ClabPolicy,'Number')
      label = [num2str(ss) opt.Connector num2str(dd)];
    end
    % Add to struct
    mnt.pos_3d(:,chanIdx) = newpos;
    mnt.angulardist = [mnt.angulardist theta];
    mnt.clab = {mnt.clab{:} label};
    mnt.sd(chanIdx,:) = [ss,dd];
    chanIdx = chanIdx + 1;
  end
end
mnt = mntutil_projectNIRSChannels3dTo2d(mnt,opt.Projection);


function [ daten f107 ] = read_solarflux( filename )

% INPUT
% 'filename'  : full path of solar flux observation file
% i.e. ...
% \STP\SOLAR_DATA\SOLAR_RADIO\FLUX\Penticton_Observed\daily\DAILYPLT.OBS

% OUTPUTS
% 'daten'     : array of MATLAB datenum dates
% 'f107'      : corresponding solar flux values

% Reads solar flux data files from Penticton Observatory
% daily F10.7 flux values, downloaded from ftp.ngdc.noaa.gov

% Author : John A. Smith, CIRES/NOAA, Univ. of Colorado at Boulder

fid = fopen(filename,'r','l','US-ASCII'); % open file

% check file open successful
if fid==-1
    error('Could not open file')
end

dat=textscan(fid,'%4d %2d %2d %f','treatasempty','.'); % read file contents

fclose(fid); % close file

% assign year, month and day
year=dat{1};
month=dat{2};
day=dat{3};

daten=datenum(double([year month day])); % convert to MATLAB datenum

f107=dat{4}; % assign f10.7 data

f107(f107==0)=NaN; % NaN empty fields

end


%{
B_LoopDeLoopCessna.m

This simply shows how the imported cessna patch object in A can be plugged directly into
the loop-de-loop example with no particular complications.

Matt Sheen, mws262@cornell.edu
%}

close all; clear all;
fig = figure;

%% Make the patch object:
patchData = stlread('cessna.stl');

p1 = patch(patchData,'EdgeColor','none','FaceColor','[0.8 0.8 1.0]','FaceAlpha',0.9);

axis equal
material('metal')
camlight('headlight');

p1.FaceAlpha = 0.5;

OrigVerts = p1.Vertices;

fig.Children.Clipping = 'off'; %Still see the plane when it leaves the axis area.
axis(2*[-30 30 -30 30 -10 50])
view(3)

%% Do the loop example.
vel = 5*[-1 0 0];
rot = angle2dcm(0,-0.04,0);
dt = 0.5;
center = [0 0 0];

totalRot = eye(3,3);

for i = 1:1000
    totalRot = totalRot*rot;
    center = dt*(totalRot*vel')' + center; %transform the velocity, keep track of the center position.
    p1.Vertices = (totalRot*OrigVerts')' + repmat(center,[size(OrigVerts,1),1]); %Rotate the patch object about its center and add new center point.

    pause(0.01);
end


function [VariableType,F_x,F_w,F_xw,h] = robust_classify_variables_newest(F,h,ops,w);

Dependency = iterateDependance( yalmip('monomtable') | yalmip('getdependence') |  yalmip('getdependenceUser'));
DependsOnw = find(any((Dependency(:,getvariables(w))),2));

h_variables = getvariables(h);
h_w = find(ismember(h_variables,DependsOnw));
if ~isempty(h_w)
    % If we have Q(x) + w*h(x) we can replace w*h(x) with t and have a
    % quadratic objective, and the linearly parameterized w*h(x) <= t
    % However, if h is quadratic in w such as x^2+2*x*w+w^2 we must move
    % everything to the cone epigraph, otherwise it is not convex in x
    if ~is(h,'compound') && max(degree(h,w)) == 1 && min(degree(h,w)) == 1
        base = getbase(h);
        h0 = base(1);
        base = base(2:end);base = base(:);
        sdpvar t

        F = [F,base(h_w(:))'*recover(h_variables(h_w)) <= t];
        base(h_w) = 0;
        h = h0 + base(:)'*recover(h_variables) + t;
    else
        sdpvar t
        F = [F, h <= t];
        h = t;
    end
    Dependency = iterateDependance(yalmip('monomtable') | yalmip('getdependence') | yalmip('getdependenceUser'));
    DependsOnw = find(any((Dependency(:,getvariables(w))),2));
end

DoesNotDependOnw = find(~any((Dependency(:,getvariables(w))),2));
[notused,x_variables] = find(Dependency(DoesNotDependOnw,:));

F_w = [];
F_x = [];
F_xw = [];
for i = 1:length(F)
    F_vars = getvariables(F(i));
    F_vars = find(any((Dependency(F_vars,:)),1));
    if all(ismember(F_vars,DependsOnw))
        F_w = F_w + F(i);
    elseif all(ismember(F_vars,DoesNotDependOnw))
       F_x = F_x + F(i);
    else
       F_xw = F_xw + F(i);
    end
end
ops.removeequalities = 0;
[F_x,failure,cause] = expandmodel(F_x,h,ops);
[F_w,failure,cause] = expandmodel(F_w,[],ops);
ops.expandbilinear = 1;
ops.reusemodel = 1; % Might be case x+norm(w)<1, norm(w)<1
[F_xw,failure,cause] = expandmodel(F_xw,h,ops,w);

w_variables = depends(F_w);
x_variables = unique([depends(F_x) depends(F_xw) depends(h)]);
x_variables = setdiff(x_variables,w_variables);

% After exanding the conic represntable, we have introduced new variables
Dependency = iterateDependance(yalmip('monomtable') | yalmip('getdependence') | yalmip('getdependenceUser'));

auxiliary = unique([yalmip('extvariables') yalmip('auxvariables')]);
if 1%~isempty(auxiliary)
    DependsOnw = find(any((Dependency(:,getvariables(w))),2));
    DoesNotDependOnw = find(~any((Dependency(:,getvariables(w))),2));

    temp = intersect(DependsOnw,x_variables);
    x_variables = setdiff(x_variables,DependsOnw);
    aux_with_w_dependence = temp;
else
    aux_with_w_dependence = [];
end

% aux_w_or_w = union(aux_with_w_dependence,w_variables);
% old_w_variables = [];
% while ~isequal(w_variables,old_w_variables);
%     old_w_variables = w_variables;
%     for i = 1:length(F_xw)
%         if all(ismember(depends(F_xw(i)),aux_w_or_w))
%             if ~any(ismember(depends(F_xw(i)),x_variables))
%                 new_w = intersect(depends(F_xw(i)),aux_w_or_w);
%                 w_variables = union(w_variables,new_w);
%                 aux_with_w_dependence = setdiff(aux_with_w_dependence,new_w);
%                 goon = 1;
%             end
%         end
%     end
%     aux_w_or_w = union(aux_with_w_dependence,w_variables);
% end

x = recover(x_variables);
w = recover(w_variables);

if ~isempty(F_xw)
    F_xw_scalar =  F_xw(find(is(F_xw,'elementwise') | is(F_xw,'equality')));
    F_xw_multi = F_xw - F_xw_scalar;
else
    F_xw_scalar = [];
    F_xw_multi = F_xw;
end

[MonomTable,Nonlinear] = yalmip('monomtable');
Dependency = yalmip('getdependenceUser');
evar = yalmip('extvariables');
if length(F_xw_scalar)>0
    % Optimize dependency graph
    X = sdpvar(F_xw_scalar);
    Xvar = getvariables(X);
    Xbase = getbase(X);Xbase = Xbase(:,2:end);
    for i = 1:size(Xbase,1)
        used = Xvar(find(Xbase(i,:)));
        if any(Nonlinear(used))
        used = find(any(MonomTable(used,:),1));
        end
        auxUsed = intersect(used,aux_with_w_dependence);

        if ~isempty(auxUsed)
            wUsed = intersect(used,w_variables);
            if ~isempty(wUsed)
                Dependency(auxUsed,wUsed) = 1;
            end
            eUsed = intersect(used,evar);
            if ~isempty(eUsed)
                Dependency(eUsed,auxUsed) = 1;
            end
        end
    end
end
if length(F_xw_multi) > 0
    for i = 1:length(F_xw_multi)
        used = getvariables(F_xw_multi(i));
        used = find(any(MonomTable(used,:),1));
        auxUsed = intersect((used),aux_with_w_dependence);
        wUsed = intersect((used),w_variables);
        if ~isempty(auxUsed) & ~isempty(wUsed)
            Dependency(auxUsed,wUsed) = 1;
        end
        eUsed = intersect((used),evar);
        if ~isempty(auxUsed) & ~isempty(eUsed)
            Dependency(eUsed,auxUsed) = 1;
        end
    end
end
UserDependency = yalmip('getdependenceUser');
fixed = find(any(UserDependency,2));
Dependency(fixed,:) = UserDependency(fixed,:);
Dependency = iterateDependance(Dependency);

VariableType.Graph = Dependency;
VariableType.x_variables = x_variables;
VariableType.w_variables = w_variables;
VariableType.aux_with_w_dependence = aux_with_w_dependence;

function Graph = iterateDependance(Graph)

Graph = Graph + speye(length(Graph));
Graph0 = double(Graph*Graph ~=0);
while ~isequal(Graph,Graph0)
    Graph = Graph0;
    Graph0 = double(Graph*Graph~=0);
end


function prediction = rmHrfSearchFit_oneGaussian(model, params, loopSlices, wProcess,varexp,allstimimages)
% rmHrfSearchFit_oneGaussian - make predictions without HRF
%
% prediction = rmHrfSearchFit_oneGaussian(model, params, loopSlices, wProcess)
%
% 10/2009: SD & WZ wrote it.

% intiate data stucture for our pRF values
tmp.x    = [];
tmp.y    = [];
tmp.s    = [];
tmp.ve   = [];

% Get the data required to build our pRF
for slice=loopSlices,
    % now we extract only the data from that slice
    s = rmSliceGet(model,slice);

    % if we have more than one slice, then check which voxels to process in
    % each slice
    if length(loopSlices) > 1
        wProcessSlice = wProcess{slice};
        varexpSlice  = varexp{slice};
    else
        wProcessSlice = wProcess;
        varexpSlice  = varexp;
    end

    % store
    tmp.x = [tmp.x s{1}.x0(wProcessSlice)];
    tmp.y = [tmp.y s{1}.y0(wProcessSlice)];
    tmp.s = [tmp.s s{1}.s(wProcessSlice)];
    tmp.ve = [tmp.ve varexpSlice(wProcessSlice)];
end

% build pRF and make our predictions
n = numel(tmp.x);
s = [[1:ceil(n./1000):n-2] n+1]; %#ok<NBRAK>
prediction = zeros(size(allstimimages,1),n);
fprintf(1,'[%s]:Making predictions without HRF for each voxel (%d):',mfilename,n);
drawnow;tic;
for n=1:numel(s)-1,
    % make rfs
    rf   = rfGaussian2d(params.analysis.X, params.analysis.Y,...
        tmp.s(s(n):s(n+1)-1), ...
        tmp.s(s(n):s(n+1)-1),...
        zeros(size(tmp.s(s(n):s(n+1)-1))), ...
        tmp.x(s(n):s(n+1)-1), ...
        tmp.y(s(n):s(n+1)-1));

    % convolve with stimulus
    pred = allstimimages*rf;

    % store
    prediction(:,s(n):s(n+1)-1) = pred;
    fprintf(1,'.');drawnow;
end;
clear n s rf pred;
fprintf(1, 'Done[%d min].\t(%s)\n', round(toc/60), datestr(now));
drawnow;


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

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

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


classdef casadi_block < matlab.System & matlab.system.mixin.Propagates
    % Casadi Implementation of the Momentum Based Velocity COntrol.

    properties
        % Public, tunable properties.

    end

    properties (DiscreteState)
    end

    properties (Access = private)
        casadi_optimizer;
        M;
        h;
        Jc;
        Jcmm;
        nu_k;
        C_friction;
        b_friction;
        s_dot_star;
        H_star;
        tau_k;
        s_dot_k_1;
        v_b_k_1;
        f_k;
        Jc_dot_nu;
        sol;
        tau_meas;
        f_meas;
        solver_fails_counter;
        s;
        CoM_measured;
        K_friction;
        t_step;
        epsilon_CoM;
        CoM_max;
        CoM_min;
        CoM_vel_max;
        CoM_vel_min;
        ActivateComHeightConstraint;
        CoM_Z_reference_velocity;
        feet_correction;
        holonomicConstraintActivation;
        JTorso;
        SaturationMinJoints;
        SaturationMaxJoints;
        J_com;

    end

    methods (Access = protected)
        function num = getNumInputsImpl(~)
            num = 36;
        end
        function num = getNumOutputsImpl(~)
            num = 4;
        end
        function [dt1,dt2,dt3,dt4] = getOutputDataTypeImpl(~)
            dt1 = 'double';
            dt2 = 'double';
            dt3 = 'double';
            dt4 = 'double';

        end

        function [dt1,dt2,dt3,dt4,dt5,dt6,dt7,dt8,dt9,dt10,dt11,dt12,dt13,dt14,dt15,dt16,dt17,dt18,dt19,dt20,dt21,dt22,dt23,dt24, ...
                dt25,dt26,dt27,dt28,dt29,dt30,dt31,dt32,dt33,dt34,dt35,dt36] = getInputDataTypeImpl(~)
            dt1 = 'double';
            dt2 = 'double';
            dt3 = 'double';
            dt4 = 'double';
            dt5 = 'double';
            dt6 = 'double';
            dt7 = 'double';
            dt8 = 'double';
            dt9 = 'double';
            dt10 = 'double';
            dt11 = 'double';
            dt12 = 'double';
            dt13 = 'double';
            dt14 = 'double';
            dt15 = 'double';
            dt16 = 'double';
            dt17 = 'double';
            dt18 = 'double';
            dt19 = 'double';
            dt20 = 'double';
            dt21 = 'double';
            dt22 = 'double';
            dt23 = 'double';
            dt24 = 'double';
            dt25 = 'double';
            dt26 = 'double';
            dt27 = 'double';
            dt28 = 'double';
            dt29 = 'double';
            dt30 = 'double';
            dt31 = 'double';
            dt32 = 'double';
            dt33 = 'double';
            dt34 = 'double';
            dt35 = 'double';
            dt36 = 'double';
        end
        function [sz1,sz2,sz3,sz4] = getOutputSizeImpl(obj)
            % Hack for avoiding hard coding the DoF of the Robot
            size_temp = propagatedInputSize(obj,1);
            sz1 = [6,1];
            sz2 = [size_temp(1)-6,1];
            sz3 = [size_temp(1)-6,1];
            sz4 = [12,1];
        end
        function [sz1,sz2,sz3,sz4,sz5,sz6,sz7,sz8,sz9,sz10,sz11,sz12,sz13,sz14,sz15,sz16, sz17,sz18,sz19,sz20,sz21,sz22, ...
                sz23,sz24,sz25,sz26,sz27,sz28,sz29,sz30,sz31,sz32,sz33,sz34, sz35,sz36] = getInputSizeImpl(~)
            sz1 = [1,1];
            sz2 = [1,1];
            sz3 = [1,1];
            sz4 = [1,1];
            sz5 = [1,1];
            sz6 = [1,1];
            sz7 = [1,1];
            sz8 = [1,1];
            sz9 = [1,1];
            sz10 = [1,1];
            sz11 = [1,1];
            sz12 = [1,1];
            sz13 = [1,1];
            sz14 = [1,1];
            sz15 = [1,1];
            sz16 = [1,1];
            sz17 = [1,1];
            sz18 = [1,1];
            sz19 = [1,1];
            sz20 = [1,1];
            sz21 = [1,1];
            sz22 = [1,1];
            sz23 = [1,1];
            sz24 = [1,1];
            sz25 = [1,1];
            sz26 = [1,1];
            sz27 = [1,1];
            sz28 = [1,1];
            sz29 = [1,1];
            sz30 = [1,1];
            sz31 = [1,1];
            sz32 = [1,1];
            sz33 = [1,1];
            sz34 = [1,1];
            sz35 = [1,1];
            sz36 = [1,1];
        end
        function [cp1,cp2,cp3,cp4,cp5,cp6,cp7,cp8,cp9,cp10,cp11,cp12,cp13, cp14, cp15,cp16, cp17,cp18,cp19,cp20,cp21,...
                cp22,cp23,cp24,cp25,cp26,cp27,cp28,cp29,cp30,cp31,cp32,cp33,cp34,cp35,cp36] = isInputComplexImpl(~)
            cp1 = false;
            cp2 = false;
            cp3 = false;
            cp4 = false;
            cp5 = false;
            cp6 = false;
            cp7 = false;
            cp8 = false;
            cp9 = false;
            cp10 = false;
            cp11 = false;
            cp12 = false;
            cp13 = false;
            cp14 = false;
            cp15 = false;
            cp16 = false;
            cp17 = false;
            cp18 = false;
            cp19 = false;
            cp20 = false;
            cp21 = false;
            cp22 = false;
            cp23 = false;
            cp24 = false;
            cp25 = false;
            cp26 = false;
            cp27 = false;
            cp28 = false;
            cp29 = false;
            cp30 = false;
            cp31 = false;
            cp32 = false;
            cp33 = false;
            cp34 = false;
            cp35 = false;
            cp36 = false;
        end
        function [cp1,cp2,cp3,cp4] = isOutputComplexImpl(~)
            cp1 = false;
            cp2 = false;
            cp3 = false;
            cp4 = false;
        end
        function [fz1,fz2,fz3,fz4,fz5,fz6,fz7,fz8,fz9,fz10, fz11,fz12,fz13,fz14,fz15,fz16, fz17,fz18,fz19,fz20,fz21, ...
                fz22,fz23,fz24,fz25,fz26,fz27,fz28,fz29,fz30,fz31,fz32,fz33,fz34, fz35,fz36] = isInputFixedSizeImpl(~)
            fz1 = true;
            fz2 = true;
            fz3 = true;
            fz4 = true;
            fz5 = true;
            fz6 = true;
            fz7 = true;
            fz8 = true;
            fz9 = true;
            fz10 = true;
            fz11 = true;
            fz12 = true;
            fz13 = true;
            fz14 = true;
            fz15 = true;
            fz16 = true;
            fz17 = true;
            fz18 = true;
            fz19 = true;
            fz20 = true;
            fz21 = true;
            fz22 = true;
            fz23 = true;
            fz24 = true;
            fz25 = true;
            fz26 = true;
            fz27 = true;
            fz28 = true;
            fz29 = true;
            fz30 = true;
            fz31 = true;
            fz32 = true;
            fz33 = true;
            fz34 = true;
            fz35 = true;
            fz36 = true;
        end
        function [fz1,fz2,fz3,fz4] = isOutputFixedSizeImpl(~)
            fz1 = true;
            fz2 = true;
            fz3 = true;
            fz4 = true;
        end
        function setupImpl(obj,M,h,Jc,Jcmm,nu_k,Jc_dot_nu,C_friction,b_friction,H_star,s_dot_star,tau_meas, contact_status, f_meas, x_com, J_com, w_H_b,...
                s,CoM_measured,ADD_FRICTION, K_viscous_friction, T, Gamma, baseVelocitySat, jointsVelocitySat, torqueSat, wrenchesSat, tStep, NDOF, epsilon_CoM, CoM_limits,CoM_vel_limits, activateCoMHeightTask, CoMZReferenceVelocity, correctionFeet, ActivateHolonomicConstraint, JTorso)

            NDOF = size(K_viscous_friction,1);
            obj.solver_fails_counter = 0;
            import casadi.*
            obj.solver_fails_counter = 0;
            optimization_type = 'conic';
            solver = 'osqp';
            p_opts = struct('expand',true);
            s_opts = struct();

            obj.casadi_optimizer = casadi.Opti(optimization_type);
            obj.casadi_optimizer.solver(solver, p_opts, s_opts);

            %Definition of the decision variables
            obj.tau_k = obj.casadi_optimizer.variable(NDOF);
            obj.s_dot_k_1 = obj.casadi_optimizer.variable(NDOF);
            obj.v_b_k_1 = obj.casadi_optimizer.variable(6);
            obj.f_k = obj.casadi_optimizer.variable(12);
            nu_k_1 = [obj.v_b_k_1; obj.s_dot_k_1];

            % Setting the parameter for the optimization solver
            obj.M = obj.casadi_optimizer.parameter(NDOF+6,NDOF+6);
            obj.h = obj.casadi_optimizer.parameter(NDOF+6);
            obj.Jc = obj.casadi_optimizer.parameter(12,NDOF+6);
            obj.Jcmm = obj.casadi_optimizer.parameter(6,NDOF+6);
            obj.nu_k = obj.casadi_optimizer.parameter(NDOF+6);
            obj.C_friction = obj.casadi_optimizer.parameter(38,12);
            obj.b_friction = obj.casadi_optimizer.parameter(38);
            obj.s_dot_star = obj.casadi_optimizer.parameter(NDOF);
            obj.H_star = obj.casadi_optimizer.parameter(6);
            obj.Jc_dot_nu = obj.casadi_optimizer.parameter(12);
            obj.tau_meas = obj.casadi_optimizer.parameter(NDOF);
            obj.f_meas = obj.casadi_optimizer.parameter(12);
            obj.s = obj.casadi_optimizer.parameter(NDOF);
            obj.CoM_measured = obj.casadi_optimizer.parameter(3);
            obj.K_friction = obj.casadi_optimizer.parameter(NDOF+6,NDOF+6);
            obj.t_step = obj.casadi_optimizer.parameter(1);
            obj.CoM_max = obj.casadi_optimizer.parameter(2);
            obj.CoM_min = obj.casadi_optimizer.parameter(2);
            obj.CoM_vel_max = obj.casadi_optimizer.parameter(2);
            obj.CoM_vel_min = obj.casadi_optimizer.parameter(2);
            obj.epsilon_CoM = obj.casadi_optimizer.parameter(2);
            obj.ActivateComHeightConstraint = obj.casadi_optimizer.parameter(1);
            obj.CoM_Z_reference_velocity = obj.casadi_optimizer.parameter(1);
            obj.feet_correction = obj.casadi_optimizer.parameter(12);
            obj.holonomicConstraintActivation = obj.casadi_optimizer.parameter(1);
            obj.JTorso= obj.casadi_optimizer.parameter(6,NDOF+6);
            obj.SaturationMaxJoints = obj.casadi_optimizer.parameter(NDOF);
            obj.SaturationMinJoints = obj.casadi_optimizer.parameter(NDOF);
            obj.J_com = obj.casadi_optimizer.parameter(6,NDOF+6);

            %Weigth
            Weigth.PosturalTask    = 100;
            Weigth.MomentumLinear  = 2.0;
            Weigth.MomentumAngular = 30.00;
            Weigth.RegTorques      = 0.001;
            Weigth.RegVelocities   = 128;
            Weigth.Wrenches        = 0.01;
            Weigth.HolonomicConstraint = 10;
            % Selector Matrix

            B           = [ zeros(6,NDOF);  ...
                            eye(NDOF,NDOF)];

            % Setting the objective function
            obj.casadi_optimizer.minimize(Weigth.RegTorques*sumsqr(obj.tau_k - obj.tau_meas)+             ... % Torque Regularization
                Weigth.RegVelocities*sumsqr(nu_k_1(1:6))+                                                 ... % Base Velocity Regularization
                Weigth.PosturalTask*sumsqr(obj.s_dot_k_1-obj.s_dot_star));                                ... % Postural Task
                %Weigth.HolonomicConstraint*(1-obj.holonomicConstraintActivation)*sumsqr(obj.Jc*(nu_k_1)));                                     ... %HolonomicCOnstraint
                %Weigth.Wrenches*sumsqr(obj.f_k) +                                                        ... %Wrenches
                %Weigth.MomentumAngular*sumsqr(obj.H_star(4:6)-obj.Jcmm(4:6,:)*nu_k_1));                  ... % Angular Momentum
                %Weigth.MomentumLinear*sumsqr(obj.H_star(1:3)-obj.Jcmm(1:3,:)*nu_k_1));                   ... % Linear Momentum

            % Setting Constraint
            % Dynamics
            obj.casadi_optimizer.subject_to((obj.M/obj.t_step + obj.K_friction)*(nu_k_1)-(obj.M*obj.nu_k)/obj.t_step+obj.h== B*obj.tau_k +obj.Jc'*obj.f_k);
            % Holonomic Constraint
            % acceleration-wise
            %obj.casadi_optimizer.subject_to(obj.holonomicConstraintActivation*(obj.Jc*(nu_k_1 - obj.nu_k)/obj.t_step) == obj.holonomicConstraintActivation*(-obj.Jc_dot_nu));
            % velocity-wise
            obj.casadi_optimizer.subject_to(obj.holonomicConstraintActivation*obj.Jc*(nu_k_1)==obj.holonomicConstraintActivation*obj.feet_correction);
            % Friction Cones
            obj.casadi_optimizer.subject_to(obj.C_friction*obj.f_k<obj.b_friction);

            %% Constraint on the CoM Velocity
            obj.casadi_optimizer.subject_to(obj.J_com(1,:)*nu_k_1<=(tanh(obj.epsilon_CoM*(obj.CoM_max(1)-obj.CoM_measured(1)))*obj.CoM_vel_max(1)));
            obj.casadi_optimizer.subject_to(obj.J_com(1,:)*nu_k_1>=(tanh(obj.epsilon_CoM*(obj.CoM_measured(1)-obj.CoM_min(1)))*obj.CoM_vel_min(1)));
            obj.casadi_optimizer.subject_to(obj.J_com(2,:)*nu_k_1<=(tanh(obj.epsilon_CoM*(obj.CoM_max(2)-obj.CoM_measured(2)))*obj.CoM_vel_max(2)));
            obj.casadi_optimizer.subject_to(obj.J_com(2,:)*nu_k_1>=(tanh(obj.epsilon_CoM*(obj.CoM_measured(2)-obj.CoM_min(2)))*obj.CoM_vel_min(2)));

            % Constraint the Torso velocity on the z axis
            %obj.casadi_optimizer.subject_to(obj.ActivateComHeightConstraint*obj.Jcmm(3,7:end)*nu_k_1(7:end) >= obj.ActivateComHeightConstraint*obj.M(1,1)*obj.CoM_Z_reference_velocity);
            obj.casadi_optimizer.subject_to(obj.ActivateComHeightConstraint*obj.JTorso(3,:)*nu_k_1 >= obj.ActivateComHeightConstraint*obj.CoM_Z_reference_velocity);
%
            % Angular Momentum
            % With Control Law
            % obj.casadi_optimizer.subject_to(obj.H_star(4:6,:)==obj.Jcmm(4:6,:)*nu_k_1);
            % Setting to Zero
            %obj.casadi_optimizer.subject_to(obj.Jcmm(4:6,:)*nu_k_1 == zeros(3,1));
            %THE BOUND FOR NOW ARE DE-ACTIVATED
            % Setting upper and lower bound
            % Lower and Upper Bound NOTE by using a casadi optimizer object, we loose the capability of interpreting the lower and
            % uppe bound in a smart way.
            % obj.casadi_optimizer.subject_to(Sat.BaseLinearVelocity_min<= obj.v_b_k_1(1:3)<=Sat.BaseLinearVelocity_max);
            % obj.casadi_optimizer.subject_to(Sat.BaseAngVelocity_min<= obj.v_b_k_1(4:end)<=Sat.BaseAngVelocity_max);
            %obj.casadi_optimizer.subject_to(obj.SaturationMinJoints<= obj.s_dot_k_1);
            %obj.casadi_optimizer.subject_to(obj.s_dot_k_1<= obj.SaturationMaxJoints);
            % obj.casadi_optimizeru.sbject_to(Sat.torques_min(1:3)<= obj.tau_k(1:3)<= Sat.torques_max(1:3));

        end

        function [v_b_star,s_dot_k_1_star,tau_star,f_star] = stepImpl(obj,M,h,Jc,Jcmm,nu_k,Jc_dot_nu,C_friction,b_friction,H_star,s_dot_star,tau_meas, contact_status, f_meas, x_com, J_com, w_H_b,...
                s,CoM_measured,ADD_FRICTION, K_viscous_friction, T, Gamma, baseVelocitySat, jointsVelocitySat, torqueSat, wrenchesSat, tStep, NDOF,epsilon_CoM, CoM_limits,CoM_vel_limits, activateCoMHeightTask, CoMZReferenceVelocity, correctionFeet, activateHolonomicConstraint, JTorso )
            solver_succeded = true;

            % Compute Centroidayl Dynamics
            selector = zeros(length(nu_k),1);
            selector(3) = 1 ;
            DYNAMICS.M = M;
            DYNAMICS.h = h;
            DYNAMICS.g = M*selector*(9.81);
            DYNAMICS.Jc = Jc;
            DYNAMICS.dJc_nu = Jc_dot_nu;
            FORKINEMATICS.xCoM = x_com;
            vel_com = J_com*nu_k;
            FORKINEMATICS.dxCoM = vel_com(1:3);

            STATE.nu = nu_k;
            STATE.dx_b = nu_k(1:3);
            STATE.x_b = w_H_b(1:3,4);
            centroidalDyn = centroidalConversion(DYNAMICS,FORKINEMATICS,STATE);

            %K friction
            K_friction_new = zeros(NDOF+6);

            if(ADD_FRICTION)
                invTGamma                = eye(size(Gamma))/(T*Gamma);
                invTGamma_t              = eye(size(Gamma))/(transpose(T*Gamma));
                K_friction_new(7:end, 7:end) = invTGamma_t*K_viscous_friction*invTGamma;
            end

            % Update casadi optimizer parameters
            obj.casadi_optimizer.set_value(obj.M, centroidalDyn.M);
            obj.casadi_optimizer.set_value(obj.h,( centroidalDyn.C_nu+centroidalDyn.g));
            obj.casadi_optimizer.set_value(obj.Jc, centroidalDyn.Jc);
            obj.casadi_optimizer.set_value(obj.Jcmm, Jcmm);
            obj.casadi_optimizer.set_value(obj.nu_k, centroidalDyn.nu);
            obj.casadi_optimizer.set_value(obj.C_friction,C_friction);
            obj.casadi_optimizer.set_value(obj.b_friction,b_friction);
            obj.casadi_optimizer.set_value(obj.s_dot_star,s_dot_star);
            obj.casadi_optimizer.set_value(obj.H_star,H_star);
            obj.casadi_optimizer.set_value(obj.Jc_dot_nu, centroidalDyn.dJc_nu);
            obj.casadi_optimizer.set_value(obj.tau_meas, tau_meas);
            obj.casadi_optimizer.set_value(obj.f_meas, f_meas);
            obj.casadi_optimizer.set_value(obj.s,s);
            obj.casadi_optimizer.set_value(obj.CoM_measured,CoM_measured)
            obj.casadi_optimizer.set_value(obj.K_friction,K_friction_new);
            obj.casadi_optimizer.set_value(obj.t_step,tStep);
            obj.casadi_optimizer.set_value(obj.CoM_max, CoM_limits(:,2));
            obj.casadi_optimizer.set_value(obj.CoM_min, CoM_limits(:,1));
            obj.casadi_optimizer.set_value(obj.CoM_vel_max, CoM_vel_limits(:,2));
            obj.casadi_optimizer.set_value(obj.CoM_vel_min, CoM_vel_limits(:,1));
            obj.casadi_optimizer.set_value(obj.epsilon_CoM, epsilon_CoM);
            obj.casadi_optimizer.set_value(obj.ActivateComHeightConstraint,activateCoMHeightTask);
            obj.casadi_optimizer.set_value(obj.CoM_Z_reference_velocity, CoMZReferenceVelocity);
            obj.casadi_optimizer.set_value(obj.feet_correction, correctionFeet);
            obj.casadi_optimizer.set_value(obj.holonomicConstraintActivation, activateHolonomicConstraint);
            obj.casadi_optimizer.set_value(obj.JTorso,JTorso);
            obj.casadi_optimizer.set_value(obj.J_com,J_com);
%             obj.casadi_optimizer.set_value(obj.SaturationMaxJoints,jointsVelocitySat(:,2));
%             obj.casadi_optimizer.set_value(obj.SaturationMinJoints, jointsVelocitySat(:,1));

            % Computing the solution
            try
                obj.sol = obj.casadi_optimizer.solve; % could raise an error
                v_b_star = full(obj.sol.value(obj.v_b_k_1));
                s_dot_k_1_star = full(obj.sol.value(obj.s_dot_k_1));
                tau_star = full(obj.sol.value(obj.tau_k));
                f_star = full(obj.sol.value(obj.f_k));
                obj.casadi_optimizer.set_initial(obj.casadi_optimizer.x, obj.sol.value(obj.casadi_optimizer.x));
            catch exception
                try
                    solver_succeded = false;
                    obj.casadi_optimizer.debug.show_infeasibilities;
                    disp(exception.getReport)
                catch
                    disp(exception.message)
                end
            end

            % If the solver did not succed, put to zero the control input
            % and increase the related counter
            if(~solver_succeded)
                obj.solver_fails_counter = obj.solver_fails_counter +1;
                v_b_star = zeros(6,1);
                s_dot_k_1_star = zeros(26,1);
                tau_star = zeros(26,1);
                f_star = zeros(12,1);
                if(obj.solver_fails_counter>6)
                    error('solver failed more that 4 times');
                end
            else
                obj.solver_fails_counter = 0;

            end

        end

        function resetImpl(obj)
            % Initialize discrete-state properties.
        end
    end
end


function visual_scalpPlot(SMT, CNT, varargin)
% Description:
%   Draw  scalp topographies for all selected intervals,separately for each each class.
%   Scalp topographies of each classe are plotted in one row, and shared the same color map
%   scaling in each classes.
%
% Example Code:
%    visual_scalpPlot(SMT,CNT, {'Ival' , [start interval : time increase parameter: end intercal]});
%
% Input:
%   visual_scalpPlot(SMT,CNT, <OPT>);
%   SMT: Data structrue (ex) Epoched data structure
%   CNT: Continuous data structure
%
% Option:
%      .Ival - Selecting the interested time interval depending on time increase parameter
%                 (e.g. {'Ival' [ [-2000 : 1000: 4000]])
%
% Return:
%   Scalp topographies
%
% See also:
%    opt_getMontage, opt_cellToStruct
%
% Ji Hoon, Jeong
% jh_jeong@korea.ac.kr

%%
MNT = opt_getMontage(SMT);
opt = opt_cellToStruct(varargin{:});
opt.Interval = abs(opt.Ival(1)-opt.Ival(2));
for i = 1: size(SMT.class,1)
    for seg = 1: size(opt.Ival, 2)-1
        %     figure()
        subplot(size(SMT.class,1),size(opt.Ival,2)-1,((i-1)*(size(opt.Ival, 2)-1))+seg)

        center = [0 0];
        theta = linspace(0,2*pi,360);

        x = cos(theta)+center(1);
        y = sin(theta)+center(2);

        oldUnit = get(gcf,'units');
        set(gcf,'units','normalized');

        H = struct('ax', gca);
        set(gcf,'CurrentAxes',H.ax);

        % ----------------------------------------------------------------------
        % nose plot
        nose = [1 1.2 1];
        nosi = [83 90 97]+1;
        H.nose = plot(nose.*x(nosi), nose.*y(nosi), 'k', 'linewidth', 2.4 );

        hold on;

        % ----------------------------------------------------------------------
        % ears plot
        earw = .08; earh = .3;
        H.ears(1) = plot(x*earw-1-earw, y*earh, 'k',  'linewidth', 2);
        H.ears(2) = plot(x*earw+1+earw, y*earh, 'k',  'linewidth', 2);
        hold on;

        % ----------------------------------------------------------------------
        % main circle plot
        H.main = plot(x,y, 'k', 'linewidth', 2.2);
        set(H.ax, 'xTick',[], 'yTick',[]);
        axis('xy', 'tight', 'equal', 'tight');
        hold on;
        %         if (i == 1 && seg == 1)
        %             ylabel([SMT.class{i,2},' class'])
        %             hold on;
        %         elseif (i == 2 && seg == 1)
        %             ylabel([SMT.class{i,2},' class'])
        %             hold on;
        %         elseif (i == 3 && seg == 1)
        %             ylabel([SMT.class{i,2},' class'])
        %             hold on;
        %         elseif (i == 4 && seg == 1)
        %             ylabel([SMT.class{i,2},' class'])
        %             hold on;
        %         end
        % ----------------------------------------------------------------------
        % Rendering the contourf
        xe_org = MNT.x';
        ye_org = MNT.y';
        avgSMT= prep_average(SMT);

        SMTintervalstart = find(avgSMT.ival == opt.Ival(seg));
        SMTintervalEnd = SMTintervalstart+opt.Interval/10;
        UpdatedSMT{seg} = avgSMT.x(SMTintervalstart:SMTintervalEnd,:,:);
        w{seg,i} = UpdatedSMT{seg}(:,i,:);
        w{seg,i} = squeeze(w{seg,i});
        inputx{seg,i}  = mean(w{seg,i},1);

        % w = w(:);
        % resolution = 101;
        resolution = 500;

        maxrad = max(1,max(max(abs(MNT.x)),max(abs(MNT.y))));

        xx = linspace(-maxrad, maxrad, resolution);
        yy = linspace(-maxrad, maxrad, resolution)';

        xe_add = cos(linspace(0,2*pi,resolution))'*maxrad;
        ye_add = sin(linspace(0,2*pi,resolution))'*maxrad;
        w_add = ones(length(xe_add),1)*mean(inputx{seg,i});

        xe = [xe_org; xe_add];
        ye = [ye_org; ye_add];
        inputx{seg,i} = [inputx{seg,i}'; w_add];

        % xe_add = cos(linspace(0,2*pi,resolution))';
        % ye_add = sin(linspace(0,2*pi,resolution))';
        %
        % xe = [xe;xe_add];
        % ye = [ye;ye_add];
        % w = [w; zeros(length(xe_add),1)];

        [xg,yg,zg] = griddata(xe, ye, inputx{seg,i}, xx, yy);
        contourf(xg, yg, zg, 50, 'LineStyle','none'); hold on;

        % ----------------------------------------------------------------------
        % disp electrodes
        for j = 1:size(xe_org,1)
            plot(xe_org(j), ye_org(j), 'k*'); hold on;
            set(0,'defaultfigurecolor',[1 1 1])

        end

        axis off;
        title({[SMT.class{i,2},' class'];['[' , num2str(opt.Ival(seg)), ' ~ ' , num2str(opt.Ival(seg+1)) , '] ms']})
        %         colorbar('vert');
    end

end

end


% Codes for CVPR-15 work `Face Alignment by Coarse-to-Fine Shape Searching'
% Any question please contact Shizhan Zhu: zhshzhutah2@gmail.com
% Released on July 25, 2015

function y_hat = useLR(reg_model,X)
%y_hat = useLR(reg_model,X)
%   X: m * n
%   reg_model: (n+1)*ny
%   y: m * ny

m = size(X,1);
X = [X ones(m,1)];
y_hat = X * reg_model;

end


function Connect3D(p1,p2,option,pt)

h = plot3([p1(1) p2(1)],[p1(2) p2(2)],[p1(3) p2(3)],option);
set(h,'LineWidth',pt)