function soln = optimTraj(problem)
% soln = optimTraj(problem)
%
% Solves a trajectory optimization problem.
%
% INPUT: "problem" -- struct with fields:
%
%   func -- struct for user-defined functions, passed as function handles
%
%       Input Notes:  square braces show size:  [a,b] = size()
%               t = [1, nTime] = time vector (grid points)
%               x = [nState, nTime] = state vector at each grid point
%               u = [nControl, nTime] = control vector at each grid point
%               t0 = scalar = initial time
%               tF = scalar = final time
%               x0 = [nState, 1] = initial state
%               xF = [nState, 1] = final state
%
%       dx = dynamics(t,x,u)
%               dx = [nState, nTime] = dx/dt = derivative of state wrt time
%
%       dObj = pathObj(t,x,u)
%               dObj = [1, nTime] = integrand from the cost function
%
%       obj = bndObj(t0,x0,tF,xF)
%               obj = scalar = objective function for boundry points
%
%       [c, ceq] = pathCst(t,x,u)
%               c = column vector of inequality constraints  ( c <= 0 )
%               ceq = column vector of equality constraints ( c == 0 )
%
%       [c, ceq] = bndCst(t0,x0,tF,xF)
%               c = column vector of inequality constraints  ( c <= 0 )
%               ceq = column vector of equality constraints ( c == 0 )
%
%       How to pass parameters to your functions:
%           - suppose that your dynamics function is pendulum.m and it
%           accepts a struct of parameters p. When you are setting up the
%           problem, define the struc p in your workspace and then use the
%           following command to pass the function:
%               problem.func.dynamics = @(t,x,u)( pendulum(t,x,u,p) );
%
%       Analytic Gradients:
%           Both the "trapezoid" and "hermiteSimpson" methods in OptimTraj
%       support analytic gradients. Type help "trapezoid" or help
%       "hermiteSimpson" for details on how to pass gradients to the
%       transcription method.
%
%
%   bounds - struct with bounds for the problem:
%
%       .initialTime.low = [scalar]
%       .initialTime.upp = [scalar]
%
%       .finalTime.low = [scalar]
%       .finalTime.upp = [scalar]
%
%       .state.low = [nState,1] = lower bound on the state
%       .state.upp = [nState,1] = lower bound on the state
%
%       .initialState.low = [nState,1]
%       .initialState.upp = [nState,1]
%
%       .finalState.low = [nState,1]
%       .finalState.upp = [nState,1]
%
%       .control.low = [nControl, 1]
%       .control.upp = [nControl, 1]
%
%
%
%   guess - struct with an initial guess at the trajectory
%
%       .time = [1, nGridGuess]
%       .state = [nState, nGridGuess]
%       .control = [nControl, nGridGuess]
%
%   options = options for the transcription algorithm (this function)
%
%       .nlpOpt = options to pass through to fmincon
%
%       .method = string to pick which method is used for transcription
%           'trapezoid'
%           'hermiteSimpson'
%           'chebyshev'
%           'rungeKutta'
%           'gpops'     ( Must have GPOPS2 installed )
%
%       .[method] = a struct to pass method-specific parameters. For
%       example, to pass the number of grid-points to the trapezoid method,
%       create a field .trapezoid.nGrid = [number of grid-points].
%
%       .verbose = integer
%           0 = no display
%           1 = default
%           2 = display warnings, overrides fmincon display setting
%           3 = debug
%
%       .defaultAccuracy = {'low','medium','high'}
%           Sets the default options for each transcription method
%
%       * if options is a struct array, the optimTraj will run the optimization
%       by running options(1) and then using the result to initialize a new
%       solve with options(2) and so on, until it runs options (end). This
%       allows for successive grid and tolerance opdates.
%
%
%
%
%
% OUTPUT: "soln"  --  struct with fields:
%
%   .grid = trajectory at the grid-points used by the transcription method
%       .time = [1, nTime]
%       .state = [nState, nTime]
%       .control = [nControl, nTime];
%
%   .interp = functions for interpolating state and control for arbitrary
%       times long the trajectory. The interpolation method will match the
%       underlying transcription method. This is particularily important
%       for high-order methods, where linear interpolation between the
%       transcription grid-points will lead to large errors. If the
%       requested time is not on the trajectory, the interpolation will
%       return NaN.
%
%       .state = @(t) = given time, return state
%           In: t = [1,n] vector of time
%           Out: x = [nState,n] state vector at each point in time
%
%       .control = @(t) = given time, return control
%           In: t = [1,n] vector of time
%           Out: u = [nControl,n] state vector at each point in time
%
%   .info = information about the optimization run
%       .nlpTime = time (seconds) spent in fmincon
%       .exitFlag = fmincon exit flag
%       .objVal = value of the objective function
%       .[all fields in the fmincon "output" struct]
%
%   .problem = the problem as it was passed to the low-level transcription,
%       including the all default values that were used
%
%
%   * If problem.options was a struct array, then soln will also be a
%   struct array, with soln(1) being the solution on the first iteration,
%   and soln(end) being the final solution.
%
%   * Both the trapezoid and hermiteSimpson method support additional
%   features. The first is that they can use analytic gradients, if
%   provided by the user. ">> help trapezoid" or ">> help hermiteSimpson"
%   for more information. These methods additionally provide error
%   estimates in two forms. The continuous collocation constraint error is
%   provided as an additional function handle (soln.interp.collCst), and
%   the absolute local error estimate for each segment is provided in
%   soln.info.error.
%

problem = inputValidation(problem);   %Check inputs
problem = getDefaultOptions(problem); % Complete options struct

% Loop over the options struct to solve the problem
nIter = length(problem.options);
soln(nIter) = struct('grid',[],'interp',[],'info',[],'problem',[]);
P = problem;  %Temp variable for passing on each iteration
for iter=1:nIter
    P.options = problem.options(iter);

    if P.options.verbose > 0    %then print out iteration count:
        disp('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~');
        disp(['Running OptimTraj, iteration ' num2str(iter)]);
    end

    if iter > 1  %Use previous soln as new guess
        P.guess = soln(iter-1).grid;
    end

    %%%% This is the key part: call the underlying transcription method:
    switch P.options.method
        case 'trapezoid'
            soln(iter) = trapezoid(P);
        case 'hermiteSimpson'
            soln(iter) = hermiteSimpson(P);
        case 'chebyshev'
            soln(iter) = chebyshev(P);
        case 'multiCheb'
            soln(iter) = multiCheb(P);
        case 'rungeKutta'
            soln(iter) = rungeKutta(P);
        case 'gpops'
            soln(iter) = gpopsWrapper(P);
        otherwise
            error('Invalid method. Type: ''help optimTraj'' for a valid list.');
    end

end
end


function pass = test_sum3(pref)
% Test file for @chebfun3/sum3.m.

% Obtain preferences.
if ( nargin == 0 )
    pref = chebfunpref();
end
tol = 1e4 * pref.cheb3Prefs.chebfun3eps;

% Check empty case
pass(1) = isempty(sum3(chebfun3()));

% Check constant
f = chebfun3(@(x,y,z) 1);
pass(2) = abs(sum3(f) - 8) < tol;

% Runge function
f = cheb.gallery3('runge');
pass(3) = abs(sum3(f) - 4.28685406230184188268) < tol;

% Different domains
f = chebfun3(@(x,y,z) x, [0, 1, 0, 2, 0, 3]);
pass(4) = abs(sum3(f) - 3) < tol;

f = chebfun3(@(x,y,z) y, [0, 1, 0, 2, 0, 3]);
pass(5) = abs(sum3(f) - 6) < tol;

f = chebfun3(@(x,y,z) z, [0, 1, 0, 2, 0, 3]);
pass(6) = abs(sum3(f) - 9) < tol;

end


function datr = ksFilter(buff, ops)

if ~isfield(ops, 'b1')
    if isfield(ops,'fslow')&&ops.fslow<ops.fs/2
        [b1, a1] = butter(3, [ops.fshigh/ops.fs,ops.fslow/ops.fs]*2, 'bandpass');
    else
        [b1, a1] = butter(3, ops.fshigh/ops.fs*2, 'high');
    end
else
    b1 = ops.b1; a1 = ops.a1;
end

if ops.GPU
    dataRAW = gpuArray(buff);
else
    dataRAW = buff;
end
dataRAW = dataRAW';
dataRAW = single(dataRAW);
dataRAW = dataRAW(:, ops.chanMap.chanMap);

% subtract the mean from each channel
dataRAW = dataRAW - mean(dataRAW, 1);

datr = filter(b1, a1, dataRAW);
datr = flipud(datr);
datr = filter(b1, a1, datr);
datr = flipud(datr);

% CAR, common average referencing by median
if getOr(ops, 'CAR', 1)
    datr = datr - median(datr, 2);
end


function [ res_map ] = siam_eval( net_match, patches, feat_groups )
%DSIAM_EVAL This function evaluates the score of each positions on the
%search map
% Input:
%       net_match   - matching net
%       patches     - feature groups of the target
%       feat_groups - feature groups of the search image patch
%       sw_sz       - size of the search window [width hight]
% Output:
%       res_map     - response map
% By Xin Li, 2017-12

    for feat_i =1 : numel(feat_groups)

        [x_h, x_w, ~, num_scale] = size(feat_groups{feat_i});
        [z_h, z_w,~, num_scale_1] = size(patches);
        num_scale = max([num_scale,num_scale_1]);
        z_bh = floor(z_h/2); z_bw = floor(z_w/2);
        res_tmp = zeros(x_h, x_w, num_scale,'like',feat_groups{feat_i});

        net_match.eval({'w_feat', patches,'x_feat', feat_groups{feat_i}});
        res_tmp(z_bh+1:z_bh+x_h-z_h+1, z_bw+1:z_bw+x_w-z_w+1,:) = ...
            squeeze( net_match.vars(net_match.getVarIndex('m_score')).value);
        res_ = reshape(gather(res_tmp),x_h,x_w,[]);

        res_map = res_/max(res_(:));

    end

end


function Results = wrapper_FMINCON(P,X,Xlow,Xupp)

%Function Handles:
objfunc = @(x)Objective_Function(x,P);
nonlcon = @(x)NonLinCon(x,P);

%No linear constraints:
A=[]; Aeq=[]; B=[]; Beq=[];

%Store things in a Problem struct:
  Problem.objective = objfunc;
  Problem.x0 = convertVecStruct(X,P.structDat);
  Problem.Aineq = A;
  Problem.bineq = B;
  Problem.Aeq = Aeq;
  Problem.beq = Beq;
  Problem.lb = convertVecStruct(Xlow,P.structDat);
  Problem.ub = convertVecStruct(Xupp,P.structDat);
  Problem.nonlcon = nonlcon;
  Problem.options = P.options;
  Problem.solver = 'fmincon';

%Run fmincon:
tic
  [xSoln, Fval, ExitFlag, Output] = fmincon(Problem);
cpuTime = toc;

%Get the solution struct:
Xsoln = convertVecStruct(xSoln,P.structDat);

%Store the solver output:
name = 'fmincon';
input = makeStruct(name, Problem);
output = makeStruct(xSoln, Fval, ExitFlag, Output, cpuTime);
Results = makeStruct(P,input,output,Xsoln);

end


function [Forecasteval]=TVEmafeval(data_endo_a,data_endo_c,data_endo_c_lags,data_exo_c,data_exo_c_lags,stringdates3,Fstartdate,Fcenddate,Fcperiods,Fcomp,const,n,m,p,k1,k3,It,Bu,beta_gibbs,sigma_gibbs,forecast_record,forecast_estimates,names,endo,pref,theta_gibbs,TVEHfuture,ss_record,indH)

Forecasteval.RMSE=[];
Forecasteval.MAE=[];
Forecasteval.MAPE=[];
Forecasteval.Ustat=[];
Forecasteval.CRPS_estimates=[];
Forecasteval.S1_estimates=[];
Forecasteval.S2_estimates=[];

% first, note that forecast evaluation can only be conducted if there is some observable data after the beginning of the forecast
if Fcomp==1

   % preliminary task: obtain a matrix of forecasts over the common periods
   for ii=1:n
   forecast_c(:,ii)=forecast_estimates{ii,1}(2,1:Fcperiods)';
   end
   % then compute the matrix of forecast errors
   ferrors=data_endo_c-forecast_c;

   % compute first the sequential RMSE, defined in (a.8.11)

   % square the forecast error matrix entrywise
   sferrors=ferrors.^2;
   % sum entries sequentially
   sumsferrors=sferrors(1,:);
   for ii=2:Fcperiods
   sumsferrors(ii,:)=sumsferrors(ii-1,:)+sferrors(ii,:);
   end
   % divide by the number of forecast periods and take square roots to obtain RMSE
   for ii=1:Fcperiods
   Forecasteval.RMSE(ii,:)=((1/ii)*sumsferrors(ii,:)).^0.5;
   end

   % compute then the sequential MAE, defined in (a.8.12)

   % take the absolute value of the forecast error matrix
   absferrors=abs(ferrors);
   % sum entries sequentially
   sumabsferrors=absferrors(1,:);
   for ii=2:Fcperiods
   sumabsferrors(ii,:)=sumabsferrors(ii-1,:)+absferrors(ii,:);
   end
   % divide by the number of forecast periods to obtain MAE
   for ii=1:Fcperiods
   Forecasteval.MAE(ii,:)=(1/ii)*sumabsferrors(ii,:);
   end

   % compute the sequential MAPE, defined in (a.8.13)

   % divide entrywise by actual values and take absolute values
   absratioferrors=abs(ferrors./data_endo_c);
   % sum entries sequentially
   sumabsratioferrors=absratioferrors(1,:);
   for ii=2:Fcperiods
   sumabsratioferrors(ii,:)=sumabsratioferrors(ii-1,:)+absratioferrors(ii,:);
   end
   % divide by 100*(number of forecast periods) to obtain MAPE
   for ii=1:Fcperiods
   Forecasteval.MAPE(ii,:)=(100/ii)*sumabsratioferrors(ii,:);
   end

   % compute the Theil's inequality coefficient, defined in (a.8.14)

   % first compute the left term of the denominator
   % square entrywise the matrix of actual data
   sendo=data_endo_c.^2;
   % sum entries sequentially
   sumsendo=sendo(1,:);
   for ii=2:Fcperiods
   sumsendo(ii,:)=sumsendo(ii-1,:)+sendo(ii,:);
   end
   % divide by the number of forecast periods and take square roots
   for ii=1:Fcperiods
   leftterm(ii,:)=((1/ii)*sumsendo(ii,:)).^0.5;
   end
   % then compute the right term of the denominator
   % square entrywise the matrix of forecast values
   sforecasts=forecast_c.^2;
   % sum entries sequentially
   sumsforecasts=sforecasts(1,:);
   for ii=2:Fcperiods
   sumsforecasts(ii,:)=sumsforecasts(ii-1,:)+sforecasts(ii,:);
   end
   % divide by the number of forecast periods and take square roots
   for ii=1:Fcperiods
   rightterm(ii,:)=((1/ii)*sumsforecasts(ii,:)).^0.5;
   end
   % finally, compute the U stats
   Forecasteval.Ustat=Forecasteval.RMSE./(leftterm+rightterm);

   % now compute the continuous ranked probability score

   % create the cell storing the results
   CRPS=cell(n,1);
   % loop over endogenous variables
   for ii=1:n
      % loop over forecast periods on which actual data is known
      for jj=1:Fcperiods
      % compute the continuous ranked probability score
      score=bear.crps(forecast_record{ii,1}(:,jj),forecast_estimates{ii,1}(2,jj));
      CRPS{ii,1}(1,jj)=score;
      end
   end
   Forecasteval.CRPS_estimates=(cell2mat(CRPS))';

   % finally, consider the big piece: the computation of the log predictive score
   % this part implements algorithm a.8.1, described p 115 of technical guide
   % details of the procedure can be found p 111-115 of the technical guide

   % preliminary task: if there is a constant in the model, concatenate a column of ones to data_exo_c
   if const==1
   data_exo_c=[ones(Fcperiods,1) data_exo_c];
   data_exo_c_lags=[ones(p,1) data_exo_c_lags];
   end

   % create the cells storing the results
   S1=cell(n,1);
   S2=cell(n,1);

   for ll=1:It-Bu
      % step 2: draw beta, Delta and sigma
      beta=beta_gibbs(:,ll);
      sigma=reshape(sigma_gibbs(:,ll),n,n);
      % reshape beta to obtain B
      B=reshape(beta,k1,n);
      theta=theta_gibbs(:,ii);
      tempeq = [];
      for nnn = 1:n
        tempeq = [tempeq ss_record{nnn,1}(ii,end-p+1:end)'];
      end

      % step 3: obtain mu, the mean vector
      temp=bear.TVEmaforecastsim(data_endo_a,B,p,n,Fcperiods,theta,TVEHfuture,tempeq, indH);
      mu=reshape(temp',n*Fcperiods,1);

      % step 4: obtain upsilon, the covariance matrix
      % recover the A1,A2,... matrices by reshaping beta
      temp=B';
      Amatrices=cell(1,p);
      for ii=1:p
      Amatrices{1,ii}=temp(:,n*(ii-1)+1:n*ii);
      end
      % initiate upsilon with upsilon_1,1
      upsilon=cell(Fcperiods,Fcperiods);
      upsilon{1,1}=sigma;
      % complete the first column  using (a.8.23)
      for ii=2:Fcperiods
      upsilon_ij=Amatrices{1,1}*upsilon{ii-1,1};
      summ=min(ii-1,p);
         for jj=2:summ
         upsilon_ij=upsilon_ij+Amatrices{1,jj}*upsilon{ii-jj,1};
         end
      upsilon{ii,1}=upsilon_ij;
      end
      % complete the first row, using symmetry
      for ii=2:Fcperiods
      upsilon{1,ii}=upsilon{ii,1}';
      end
      % now take care of the other columns
      for jj=2:Fcperiods
      % first complete the variance matrix (the diagonal one), using (a.8.24)
      upsilon_jj=sigma;
      summ=min(jj-1,p);
         for kk=1:summ
         upsilon_jj=upsilon_jj+Amatrices{1,kk}*upsilon{jj-kk,jj};
         end
      upsilon{jj,jj}=upsilon_jj;
      % then complete the rest of the column, using (a.8.23)
         for ii=jj+1:Fcperiods
         % initiate upsilon_ij
         upsilon_ij=Amatrices{1,1}*upsilon{ii-1,jj};
         % continue the summation
         summ=min(ii-1,p);
            for kk=2:summ
            upsilon_ij=upsilon_ij+Amatrices{1,kk}*upsilon{ii-kk,jj};
            end
         upsilon{ii,jj}=upsilon_ij;
         end
         % complete the corresponding row, using symmetry
         for kk=jj+1:Fcperiods
         upsilon{jj,kk}=upsilon{kk,jj}';
         end
      end
      upsilon=cell2mat(upsilon);

      % step 5: obtain the predictive density

      % first consider scenario 1 (forecast at period T+i)
      % loop over variables
      for ii=1:n
         % loop over periods
         for jj=1:Fcperiods
         % define R
         R=zeros(1,n*Fcperiods);
         R(1,n*(jj-1)+ii)=1;
         % define the mean vector
         mean=R*mu;
         % define the covariance matrix
         covar=R*upsilon*(R');
         % define the vector of actual values
         actual=data_endo_c(jj,ii);
         % determine the density
         [~,density]=bear.mndensity(actual,mean,covar,1);
         % record the result
         S1{ii,1}(ll,jj)=density;
         end
      end
      % then consider scenario 2 (forecast from beginning until period T+i)
      % loop over variables
      for ii=1:n
      R=[];
         % loop over periods
         for jj=1:Fcperiods
         % define R
         R=[R;zeros(1,n*Fcperiods)];
         R(jj,n*(jj-1)+ii)=1;
         % define the mean vector
         mean=R*mu;
         % define the covariance matrix
         covar=R*upsilon*(R');
         % define the vector of actual values
         actual=data_endo_c(1:jj,ii);
         % determine the density
         [~,density]=bear.mndensity(actual,mean,covar,jj);
         % record the result
         S2{ii,1}(ll,jj)=density;
         end
      end
   end

   % step 6: compute the log predictive score from (XXX.11)
   Forecasteval.S1_estimates=cell(n,1);
   Forecasteval.S2_estimates=cell(n,1);
   for ii=1:n
      for jj=1:Fcperiods
      Forecasteval.S1_estimates{ii,1}(1,jj)=log((1/(It-Bu))*sum(S1{ii,1}(:,jj)));
      Forecasteval.S2_estimates{ii,1}(1,jj)=log((1/(It-Bu))*sum(S2{ii,1}(:,jj)));
      end
   end
   Forecasteval.S1_estimates=(cell2mat(Forecasteval.S1_estimates))';
   Forecasteval.S2_estimates=(cell2mat(Forecasteval.S2_estimates))';

% if forecast evaluation is not possible, do not do anything
elseif Fcomp==0
end

% now, print the results and display them

filelocation=fullfile(pref.results_path, [pref.results_sub '.txt']);
fid=fopen(filelocation,'at');

fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

Fevalinfo='Forecast evaluation:';
fprintf('%s\n',Fevalinfo);
fprintf(fid,'%s\n',Fevalinfo);

fprintf('%s\n','');
fprintf(fid,'%s\n','');

% if forecast evaluation is not possible, return a message to signal it

if Fcomp==0

finfo1=['Forecast evaluation cannot be conducted.'];
fprintf('%s\n',finfo1);
fprintf(fid,'%s\n',finfo1);
finfo2=['Forecasts start in ' Fstartdate ', while observable data is available only until ' names{end,1} '.'];
fprintf('%s\n',finfo2);
fprintf(fid,'%s\n',finfo2);
finfo3=['To obtain forecast evaluation, the forecast start date must be anterior to the end of the data set.'];
fprintf('%s\n',finfo3);
fprintf(fid,'%s\n',finfo3);

% if forecast evaluation is possible, display the results
elseif Fcomp==1

finfo1=['Evaluation conducted over ' num2str(Fcperiods) ' periods (from ' Fstartdate ' to ' Fcenddate ').'];
fprintf('%s\n',finfo1);
fprintf(fid,'%s\n',finfo1);

   % loop over endogenous variables
   for ii=1:n

   fprintf('%s\n','');
   fprintf(fid,'%s\n','');

   endoinfo=['Endogenous: ' endo{ii,1}];
   fprintf('%s\n',endoinfo);
   fprintf(fid,'%s\n',endoinfo);

   temp='fprintf(''%12s';
      for jj=1:Fcperiods-1
      temp=[temp ' %10s'];
      end
   temp=[temp ' %10s\n'','''''];
      for jj=1:Fcperiods
      temp=[temp ',''' stringdates3{jj,1} ''''];
      end
   temp=[temp ');'];
   eval(temp);
   temp='fprintf(fid,''%12s';
      for jj=1:Fcperiods-1
      temp=[temp ' %10s'];
      end
   temp=[temp ' %10s\n'','''''];
      for jj=1:Fcperiods
      temp=[temp ',''' stringdates3{jj,1} ''''];
      end
   temp=[temp ');'];
   eval(temp);

   label='RMSE:       ';
   values=Forecasteval.RMSE(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

   label='MAE:        ';
   values=Forecasteval.MAE(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

   label='MAPE:       ';
   values=Forecasteval.MAPE(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

   label='Theil''s U:  ';
   values=Forecasteval.Ustat(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

   label='CRPS:       ';
   values=Forecasteval.CRPS_estimates(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

   label='Log score 1:';
   values=Forecasteval.S1_estimates(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

   label='Log score 2:';
   values=Forecasteval.S2_estimates(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

    end

end

fclose(fid);


Iref=(im2double(imread('test.jpg')));
tau=[0.2,0.3,0.1,0.2,200,100];
% tau=[0,0,0,0,50,100];
It=Iref;
for i=1:3
It(:,:,i)=warpImg(Iref(:,:,i),tau);
end
It=1.4*abs(It+0.0001)+0.1;
imshow([Iref,It])

tic
[ImTrans,tau,ImTrans_c] = align_c(It,Iref, zeros(6,1),2);
toc;
imshow([It,Iref,ImTrans,ImTrans_c,10*abs(ImTrans_c-Iref)])


classdef STFTNode < GraphNode
    properties
        fft_len=512;
        frame_len=400;
        frame_shift=160;
        win_type = 'hamming';
        removeDC = 1;
        doDithering = 1;
    end

    methods
        function obj = STFTNode(dimOut)
            obj = obj@GraphNode('STFT',dimOut);
        end

        function obj = forward(obj,prev_layers)
            input = prev_layers{1}.a;
            [nCh,D2,T,N] = size(input);
            assert(D2==1, sprintf('%s:forward, Error: dimension 2 (%d) is not equal to 1', obj.name, D2));
            input = permute(input, [1 3 4 2]);

            precision = class(gather(input(1)));
            useGPU = IsInGPU(input);

            if N==1
                fft_x = sfft_multi(input',obj.frame_len,obj.frame_shift,obj.fft_len, obj.win_type,obj.removeDC, useGPU, obj.doDithering);
                fft_x = fft_x(1:obj.fft_len/2+1,:,:);
                [d1,d2,d3] = size(fft_x);
                fft_x = reshape(fft_x, d1*d2, d3);
                obj.mask = [];
            else
                [mask, variableLength] = CheckTrajectoryLength(input);
                input2 = PadShortTrajectory(input, mask, 0);
                input2 = reshape(permute(input2, [2 1 3]), T, nCh*N);

                fft_x = sfft_multi(input2,obj.frame_len,obj.frame_shift,obj.fft_len, obj.win_type,obj.removeDC, useGPU, obj.doDithering);
                fft_x = fft_x(1:obj.fft_len/2+1,:,:);
                fft_x = permute(fft_x, [1 3 2]);
                [nBin,nFr,d3] = size(fft_x);
                fft_x = reshape(fft_x, nBin, nFr, nCh, N);
                fft_x = permute(fft_x, [1 3 2 4]);
                fft_x = reshape(fft_x, nBin*nCh, nFr, N);

                % now build a mask for spectrogram
                nSampleChannel = sum(mask==0);
                nSampleChannel = gather(nSampleChannel);
                if useGPU
                    obj.mask = gpuArray.zeros(nFr, N, precision);
                else
                    obj.mask = zeros(nFr, N, precision);
                end
                for i=1:N
                    nFrChannel = enframe_decide_frame_number(nSampleChannel(i), obj.frame_len, obj.frame_shift, 0);
                    obj.mask(nFrChannel+1:end,i) = 1;
                end
            end
            obj.a = permute(fft_x, [1 4 2 3]);
            obj = forward@GraphNode(obj, prev_layers);
        end

        function obj = backward(obj,future_layers, prev_layers)
            % to be implemented
        end

    end

end


function H = deriveBasis(A_equ)
[L,U,P] = lu(A_equ);
[L,U,P] = lu(A_equ');
r = colspaces(L');
AA = L';
H1 = AA(:,r);
H2 = AA(:,setdiff(1:size(AA,2),r));
H = P'*[-H1\H2;speye(size(H2,2))];
function  [indx]=colspaces(A)
indx = [];
for i = 1:size(A,2)
    s = max(find(A(:,i)));
    indx = [indx s];
end
indx = unique(indx);


function [res, runTime] = run_tracker(seqLen, detections)

%% setting parameters for tracking
c_en      = 12;     %% 10 birth cost, has no influence on the result
c_ex      = 9;     %% 10 death cost, has no influence on the result
c_ij      = 0;     %% 0 transition cost
betta     = 0.2;   %% 0.2 betta, increase will have less tracks, for every single detection
max_it    = inf;    %% inf max number of iterations (max number of tracks)
thr_cost  = 15;     %% 18 max acceptable cost for a track (increase it to have more tracks.), for every tracklet 19.8

time_start = tic;
%% Run object/human detector on all frames.
frameNums = 1:seqLen;
dres = greedy_detect_generator(detections, frameNums);

%% Running tracking algorithms
dres_dp_nms   = tracking_dp(dres, c_en, c_ex, c_ij, betta, thr_cost, max_it, 1);
dres_dp_nms.r = -dres_dp_nms.id;
runTime = toc(time_start);

%% save the tracking result
bboxes_tracked = dres2bboxes(dres_dp_nms, numel(frameNums));  %% we are visualizing the "DP with NMS in the lop" results. Can be changed to show the results of DP or push relabel algorithm.
res = saveResTxt(bboxes_tracked);


function [F,feasible] = filter_normball(F_xw,Zmodel,x,w,allw,norm_p,ops,VariableType)

% Creates robustified version of the uncertain set of linear inequalities
% s.t A(w)*x <= b(w) for all norm(w,p)<r

feasible = 1;
if length(F_xw) == 0
    F = [];
    return
end

X = sdpvar(F_xw);

% Some pre-calc. Consider uncertainties not in this class as decision
% variables (i.e. other than the ones we are robustifying)
vars = [getvariables(x) depends(X) getvariables(allw)];
vars = setdiff(vars,getvariables(w));
x = recover(vars);

% Trying to cope with cases as x+norm(w,1)+norm(w,2)<1, s.t norm(w,2)<1
% In test_robust_5
%extended_in_w = intersect(VariableType.w_variables,yalmip('extvariables'));
%extended_in_F = intersect(vars,yalmip('extvariables'));
if ~(strcmp(ops.robust.auxreduce,'none') | strcmp(ops.robust.auxreduce,'affine'))
    if ~isempty(intersect(vars,intersect(VariableType.w_variables,yalmip('extvariables'))))
        F = F_xw;
        return
    end
end

% Create a bilinear decomposition of the constraints
try
xw = [x;w];
xind = find(ismembcYALMIP(getvariables(xw),getvariables(x)));
wind = find(ismembcYALMIP(getvariables(xw),getvariables(w)));
[Qs,cs,fs,dummy,nonquadratic] = vecquaddecomp(X,xw);
all_f = [];
all_c_w = [];
all_c_x = [];
all_Q_xw = [];
HigherOrder = [];
top = 1;
for i = 1:length(X)
    Q = Qs{i};
    c = cs{i};
    f = fs{i};
    if nonquadratic
        HigherOrder = [HigherOrder;i];
        %error('Constraints can be at most quadratic, with the linear term uncertain');
    else
        Q_ww = Q(wind,wind);
        Q_xw = Q(xind,wind);
        Q_xx{top} = Q(xind,xind);
        c_x = c(xind);
        c_w = c(wind);

        all_f = [all_f;f];
        all_c_w = [all_c_w;c_w'];
        all_c_x = [all_c_x;sparse(c_x)];
        all_Q_xw = [all_Q_xw Q_xw'];
        top = top + 1;
    end
end
% Linear uncertain constraint is (Bbetai*x + cdi) >= 0 for all w, or
% (bi' + (Bi*w)')*x + (ci'*w + di).
catch
    % Complicated epression, so go for polynomial decomposition below
    Q = [];
end

% Currently 3 special cases implemented
if ops.verbose
    disp([' - Eliminating uncertainty using explicit maximization of ' norm_p])
end

if length(Q)> 0
    switch norm_p
        case '1-norm'
            [F,feasible] = filter_norm_1(all_f,all_c_w,all_c_x,all_Q_xw,x,Zmodel,allw,X,Q_xx,VariableType);

        case '2-norm'
            [F,feasible] = filter_norm_2(all_f,all_c_w,all_c_x,all_Q_xw,x,Zmodel,allw,X,Q_xx,VariableType);

        case 'inf-norm'
            [F,feasible] = filter_norm_inf(all_f,all_c_w,all_c_x,all_Q_xw,x,Zmodel,allw,X,Q_xx,VariableType);

        otherwise
            error('The detected norm-ball has not been implemented yet')
    end
elseif isequal(norm_p,'inf-norm')
    % Quadratic decomposition failed. Let's try a general basis if simple
    % inf-norm
    F = [];
    feasible = 1;
    HigherOrder = [];
   % [c] = coefficients([1+sum(allw);X],allw);
    for i = 1:length(X)
       % try
            [c,v] = coefficients(X(i),allw,[1;allw]);
            % Great, affine in uncertainty
            if isa(c(2:end),'double') && nnz(c(2:end))==0
                % Trivial case, no uncertainty in model
                F = [F, X(i) >= 0];
            else
                % c(1) +  c(2:end)'*w >= 0
                % c(1) +  c(2:end)'*(center + r*wnormalized) >= 0
                % c(1) +  c(2:end)'*center + r*c(2:end)'*wnormalized >= 0
                center = (Zmodel.lb + Zmodel.ub)/2;
                r = (Zmodel.ub - Zmodel.lb)/2;
                t = sdpvar(length(allw),1);
                F = [F, c(1) + c(2:end)'*center - sum(t) >= 0, -t <= r.*c(2:end) <= t];
            end
      %  catch
      %      HigherOrder = [HigherOrder;i];
      %  end
    end
else
    F = [];
    feasible = 1;
end

if ~isempty(HigherOrder)
    F = [F,X(HigherOrder) >= 0];
end


function [h_x,h_u] = sys_Dh(neq,t,x,u)

global sys_params A B C D

% h_x must be an n by n matrix.
% h_u must be an n by m matrix.

h_u = A+B*(C).*t;

h_x = B;


classdef ShFunc_Volume < ShapeFunctional

    properties (Access = public)
        geometricVolume
    end

    methods (Access = public)

        function obj = ShFunc_Volume(cParams)
            cParams.filterParams.quadratureOrder = 'LINEAR';
            obj.init(cParams);
            obj.geometricVolume = sum(obj.dvolu(:));
        end

        function computeFunctionAndGradient(obj)
            obj.nVariables = obj.designVariable.nVariables;
            obj.updateHomogenizedMaterialProperties();
            obj.computeFunction();
            obj.computeGradient();
        end

        function computeFunction(obj)
            density = obj.homogenizedVariablesComputer.rho;
            obj.computeFunctionFromDensity(density);
        end

        function computeFunctionFromDensity(obj,dens)
            densV = dens;
            volume = (sum(obj.dvolu,2)'*mean(densV,2));
            volume = volume/(obj.geometricVolume);
            obj.value = volume;
        end

        function v = getVariablesToPlot(obj)
            v{1} = obj.value*obj.value0;
        end

        function t = getTitlesToPlot(obj)
            t{1} = 'Volume non scaled';
        end

    end

    methods (Access = private)

        function computeGradient(obj)
            drho = obj.homogenizedVariablesComputer.drho;
            g = drho;
            gf = zeros(size(obj.Msmooth,1),obj.nVariables);
            for ivar = 1:obj.nVariables
                gs = g{ivar}/obj.geometricVolume;
                gf(:,ivar) = obj.filter.getP1fromP0((gs));
            end
            g = obj.Msmooth*gf;
            %g = gf;
            obj.gradient = g(:);
            %obj.gradient = ones(size(g(:)))/obj.geometricVolume;
        end

        function updateHomogenizedMaterialProperties(obj)
            nx = length(obj.designVariable.value)/obj.designVariable.nVariables;
            x  = obj.designVariable.value;
            xf = cell(obj.designVariable.nVariables,1);
            for ivar = 1:obj.nVariables
                i0 = nx*(ivar-1) + 1;
                iF = nx*ivar;
                xs = x(i0:iF);
                xf{ivar} = obj.filter.getP0fromP1(xs);
            end
            obj.homogenizedVariablesComputer.computeDensity(xf);
        end

    end
end


function [post, a] = gtmpost(net, data)
%GTMPOST Latent space responsibility for data in a GTM.
%
%	Description
%	 POST = GTMPOST(NET, DATA) takes a GTM structure NET, and computes
%	the  responsibility at each latent space sample point NET.X for each
%	data point in DATA.
%
%	[POST, A] = GTMPOST(NET, DATA) also returns the activations A of the
%	GMM NET.GMMNET as computed by GMMPOST.
%
%	See also
%	GTM, GTMEM, GTMLMEAN, GMLMODE, GMMPROB
%

%	Copyright (c) Ian T Nabney (1996-2001)

% Check for consistency
errstring = consist(net, 'gtm', data);
if ~isempty(errstring)
  error(errstring);
end

net.gmmnet.centres = rbffwd(net.rbfnet, net.X);

[post, a] = gmmpost(net.gmmnet, data);


%  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 2 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, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
%function tfrwvt
%TFRWVT	Unit test for the function TFRWV.

%	F. Auger, December 1995 - O. Lemoine, March 1996.

N=128;

% Covariance by translation in time
t1=60; t2=70; f=0.3;
sig1=amgauss(N,t1).*fmconst(N,f,t1);
sig2=amgauss(N,t2).*fmconst(N,f,t2);
tfr1=tfrwv(sig1);
tfr2=tfrwv(sig2);
[tr,tc]=size(tfr1);
tfr=tfr1-tfr2(:,modulo((1:tc)-t1+t2,tc));
if any(any(abs(tfr)>sqrt(eps))),
 error('tfrwv test 1 failed');
end

% Reality of the TFR
sig=noisecg(N);
tfr=tfrwv(sig);
if sum(any(abs(imag(tfr))>sqrt(eps)))~=0,
 error('tfrwv test 2 failed');
end

% Energy conservation
sig=noisecg(N);
tfr=tfrwv(sig);
Es=norm(sig)^2;
Etfr=sum(mean(tfr));
if abs(Es-Etfr)>sqrt(eps),
 error('tfrwv test 3 failed');
end

% Time-marginal
sig=noisecg(N);
tfr=tfrwv(sig);
ip1=abs(sig).^2;
ip2=mean(tfr)';
if any(abs(ip1-ip2)>sqrt(eps)),
 error('tfrwv test 4 failed');
end

% Frequency-marginal
sig=noisecg(N);
tfr=tfrwv(sig);
FFT=fft(sig);
psd1=abs(FFT(2:N/2)).^2/(2*N);
psd2=mean(tfr(1:2:N,:)')';
if any(abs(psd1-psd2(2:N/2))>sqrt(eps)),
 error('tfrwv test 5 failed');
end

% Compatibility with filtering
h=amgauss(N,N/2,sqrt(N)).*fmlin(N);
x=amgauss(N).*fmconst(N);
y=conv(x,h); y=y(:);  y=y((N/2+1):3*N/2);
tfrx=tfrwv(x);
tfry=tfrwv(y)/(2*N);
tfrh=tfrwv(h);
tfr=zeros(N);
for f=1:N,
 tmp=conv(tfrx(f,:),tfrh(f,:));
 tfr(f,:)=tmp((N/2+1):3*N/2)/N;
end
if any(any(abs(tfr-tfry)>sqrt(eps))),
 error('tfrwv test 6 failed');
end

% Unitarity
x1=amgauss(N).*fmlin(N);
x2=amgauss(N).*fmsin(N);
tfr1=tfrwv(x1);
tfr2=tfrwv(x2);
cor1=abs(x1'*x2)^2/2;
cor2=sum(sum(tfr1.*conj(tfr2)))/N;
if abs(cor1-cor2)>sqrt(eps),
 error('tfrwv test 7 failed');
end

% Conservation of the time support (wide-sense)
sig=[zeros(N/4,1);fmlin(N/2);zeros(N/4,1)];
tfr=tfrwv(sig);
if sum(any(abs(tfr(:,1:N/4-1))>sqrt(eps))) | ...
   sum(any(abs(tfr(:,(3*N/4+1):N))>sqrt(eps))),
 error('tfrwv test 8 failed');
end

% time localization
t0=30; sig= ((1:N)'==t0);
tfr=tfrwv(sig);
[ik,jk]=find(tfr~=0.0);
if any(jk~=t0)|any(ik'-(1:N)),
 error('tfrwv test 9 failed');
end;

% frequency localization
f0=10;
sig=fmconst(N+6,f0/N);
tfr=tfrwv(sig,N/2+2,N);
if (find(tfr>1/N)~=2*f0+1)|(abs(mean(tfr)-1.0)>2.0*eps),
 error('tfrwv test 10 failed');
end;

N=127;

% Covariance by translation in time
t1=60; t2=70; f=0.3;
sig1=amgauss(N,t1).*fmconst(N,f,t1);
sig2=amgauss(N,t2).*fmconst(N,f,t2);
tfr1=tfrwv(sig1);
tfr2=tfrwv(sig2);
[tr,tc]=size(tfr1);
tfr=tfr1-tfr2(:,modulo((1:tc)-t1+t2,tc));
if any(any(abs(tfr)>sqrt(eps))),
 error('tfrwv test 11 failed');
end

% Reality of the TFR
sig=noisecg(N);
tfr=tfrwv(sig);
if sum(any(abs(imag(tfr))>sqrt(eps)))~=0,
 error('tfrwv test 12 failed');
end

% Energy conservation
sig=noisecg(N);
tfr=tfrwv(sig);
Es=norm(sig)^2;
Etfr=sum(mean(tfr));
if abs(Es-Etfr)>sqrt(eps),
 error('tfrwv test 13 failed');
end

% Time-marginal
sig=noisecg(N);
tfr=tfrwv(sig);
ip1=abs(sig).^2;
ip2=mean(tfr)';
if any(abs(ip1-ip2)>sqrt(eps)),
 error('tfrwv test 14 failed');
end

% Frequency-marginal
sig=noisecg(N);
tfr=tfrwv(sig);
FFT=fft(sig);
psd1=abs(FFT(2:fix(N/2)-3)).^2/(2*N);
psd2=mean(tfr(1:2:N,:)')';
if any(abs(psd1-psd2(2:fix(N/2)-3))>5e-2),
 error('tfrwv test 15 failed');
end

% Compatibility with filtering
h=amgauss(N,N/2,sqrt(N)).*fmlin(N);
x=amgauss(N).*fmconst(N);
y=conv(x,h);  y=y(:); y=y((round(N/2)+1):round(3*N/2));
tfrx=tfrwv(x);
tfry=tfrwv(y)/(2*N);
tfrh=tfrwv(h);
tfr=zeros(N);
for f=1:N,
 tmp=conv(tfrx(f,:),tfrh(f,:));
 tfr(f,:)=tmp((round(N/2)+1):round(3*N/2))/N;
end
if any(any(abs(tfr-tfry)>sqrt(eps))),
 error('tfrwv test 16 failed');
end

% Unitarity
x1=amgauss(N).*fmlin(N);
x2=amgauss(N).*fmsin(N);
tfr1=tfrwv(x1);
tfr2=tfrwv(x2);
cor1=abs(x1'*x2)^2/2;
cor2=sum(sum(tfr1.*conj(tfr2)))/N;
if abs(cor1-cor2)>sqrt(eps),
 error('tfrwv test 17 failed');
end

% Conservation of the time support (wide-sense)
sig=[zeros(round(N/4),1);fmlin(round(N/2));zeros(round(N/4),1)];
tfr=tfrwv(sig);
if sum(any(abs(tfr(:,1:round(N/4)-1))>sqrt(eps))) | ...
   sum(any(abs(tfr(:,(round(3*N/4)+2):N))>sqrt(eps))),
 error('tfrwv test 18 failed');
end

% time localization
t0=30; sig= ((1:N)'==t0);
tfr=tfrwv(sig);
[ik,jk]=find(tfr~=0.0);
if any(jk~=t0)|any(ik'-(1:N)),
 error('tfrwv test 19 failed');
end;

% frequency localization
f0=10;
sig=fmconst(N+6,f0/N);
tfr=tfrwv(sig,round(N/2)+2,N);
if (find(tfr>1/N)~=2*f0+1)|(abs(mean(tfr)-1.0)>sqrt(eps)),
 error('tfrwv test 20 failed');
end;


% Copyright (C) 2017-2018 Titus Cieslewski, RPG, University of Zurich,
%   Switzerland
%   You can contact the author at <titus at ifi dot uzh dot ch>
% Copyright (C) 2017-2018 Siddharth Choudhary, College of Computing,
%   Georgia Institute of Technology, Atlanta, GA, USA
% Copyright (C) 2017-2018 Davide Scaramuzza, RPG, University of Zurich,
%   Switzerland
%
% This file is part of dslam_open.
%
% dslam_open 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.
%
% dslam_open 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 dslam_open. If not, see <http://www.gnu.org/licenses/>.

function result = areMatchesConsistent(all_robot_data, query_robot, ...
    match_a, match_b, position_tolerance)
% Basically make sure that
% T_M1_Q1 * T_Q1_Q2 is similar to T_M1_M2 * T_M2_Q2, i.e.
% that they are within tolerance.
T_M1_Q1 = match_a.match.Sim_M_Q;
T_M2_Q2 = match_b.match.Sim_M_Q;
% T_Q1_Q2
T_O_Q1 = all_robot_data{query_robot}.Sim_O_C{match_a.frame_i};
T_O_Q2 = all_robot_data{query_robot}.Sim_O_C{match_b.frame_i};
T_Q1_Q2 = tInv(T_O_Q1) * T_O_Q2;
% T_M1_M2
match_robot = match_a.match.robot_i;
assert(match_robot == match_b.match.robot_i);
T_O_M1 = all_robot_data{match_robot}.Sim_O_C{match_a.match.frame_i};
T_O_M2 = all_robot_data{match_robot}.Sim_O_C{match_b.match.frame_i};
T_M1_M2 = tInv(T_O_M1) * T_O_M2;

T_M1_Q2_1 = T_M1_Q1 * T_Q1_Q2;
T_M1_Q2_2 = T_M1_M2 * T_M2_Q2;

error = tInv(T_M1_Q2_1) * T_M1_Q2_2;

result = norm(error(1:3, 4)) < position_tolerance;

end


function [V,E,P,BE,CE,PE] = readTGF(filename)
  % READTGF
  %
  % [V,E,P,BE,CE,PE] = readTGF(filename)
  %
  % Read a graph from a .tgf file
  %
  % Input:
  %  filename  .tgf file name
  % Ouput:
  %  V  # vertices by 3 list of vertex positions
  %  E  # edges by 2 list of edge indices
  %  P  # point-handles list of point handle indices
  %  BE # bone-edges by 2 list of bone-edge indices
  %  CE # cage-edges by 2 list of cage-edge indices
  %  PE # pseudo-edges by 2 list of pseudo-edge indices
  %
  % Assumes that graph vertices are 3 dimensional

  V = [];
  E = [];
  BE = [];
  CE = [];
  PE = [];
  fp = fopen(filename,'r');
  % read next whole line
  line = fscanf(fp,' %[^\n]s');
  % read vertices until seeing separator
  while(line(1,1) ~= '#')
    [v,count] = sscanf(line,'%d %g %g %g',4);
    % first number is always index
    v = v(2:min(count,4));
    V = [V;v'];
    line = fscanf(fp,' %[^\n]s');
  end
  % read edges until file is through
  line = fscanf(fp,' %[^\n]s');
  while(sum(size(line)) > 0 && line(1,1) ~= '#')
    [e,count] = sscanf(line,'%d %d %d %d %d',5);
    assert(count >= 2);
    % add edge
    % .tgf format is 1-indexed
    E = [E;e(1:2)'];
    if count >= 3 && e(3)
      BE = [BE;e(1:2)'];
    end
    % bone edges trump pseudo-edges
    if count >= 4 && e(4) && ~e(3)
      PE = [PE;e(1:2)'];
    end
    % bone edges trump cage edges
    if count >= 5 && e(5) && ~e(3)
      CE = [CE;e(1:2)'];
    end
    line = fscanf(fp,' %[^\n]s');
  end

  % point handles are points not attached to a bone
  P = 1:size(V,1);
  P = P(~ismember(P,BE(:)));
  fclose(fp);
end


classdef prtClassLogisticDiscriminant < prtClass & prtClassBig
    % prtClassLogisticDiscriminant  Logistic Discriminant classifier
    %
    %    CLASSIFIER = prtClassLogisticDiscriminant returns a LogisticDiscriminant classifier
    %
    %    CLASSIFIER = prtClassLogisticDiscriminant(PROPERTY1, VALUE1, ...) constructs a
    %    prtClassLogisticDiscriminant object CLASSIFIER with properties as specified by
    %    PROPERTY/VALUE pairs.
    %
    %    A prtClassLogisticDiscriminant object inherits all properties from the abstract class
    %    prtClass. In addition is has the following properties:
    %
    %   wTolerance       - The convergance tolerance of the weights
    %   irlsStepSize     - Step size used in training. Can be set to a
    %                      double, or 'hessian'. If 'hessian', IRLS is
    %                      solved using the Hessian to estimate steps.
    %   maxIter          - maximum IRLS iterations
    %   nIterations      - number of iterations used, set during training
    %   wInitTechnique   - Technique to initialize weights, can be set to
    %                      'FLD', 'randn', and 'manual'
    %   manualInitialW   - The values the weights are initialized to if
    %                      wInitTechnique is set to 'manual'
    %   wTolerance       - Convergence tolerance on weight vector
    %   handleNonPosDefR - What to do when R is non-positive definte, can
    %                      be set to 'regularize' or 'exit'. When set to
    %                      regularize, the classifier will attempt to
    %                      regularize the matrix. When set to exit the
    %                      classifier will exit.
    %
    %   w                - The regression weights, estimated during training
    %                      w(1) corresponds to the DC bias and w(2:end)
    %                      corresponds to the weights for the features
    %
    %    For more information on LogisticDiscriminant classifiers, refer to the
    %    following URL:
    %
    %    http://en.wikipedia.org/wiki/Logistic_regression
    %
    %    A prtClassLogisticDiscriminant object inherits the TRAIN, RUN,
    %    CROSSVALIDATE and KFOLDS methods from prtAction. It also inherits
    %    the PLOT method from prtClass.
    %
    %    Example:
    %
    %     TestDataSet = prtDataGenUnimodal;           % Create some test and
    %     TrainingDataSet = prtDataGenUnimodal;       % training data
    %     classifier = prtClassLogisticDiscriminant;  % Create a classifier
    %     classifier = classifier.train(TrainingDataSet);       % Train
    %     classified = run(classifier, TestDataSet);            % Test
    %     subplot(2,1,1);
    %     classifier.plot;
    %     subplot(2,1,2);
    %     [pf,pd] = prtScoreRoc(classified,TestDataSet);
    %     h = plot(pf,pd,'linewidth',3);
    %     title('ROC'); xlabel('Pf'); ylabel('Pd');
    %
    %    See also prtClass, prtClassLogisticDiscriminant, prtClassBagging,
    %    prtClassMap, prtClassCap, prtClassBinaryToMaryOneVsAll,
    %    prtClassDlrt, prtClassPlsda, prtClassFld, prtClassRvm,
    %    prtClassLogisticDiscriminant,  prtClass

    properties (SetAccess=private)
        name = 'Logistic Discriminant'  % Logistic Discriminant

        nameAbbreviation = 'LogDisc'  % LogDisc

        isNativeMary = false;  % True
    end

    % properties (SetAccess = protected)
    properties
        % w
        %   w is a DataSet.nDimensions + 1 x 1 vector of projection weights
        %   learned during LogDisc.train(DataSet)

        w = [];  % Regression weights

        % nIterations
        %   Number of iterations used in training.  This is set to a number
        %   between 1 and maxIter during training.

        nIterations = nan;  % The number of iterations used in training

        wPriorSigma = nan;  % Prior standard deviation on weight vector, use NAN to implement maximum likelihood estimates
    end
    properties
        % irlsStepSize
        %   irlsStepSize can be the string 'hessian', or a double value
        %   (typically << 1).  If irlsStepSize is 'hessian', IRLS is solved
        %   using the Hessian to estimate steps.  This can be numerically
        %   unstable.  Otherwise, training takes steps in the direction of
        %   the gradient with a step size of irlsStepSize*gradient.  Default
        %   value is 0.05.

        irlsStepSize = .05; % The stepsize

        % maxIter
        %   Maximum number of iterations to allow before exiting without
        %   convergence.

        maxIter = 500;  % Maxmimuum number of iterations

        % wInitTechnique
        %   wInitTechnique specifies how training should initialize the w
        %   vector.  Possible values are 'FLD', 'randn', and 'manual'.

        wInitTechnique = 'FLD';  % Weight initialization technique

        % manualInitialW
        %   manualInitialW is used to set the initial w value when
        %   wInitTechnique is 'manual'.  manualInitialW must be a vector
        %   and of length(TrainDataSet.nDimensions + 1)

        manualInitialW = []; % The value of the initial weights if initialized manually

        % wTolerance
        %   Convergence tolerance on w.  When norm(w - wOld) < wTolerance,
        %   convergence is reached, and training exits.

        wTolerance = 1e-2;  % The convergance tolerance of the weights

        % handleNonPosDefR
        %   It is possible to obtain non-positive definite re-weighted
        %   least-squares matrices in the optimization process.  Often this
        %   indicates convergence.  handleNonPosDefR can be one of 'exit'
        %   or 'regularize'.  'exit' tells training to exit with the
        %   current weight vector, and 'regularize' attempts to
        %   diagonal-load the R matrix to acheive a well conditioned
        %   matrix.  Often regularizing is a losing battle.

        handleNonPosDefR = 'exit';  % The action taken when R is non-positive definite

        sgdPassesThroughTheData = 10;
        sgdLearningRate = @(t)((sqrt(t) + 10).^(-0.9));
        sgdRegularization = 0.1;
        sgdWeightTolerance = 1e-6;

    end

    methods

        function Obj = prtClassLogisticDiscriminant(varargin)
            Obj = prtUtilAssignStringValuePairs(Obj,varargin{:});
        end

        function Obj = set.irlsStepSize(Obj,val)
            if ~prtUtilIsPositiveScalar(val) && ~strcmpi(val,'hessian')
                error('prt:prtClassLogisticDiscriminant:irlsStepSize','irlsStepSize must be a positive scalar (or the string ''hessian'')');
            end
            Obj.irlsStepSize = val;
        end

        function Obj = set.maxIter(Obj,val)
            if ~prtUtilIsPositiveScalarInteger(val)
                error('prt:prtClassLogisticDiscriminant:maxIter','maxIter must be a positive scalar integer');
            end
            Obj.maxIter = val;
        end

        function Obj = set.wInitTechnique(Obj,val)
            if ~isa(val,'char') || ~any(strcmpi(val,{'fld','randn','manual'}))
                error('prt:prtClassLogisticDiscriminant:wInitTechnique','wInitTechnique must be a string matching one of ''fld'', ''randn'', or ''manual''');
            end
            Obj.wInitTechnique = val;
        end

        function Obj = set.manualInitialW(Obj,val)
            if ~isnumeric(val) && ~isvector(val) && ~isempty(val)
                error('prt:prtClassLogisticDiscriminant:manualInitialW','manualInitialW must be a numeric vector');
            end
            Obj.manualInitialW = val;
        end

        function Obj = set.wTolerance(Obj,val)
            if ~prtUtilIsPositiveScalar(val)
                error('prt:prtClassLogisticDiscriminant:wTolerance','wTolerance must be a positive scalar');
            end
            Obj.wTolerance = val;
        end

        function Obj = set.handleNonPosDefR(Obj,val)
            if ~isa(val,'char') || ~any(strcmpi(val,{'exit','regularize'}))
                error('prt:prtClassLogisticDiscriminant:handleNonPosDefR','handleNonPosDefR must be a string matching one of ''exit'' or ''regularize''');
            end
            Obj.handleNonPosDefR = val;
        end

    end

    methods (Access=protected, Hidden = true)

        function Obj = trainAction(Obj,DataSet)
            %Obj = trainAction(Obj,DataSet)

            if ~DataSet.isBinary
                error('prtClassLogisticDiscriminant:nonBinaryTraining','Input dataSet for prtClassLogisticDiscriminant.train must be binary');
            end

            %Helper functions:
            sigmaFn = @(x) 1./(1 + exp(-x));
            rCondDiag = @(vec) min(vec)/max(vec);

            switch lower(Obj.wInitTechnique)
                case 'fld'
                    Fld = prtClassFld;
                    Fld = Fld.train(DataSet);
                    Obj.w = [1;Fld.w]; %append zero for offset
                case 'randn'
                    Obj.w = randn(DataSet.nFeatures+1,1);
                case 'manual'
                    assert(isvector(Obj.manualInitialW) & numel(Obj.manualInitialW) == DataSet.nFeatures + 1,'manualInitialW must be a vector and have %d elements',DataSet.nFeatures + 1);
                    assert(isequal(size(Obj.manualInitialW), [DataSet.nFeatures + 1,1]), 'manualInitialW must be a %d x 1 vector',DataSet.nFeatures + 1);
                    Obj.w = Obj.manualInitialW;
                otherwise
                    error('Invalid value for Options.wInitTechnique; wInitTechnique must be one of {''FLD'',''randn'',''manual''}');
            end

            DataSet = DataSet.retainLabeled; % Only use labeled data to train

            x = DataSet.getObservations;
            x = cat(2,ones(size(x,1),1),x);  %append "ones"
            %             y = DataSet.getTargets;
            %             if ~isequal(DataSet.uniqueClasses,[0;1])
            %                 bm = DataSet.getTargetsAsBinaryMatrix;
            %                 y(logical(bm(:,1))) = 0;
            %                 y(logical(bm(:,2))) = 1;
            %             end
            y = DataSet.getBinaryTargetsAsZeroOne;

            yOut = sigmaFn((x*Obj.w)')';
            rVec = yOut.*(1-yOut);

            converged = 0;
            nIter = 1;

            while ~converged
                Obj.nIterations = nIter;

                %these next lines are for numerical instabilities; these are detected
                %using the rCond of rVec or any nans in rVec
                if rCondDiag(rVec) < eps*2 || any(isnan(rVec))
                    %Numerical instability options are normalize, exit, or stepsize.
                    switch lower(Obj.handleNonPosDefR)
                        case 'regularize'
                            warning('prt:generateLogDisc:stepSize','rcond(R) < eps; attempting to diagonally load R');
                            diagAdd = 1e-5*max(rVec);
                            while(rCondDiag(rVec) < eps*2)
                                rVec = rVec + diagAdd;
                            end
                        case 'exit'
                            warning('prt:generateLogDisc:stepSize','rcond(R) < eps; Exiting; Try reducing classifier.irlsStepSize');
                            return;
                        otherwise
                            error('Invalid Obj.handleNonPosDefR field');
                    end
                end
                if isnan(Obj.wPriorSigma)
                    priorSigma = inf;
                else
                    priorSigma = Obj.wPriorSigma;
                end
                if isa(Obj.irlsStepSize,'char') && strcmpi(Obj.irlsStepSize,'hessian')
                    % Bishop equations:
                    %         wOld = w;
                    %         z = x*wOld - (R^-1)*(yOut - y);
                    %         w = (x'*R*x)^-1*x'*R*z;
                    %         %re-calculate yOut and R
                    %         yOut = sigmaFn((x*w)')';
                    %         R = diag(yOut.*(1-yOut));

                    %Non-matrix R Bishop equations (memory saving equations):
                    wOld = Obj.w;
                    z = x*wOld - bsxfun(@times,rVec.^-1,(yOut - y));
                    Obj.w = (x'*bsxfun(@times,rVec,x) + eye(size(x,2))*1./priorSigma)^-1*x'*bsxfun(@times,rVec,z);
                    %re-calculate yOut and R
                    yOut = sigmaFn((x*Obj.w)')';
                    rVec = yOut.*(1-yOut);
                elseif isa(Obj.irlsStepSize,'char')
                    error('String irlsStepSize must be ''hessian''');
                elseif isa(Obj.irlsStepSize,'double')
                    % Raw update equations:
                    %         wOld = w;
                    %         H = x'*R*x;
                    %         dE = x'*(yOut - y);
                    %         w = w - H^-1*dE*Options.irlsStepSize;  %move by stepsize * the hessian
                    %         %re-calculate yOut and R
                    %         yOut = sigmaFn((x*w)')';
                    %         R = diag(yOut.*(1-yOut));

                    %Non-matrix R Raw equations (memory saving equations):
                    wOld = Obj.w;
                    H = x'*bsxfun(@times,rVec,x) + eye(size(x,2))*1./priorSigma;

                    if rcond(H) < eps
                        warning('prt:generateLogDisc:stepSize','rcond(H) < eps; Exiting; Try reducing Options.irlsStepSize');
                        return;
                    end

                    dE = x'*(yOut - y);
                    Obj.w = Obj.w - H^-1*dE*Obj.irlsStepSize;  %move by stepsize * the hessian
                    %re-calculate yOut and R
                    yOut = sigmaFn((x*Obj.w)')';
                    rVec = yOut.*(1-yOut);
                end

                if norm(Obj.w - wOld) < Obj.wTolerance
                    converged = 1;
                end

                if nIter > Obj.maxIter
                    warning('prt:generateLogDisc:maxIter',sprintf('nIterations (%d) > maxIterations; exiting',nIter)); %#ok
                    return;
                end
                nIter = nIter + 1;
            end
        end

        function self = trainActionBig(self,ds)

            nTriesForNonEmptyBlock = 1000;
            useVariableLearningRate = isa(self.sgdLearningRate,'function_handle');
            converged = false;

            nMaxIterations = self.sgdPassesThroughTheData*ds.getNumBlocks();

            for iter = 1:nMaxIterations

                % Try to load a block
                for blockLoadTry = 1:nTriesForNonEmptyBlock
                    cBlockDs = ds.getRandomBlock;
                    if ~isempty(cBlockDs)
                        break
                    end
                end
                if blockLoadTry == nTriesForNonEmptyBlock
                    warning('Exiting after %d empty blocks were consecutlivly found.', nTriesForNonEmptyBlock);
                    break
                end

                % cBlockDs is now our dataset

                if iter==1
                    cW = zeros(cBlockDs.nFeatures+1,1);
                    wOld = inf;

                end
                cX = cat(2,ones(cBlockDs.nObservations,1), cBlockDs.X);

                % Get Y as -1, 1
                cY = cBlockDs.getTargetsAsBinaryMatrix;
                cY = cY(:,end); % In case of m-ary data we also do the last one.
                cY(cY == 0) = -1;

                %cW = cW + self.irlsStepSize*sum(bsxfun(@times,cY./(1 + exp(cY.*(cX*cW))),cX),1)';

                if useVariableLearningRate
                    cLearningRate = self.sgdLearningRate(iter);
                else
                    cLearningRate = self.sgdLearningRate;
                end
                cStep = (self.sgdRegularization*cW - mean(bsxfun(@times,cY./(1 + exp(cY.*(cX*cW))),cX),1)');
                cW = cW - cLearningRate*cStep;

                %cW = cW - cLearningRate*G^(-1/2)*cStep;
                cChange = norm(cW - wOld)/norm(cW);
                if cChange < self.sgdWeightTolerance
                    converged = true;
                    break
                end

                wOld = cW;

                plotOnIter = 1;
                if plotOnIter && ~mod(iter,plotOnIter)
                    plot(cW)
                    title(sprintf('iteration=%d - change=%0.3f',iter,cChange));
                    drawnow;
                end
            end

            if ~converged
                warning('prt:prtClassLogisticDiscriminant:notConverged','Convergence was not reached in the maximum number of allowed iterations.');
            end

            self.w = cW;
        end

        function ds = runAction(self,ds)
            ds.X = runFast(self,ds.X);
        end

        function y = runActionFast(self, x)
            x = cat(2,ones(size(x,1),1),x);
            y = 1./(1 + exp(-((x*self.w)')'));
        end
    end
end


function bsv_test ( )

%*****************************************************************************80
%
%% BSV_TEST tests the BURGERS_STEADY_VISCOUS (BSV) library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 April 2012
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BSV_TEST\n' );
  fprintf ( 1, '  MATLAB version.\n' );
  fprintf ( 1, '  Test the BURGERS_STEADY_VISCOUS (BSV) library.\n' );

  bsv_test01 ( );
  bsv_test02 ( );
  bsv_test03 ( );
  bsv_test04 ( );
  bsv_test05 ( );
  bsv_test06 ( );
  bsv_test07 ( );
  bsv_test08 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BSV_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function demo()
%
% demonstration file for NMFLibrary.
%
% This file illustrates how to use this library.
% This demonstrates Frobenius-norm based multiplicative updates (MU) algorithm and
% hierarchical alternative least squares (Hierarchical ALS) algorithm.
%
% This file is part of NMFLibrary.
%
% Created by H.Kasai on Apr. 05, 2017

    clc;
    clear;
    close all;

    %% generate synthetic data of (mxn) matrix
    m = 500;
    n = 100;
    V = rand(m,n);

    %% Initialize of rank to be factorized
    rank = 5;

    %% perform factroization
    options.verbose = 1;
    % MU
    options.alg = 'mu';
    [w_mu, infos_mu] = fro_mu_nmf(V, rank, options);
    % Hierarchical ALS
    options.alg = 'hals';
    [w_hals, infos_hals] = als_nmf(V, rank, options);
    % Accelerated Hierarchical ALS
    options.alg = 'acc_hals';
    [w_acchals, infos_acchals] = als_nmf(V, rank, options);

    %% plot
    display_graph('epoch','cost', {'Fro-MU', 'HALS', 'Acc-HALS'}, {w_mu, w_hals, w_acchals}, {infos_mu, infos_hals, infos_acchals});
    display_graph('time','cost', {'Fro-MU', 'HALS', 'Acc-HALS'}, {w_mu, w_hals, w_acchals}, {infos_mu, infos_hals, infos_acchals});

end


function [lin_fun, max_lin_fun] = RobertsonCRF(stack, stack_exposure, max_iterations, err_threshold, bNormalize)
%
%       [lin_fun, max_lin_fun] = RobertsonCRF(stack, stack_exposure, max_iterations, err_threshold, bNormalize)
%
%       This function computes camera response function using Mitsunaga and
%       Nayar method.
%
%        Input:
%           -stack: a stack of LDR images. If the stack is a single or
%           double values are assumed to be in [0,1].
%           -stack_exposure: an array containg the exposure time of each
%           image. Time is expressed in second (s)
%           -max_iterations: max number of iterations
%           -err_threshold: threshold after which the function can stop
%           -bNormalize: if 1 it enables function normalization
%
%        Output:
%           -lin_fun: tabled function
%           -max_lin_fun: maximum value of the inverse CRF
%
%     Copyright (C) 2016  Francesco Banterle
%
%     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/>.
%

if(isempty(stack))
    error('RobertsonCRF: a stack cannot be empty!');
end

if(isempty(stack_exposure))
    error('RobertsonCRF: a stack_exposure cannot be empty!');
end

if(~exist('err_threshold', 'var'))
    err_threshold = 1e-5;
end

if(~exist('bNormalize', 'var'))
    bNormalize = 0;
end

if(~exist('max_iterations', 'var'))
    max_iterations = 15;
end

if(isa(stack, 'double'))
    stack = uint8(ClampImg(round(stack * 255), 0, 255));
end

if(isa(stack, 'single'))
    stack = uint8(ClampImg(round(stack * 255), 0, 255));
end

if(isa(stack, 'uint16'))
    stack = uint8(stack / 255);
end

col = size(stack, 3);

lin_fun = zeros(256, col);

for i=1:col
    lin_fun(:, i) = (0:255) / 255;
end

global minM;
global maxM;

x = (0:255) / 255;

w = WeightFunction(x, 'Robertson', 0);

minM = 0;
for m=0:255
    if(w(m + 1) > 0.0)
        minM = m;
        break;
    end
end

maxM = 255;
for m=255:-1:0
    if(w(m + 1) > 0.0)
        maxM = m;
        break;
    end
end

global stack_min;
global stack_max;

[~, t_min] = min(stack_exposure);
[~, t_max] = max(stack_exposure);
stack_min = ClampImg(single(stack(:,:,:,t_min)) / 255.0, 0.0, 1.0);
stack_max = ClampImg(single(stack(:,:,:,t_max)) / 255.0, 0.0, 1.0);

global minSatTime;

minSatTime = 1e10 * ones(col, 1);

for j=1:col
    for i=1:length(stack_exposure)
        tmp = stack(:,:,j,i);

        if(~isempty(find(tmp == 255)))
            if(stack_exposure(i) < minSatTime(j))
                minSatTime(j) = stack_exposure(i);
            end
        end
    end
end

for i=1:max_iterations
    lin_fun_prev = lin_fun;

    %normalization CRF
    lin_fun = Normalization(lin_fun);

    %update X
    x_tilde = Update_X(stack, stack_exposure, lin_fun);

    %update the iCRF
    lin_fun = Update_lin_fun(x_tilde, stack, stack_exposure, lin_fun);

    %compute error
    delta = (lin_fun_prev - lin_fun).^2;
    err = mean(delta(:));

    %disp([i, err]);

    if(err < err_threshold)
        break;
    end
end

max_lin_fun = max(lin_fun(:));

if(bNormalize)
    for i=1:col
        lin_fun(:,i) = lin_fun(:,i) / max_lin_fun;
    end

    lin_fun = ClampImg(lin_fun, 0.0, 1.0);
end

% %poly-fit (rational)
% if(bPolyFit)
%     ft = fittype( 'rat33' );
%     opts = fitoptions( 'Method', 'NonlinearLeastSquares' );
%     opts.Display = 'Off';
%     opts.StartPoint = ones(7, 1);
%
%     for i=1:col
%         [xData, yData] = prepareCurveData(x', lin_fun(:,i));
%         [fit_ret, ~] = fit( xData, yData, ft, opts );
%         lin_fun(:,i) = feval(fit_ret, x');
%     end
% end

end

function lin_fun = Normalization(lin_fun)
    col = size(lin_fun, 2);

    for j=1:col
        lf = lin_fun(:, j);

        index_min = 1;
        for minIndx=1:256
            if(lf(minIndx) > 0.0)
                index_min = minIndx;
                break;
            end
        end

        index_max = 256;
        for maxIndx=256:-1:1
            if(lf(maxIndx) > 0.0)
                index_max = maxIndx;
                break;
            end
        end

        index = index_min + round((index_max - index_min) / 2);

        mid = lf(index);

        if(mid == 0.0)
            while(index < 256 && lf(index) == 0.0)
                index = index + 1;
            end

            mid = lf(index);
        end

        if(mid > 0.0)
            lin_fun(:,j) = lin_fun(:,j) / mid;
        end
    end
end

function imgOut = Update_X(stack, stack_exposure, lin_fun)
    global stack_min;
    global stack_max;
    global minM;
    global maxM;

    [r, c, col, n] = size(stack);

    %for each LDR image...
    imgOut    = zeros(r, c, col, 'single');
    totWeight = zeros(r, c, col, 'single');

    max_t = -ones(r, c, col);
    min_t = 1e10 * ones(r, c, col);

    for i=1:n
        t = stack_exposure(i);
        m = stack(:,:,:,i);

        indx = find(m > maxM);
        min_t(indx) = min(min_t(indx), t);

        indx = find(m < minM);
        max_t(indx) = max(max_t(indx), t);

        %normalizing m values in [0,1]
        tmpStack = ClampImg(single(m) / 255.0, 0.0, 1.0);

        %computing the weight function
        weight  = WeightFunction(tmpStack, 'Robertson', 0);

        tmpStack = tabledFunction(tmpStack, lin_fun);

        %summing things up...
        indx = find(tmpStack > stack_min & tmpStack < stack_max);

        if(t > 0.0 && ~isempty(indx))
            imgOut(indx) = imgOut(indx) + (weight(indx) .* tmpStack(indx)) * t;
            totWeight(indx) = totWeight(indx) + weight(indx) * t * t;
        end
    end

    imgOut = imgOut ./ totWeight;

    %taking care of saturated pixels
    saturation = 1e-4;
    imgOut(totWeight < saturation & totWeight > 0.0) = -1.0;

    for i=1:col
        io = imgOut(:,:,i);
        tw = totWeight(:,:,i);
        mxt = max_t(:,:,i);
        mnt = min_t(:,:,i);

        indx = find(tw == 0.0 & mxt > -1.0);
        io(indx) = lin_fun(minM, i) ./ mxt(indx);

        indx = find(tw == 0.0 & mnt < 1e10);
        io(indx) = lin_fun(maxM, i) ./ mnt(indx);

        imgOut(:,:,i) = io;
    end

end

function f_out = Update_lin_fun(x_tilde, stack, stack_exposure, lin_fun)
    col = size(x_tilde, 3);

    n = length(stack_exposure);
    f_out = zeros(size(lin_fun));

    global minSatTime;

    for i=1:col
        x_tilde_i = x_tilde(:,:,i);

        cardEm = zeros(256, 1);
        sumEm = zeros(256, 1);

        %for m values in [0,254]
        for m=0:254
            mp = m + 1;

            for k=1:n
                t = stack_exposure(k);

                tmp = stack(:,:,i,k);

                x = x_tilde_i((tmp == m) & (x_tilde_i > 0.0));

                sumEm(mp) = sumEm(mp) + t * sum(x(:));

                cardEm(mp) = cardEm(mp) + length(x(:));
            end
        end

        %for m = 255
        cardEm(256) = 1;

        m = 255;
        sumEm(256) = 1e10;
        for k=1:n
        	t = stack_exposure(k);

            tmp = stack(:,:,i,k);

            x = x_tilde_i((tmp == m) & (x_tilde_i > 0.0));

            if(~isempty(x) & (t == minSatTime))
               sumEm(256) = min([sumEm(256); t * x(:)]);
            end
        end

        %computing f_out + filling
        prev = 0.0;
        for m=1:256
            if(cardEm(m) > 0.0)
                f_out(m,i) = sumEm(m) / cardEm(m);
                prev = f_out(m,i);
            else
                f_out(m,i) = prev;
            end
        end
    end
end


function [ y, j ] = j_borrow_roman ( y, j )

%*****************************************************************************80
%
%% J_BORROW_ROMAN borrows year-days from years in a Roman date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input/output, integer Y, J, a YJ date.
%
  while ( j <= 0 )

    y = y - 1;

    days = year_length_roman ( y );

    j = j + days;

  end

  return
end


function h = togglePlot(Y, E, X, names, colors, styles);
% Plot time curves with errorbars, and toggles for individual curves.
%
% h = togglePlot(Y, [E], [X], [names], [colors], [styles]);
%
% This is an updated version of my old function FANCYPLOT. It produces
% a plot of the data Y, at points X, with error bar size E. For X, Y, and
% E, different columns represent different errorbar lines while rows
% represent different points along X. If X is omitted, it initializes to
% [1:size(Y, 1)] for each column. A legend is appended, along with
% checkboxes for each column in Y, which toggles the visibility of that
% data line.
%
% names, colors, and styles are all optional cell arrays specifying the
% names of each column in Y for the legend, the colors associated with each
% line, and the line styles (as set per SETLINESTYLES).
%
% Returns an aray of handles to the plot elements, in the format
%   [figure, axes, legendPanel, (errorbar handles) (checkbox handles)].
%
% EXAMPLE:
%   X = linspace(0, 2*pi, 40);
%   Y = [cos(X); sin(X); cos(X).^2; tan(X)]';
%   E = .1 * rand(40, 4);
%   names = {'cos' 'sin' 'cos^2' 'tan'};
%   colors = {[1 0 0] [.9 .2 0] [.7 .3 .1] [.5 .5 .3]};
%   styles = {'-2' '--2' '.-2' ':2'};
%   h = togglePlot(Y, E, X, names, colors, styles);
%
% ras, 03/2007.
if notDefined('Y'),     error('Need to specify data Y.');       end
if notDefined('X'),     X = repmat([1:size(Y,1)]', [1 size(Y,2)]);  end
if notDefined('E'),     E = zeros(size(Y));                     end
if notDefined('styles'),    styles = {};                    end

nCols = size(Y, 2);

if notDefined('names'),
    for i = 1:nCols
        names{i} = sprintf('Col %i', i);
    end
end

% size check on X
if ~isequal(size(X), size(Y))
    if numel(X)==size(Y, 1)
        % single X vec for all columns
        X = repmat(X(:), [1 nCols]);

    else
        error('X and Y should be same size.')

    end
end

%% create figure, axes
h(1) = figure('Color', 'w');

h(2) = axes;

%% plot the data
hBars = errorbar(X, Y, E);

% get default colors if not specified
if notDefined('colors'),
    colorOrder = get(h(2), 'ColorOrder');
    while size(colorOrder, 1) < nCols
        colorOrder = [colorOrder; colorOrder];
    end

    for col = 1:nCols
        colors{col} = colorOrder(col,:);
    end
end

setLineColors(colors);

if ~isempty(styles)
    setLineStyles(styles);
end

%% add legend panel, checkboxes
hLeg = legendPanel(names, colors);

% callback for each checkbox
cb = ['if get(gcbo,''Value'')==1, TMP=''on''; else, TMP=''off''; end; ' ...
      'set(get(gcbo,''UserData''), ''Visible'', TMP); ' ...
      'clear TMP; '];

% add checkboxes
for n = 1:nCols
    % these are positions of the axes in LEGENDPANEL
    row = mod(n-1, 20) + 1;
    pos = [.4,  .96-row*.05,  .1,  .025];

    hCheck(n) = uicontrol('Parent', hLeg, 'Style', 'checkbox', ...
                    'Units', 'normalized', 'Position', pos, ...
                    'UserData', hBars(n), 'Callback', cb, ...
                    'String', '', 'BackgroundColor', 'w', 'Value', 1);
end

%% append all handles to h
h = [h hLeg hBars hCheck];

return


% this class computes sentence-to-vec model, given word-to-vec model.
classdef Model < handle
    properties(Constant)
        MODE_AVG      = 0   % encode by average
        MODE_FISHER   = 1   % encode by fisher vectors
        MODE_BOW      = 2   % bag of words

        % improved fv options

        POWER_OFF           = 0   % no power transform
        POWER_SQRT_ALL      = 1   % sqrt all coordinates
        POWER_SQRT_GMM_ONLY = 2   % sqrt only the gmm coordinates
		POWER_EXPERIMENTAL  = 3

        NORMALIZE_OFF             = 0 % no normalization
        NORMALIZE_L2_ALL          = 1 % L2 normalization
        NORMALIZE_L1_L2_SELECTIVE = 2 % L2 for gmm coordinates, L1 for lmm
    end

    properties

        % improved fv options
        power_option;
        norm_option;

        % Word2vec model
        w2v;

        % GMM / LMM model
        mm_type
        mm_num_of_centers

        % will be used for GMM / LMM
        mm_means
        mm_covariances
        mm_priors

        % will be used for HGLMM
        hglmm_m
        hglmm_s
        hglmm_mu
        hglmm_sigma
        hglmm_b
        hglmm_prior

        % will be used for GG
        gg_p
        gg_mu
        gg_sigma

        % options paramter for gg_fv function
        gg_options

        % multiplication factor (workaround for numeric issues with LMM / HGLMM)
        mult_fctr;

        % transition matrix, for PCA / ICA
        trans_mat;

    end

    methods

        % constructor
        % mm_type: one of: 'gmm', 'lmm', 'hcg', 'hcl', 'hglmm'
        % lowercase: a flag indicating that our dictionary is in lowercase
        % (this is the case with GloVe)
        function this = Model(words_file_name, vectors_file_name, freq_file_name, mm_file_name, mm_type, trans_matrix, lowercase, power_option, norm_option)

            if ~exist('lowercase', 'var')
                lowercase = false;
            end

            if ~exist('power_option', 'var')
                power_option = this.POWER_SQRT_ALL;
            end

            if ~exist('norm_option', 'var')
                norm_option = this.NORMALIZE_L2_ALL;
            end

            this.gg_options = [];

            this.power_option = power_option;
            this.norm_option = norm_option;

            this.trans_mat = trans_matrix;

			% load word2vec
            this.w2v = Str2vec(words_file_name, vectors_file_name, freq_file_name, lowercase);

            if ~isempty(mm_file_name)

                this.mm_type = mm_type;

                lg(1, 'MM type: %s\n', this.mm_type);

                % read GMM/LMM file
                lg(1, 'reading %s file %s\n', this.mm_type, mm_file_name);
                load(mm_file_name);
                lg(1, 'done\n');

                % if mult_factor was saved in the mm file (it is relevnt
                % for LMM / HGLMM)
                if exist('mult_factor', 'var')
                    this.mult_fctr = mult_factor;
                else
                    switch this.mm_type
                        case {'lmm', 'hglmm'}
                            % default is 10 (backward compatibility)
                            this.mult_fctr = 10;
                        otherwise
                            this.mult_fctr = 1;
                    end
                end

                switch this.mm_type
                    case 'hglmm'
                        this.hglmm_m = m_out;
                        this.hglmm_s = s_out;
                        this.hglmm_mu = mu_out;
                        this.hglmm_sigma = sigma_out;
                        this.hglmm_b = b_out;
                        this.hglmm_prior = prior_out;

                        this.mm_num_of_centers = length(this.hglmm_prior);

                    case 'gg'
                        this.gg_p = gg_p;
                        this.gg_mu = gg_mu;
                        this.gg_sigma = gg_sigma;

                        this.mm_num_of_centers = size(gg_mu,2);

                    % gmm or lmm
                    otherwise
                        this.mm_means = n_means;
                        this.mm_covariances = n_covariances;
                        this.mm_priors = n_priors;

                        this.mm_num_of_centers = length(this.mm_priors);
                end
            else
                lg(1, 'no MM file was provided\n');
            end
        end

        function set_gg_options(this, gg_options)
            this.gg_options = gg_options;
        end

        function str = mode2str(this, mode)
            switch mode
                case this.MODE_AVG
                    str = 'AVG';
                case this.MODE_FISHER
                    str = 'FISHER';
                case this.MODE_BOW
                    str = 'BOW';
                otherwise
                    str = 'UNKNOWN';
            end
        end

        function d = encoding_size(this, mode)
            dim = size(this.trans_mat, 1);

            % trans_mat is empty
            if dim == 0
                dim = this.w2v.dimension;
            end

            switch mode
                case this.MODE_AVG
                    d = dim;
                case this.MODE_FISHER
                    switch this.mm_type
                        case 'gg'
                            d = 2 * this.mm_num_of_centers * dim;
                        otherwise
                            d = 2 * this.mm_num_of_centers * dim;
                    end
                case this.MODE_BOW
                    d = this.w2v.num_strings;
                otherwise
                    d = 0;
            end
        end

        % encodes a sentence using fisher vectors
        function encoding = encode_sen_fv(this, sentence, num_infreq_words, sum_pair_mode)

			if ~exist('sum_pair_mode', 'var')
				sum_pair_mode = false;
            end

            X = this.w2v.sen_to_word_vecs(sentence, num_infreq_words);
            if isempty(X)
                encoding = [];
                return;
            end

            % encode the sentence using fisher vectors

            if sum_pair_mode
                Z = zeros(size(X,1), size(X,2) - 1);
                for i = 1:size(Z,2)
                    Z(:,i) = X(:,i) + X(:,i+1);
                end

                X = [X, Z];
            end

            if ~isempty(this.trans_mat)
                X = this.trans_mat * X;

                % in pca and ica, we normalized the vectors after
                % transformation
                X = normc(X);
            end

            switch this.mm_type
                case {'gmm', 'hcg'}

					switch this.power_option
                        case {this.POWER_OFF, this.POWER_EXPERIMENTAL}
                            error('need to add support for POWER_OFF, POWER_EXPERIMENTAL');
                    end

                    if this.norm_option == this.NORMALIZE_OFF
                        error('need to add support for NORMALIZE_OFF');
                    end

                    encoding = vl_fisher(X, this.mm_means, this.mm_covariances, this.mm_priors, 'SquareRoot', 'Normalized');

                case 'gg'

					switch this.power_option
                        case {this.POWER_OFF, this.POWER_EXPERIMENTAL}
                            error('need to add support for POWER_OFF, POWER_EXPERIMENTAL');
                    end

                    if this.norm_option == this.NORMALIZE_OFF
                        error('need to add support for NORMALIZE_OFF');
                    end

                    encoding = gg_fv(X, this.gg_p, this.gg_mu, this.gg_sigma, this.gg_options);

                case {'lmm', 'hcl'}
                    encoding = FVMex(double(this.mult_fctr * X'),double(this.mm_means), double(this.mm_covariances), double(this.mm_priors),1.0);

                    switch this.power_option
                        case this.POWER_SQRT_ALL
                            encoding = sign(encoding) .* sqrt(abs(encoding));
                        case this.POWER_EXPERIMENTAL
                            %encoding = sign(encoding) .* (abs(encoding).^experimental_power);
                            %encoding = sign(encoding) .* log(abs(encoding) + 1);
                            encoding = asinh(encoding);
                    end

                    switch this.norm_option
                        case this.NORMALIZE_L2_ALL
                            encoding = encoding / norm(encoding, 2);
                        case this.NORMALIZE_L1_L2_SELECTIVE
                            encoding = encoding / norm(encoding, 1);
                    end

                case 'hglmm'
                    numOfCpus = 8;

                    encoding = HybridFV(double(this.mult_fctr * X'), this.hglmm_m, this.hglmm_s, this.hglmm_mu, this.hglmm_sigma, this.hglmm_b, this.hglmm_prior, numOfCpus);

                    I_lmm = this.hglmm_b';
                    I_lmm = I_lmm(:);
                    I_lmm = [I_lmm; I_lmm];
                    I_gmm = ~I_lmm;

                    switch this.power_option
                        case this.POWER_OFF
                            I_power = false(length(encoding), 1);
                        case this.POWER_SQRT_ALL
                            I_power = true(length(encoding), 1);
                        case this.POWER_SQRT_GMM_ONLY
                            I_power = I_gmm;
                    end

                    switch this.norm_option
                        case this.NORMALIZE_OFF
                            I_norm2 = false(length(encoding), 1);
                            I_norm1 = false(length(encoding), 1);
                        case this.NORMALIZE_L2_ALL
                            I_norm2 = true(length(encoding), 1);
                            I_norm1 = false(length(encoding), 1);
                        case this.NORMALIZE_L1_L2_SELECTIVE
                            I_norm2 = I_gmm;
                            I_norm1 = I_lmm;
                    end

                    if this.power_option == this.POWER_EXPERIMENTAL
                        %encoding = sign(encoding) .* (abs(encoding).^experimental_power);
                        %encoding = sign(encoding) .* log(abs(encoding) + 1);
                        encoding = asinh(encoding);
                    else
                        encoding(I_power) = sign(encoding(I_power)) .* sqrt(abs(encoding(I_power)));
                    end

                    encoding(I_norm2) = encoding(I_norm2) / norm(encoding(I_norm2),2);
                    encoding(I_norm1) = encoding(I_norm1) / norm(encoding(I_norm1),1);

                otherwise
                    error('illegal mm_type');
            end
        end

        % encodes a sentence using average
        function encoding = encode_sen_avg(this, sentence, num_infreq_words)

            X = this.w2v.sen_to_word_vecs(sentence, num_infreq_words);
            if isempty(X)
                encoding = [];
                return;
            end

            if ~isempty(this.trans_mat)
                X = this.trans_mat * X;

                % in pca and ica, we normalized the vectors after
                % transformation
                X = normc(X);
            end

            % encode the sentence using average
            encoding = sum(X, 2) / size(X, 2);
            % normalize
            encoding = norma(encoding);
        end

        % bag of words (normalized)
        function bow = encode_sen_norm_bow(this, sentence, num_infreq_words)
            if ~isempty(num_infreq_words)
                error('encode_sen_norm_bow does not support num_infreq_words yet');
            end

            valid_words_in_sen = this.w2v.all_valid_words(sentence, false);
            num_valid_words_in_sen = length(valid_words_in_sen);

            bow = zeros(this.w2v.num_strings, 1);

            for j = 1:num_valid_words_in_sen
                word = valid_words_in_sen{j};
                word_idx = this.w2v.str2idx(word);
                bow(word_idx) = bow(word_idx) + 1;
            end

            % normalize
            bow = norma(bow);
        end

        % 'sentence' may be either a string, or a vector with words indices
        % (according to their order in the word2vec) of the valid words of
        % a sentence. for MODE_BOW, the words indices option is currenlty
        % not supported.
        function encoding = encode_sen(this, mode, sentence, num_infreq_words)

            if ~exist('num_infreq_words', 'var')
                num_infreq_words = [];
            end

            switch mode
                case this.MODE_FISHER
                    encoding = this.encode_sen_fv(sentence, num_infreq_words);
                case this.MODE_AVG
                    encoding = this.encode_sen_avg(sentence, num_infreq_words);
                case this.MODE_BOW
                    if ~ischar(sentence)
                        error('for MODE_BOW, the words indices option is currenlty not supported');
                    end
                    encoding = this.encode_sen_norm_bow(sentence, num_infreq_words);
                otherwise
                    error('illegal mode');
            end
        end

        % words_I: a vector of indices of valid words (return by
        % all_valid_words with paramter word_indices==true).
        % this function encodes each word as an entire sentence and retures
        % the matrix of all encodings
        function words_encoding = encode_words_as_sentences(this, mode, words_I)
            num_words = length(words_I);

            words_encoding = zeros(this.encoding_size(mode), num_words);

            for i = 1:num_words
                words_encoding(:,i) = this.encode_sen(mode, words_I(i));
            end
        end

        % returns a matrix containing the encodings of the given sentences.
        % I_bad: indicators for sentences which could not be encoded since
        % none of their words is in the vocabulary. in such cases, the
        % vector of the sentence will be all zeros
        function [encoding, I_bad] = encode_sentences(this, mode, sentences)
            % if only one sentence - make it a list
            if ~iscell(sentences)
                sentences = {sentences};
            end

            num_sentences = length(sentences);

            encoding = zeros(this.encoding_size(mode), num_sentences);
            I_bad = false(1, num_sentences);

            lg(1, 'encoding %d sentences...\n', num_sentences);

            for i = 1:num_sentences
                v = this.encode_sen(mode, sentences{i});

                if isempty(v)
                    I_bad(i) = true;
                    lg(0, '\n** Sentence number %d could not be encoded since none of its words is in the vocabulary:\n', i);
                    lg(0, '%s\n', sentences{i});
                else
                    encoding(:,i) = v;
                end

                printmark(i, 100, num_sentences)
            end

            lg(1, 'done\n');
        end

    end

    methods(Static)

        function str = power2str(power_option)
            switch power_option
                case Model.POWER_OFF
                    str = 'POWER_OFF';
                case Model.POWER_SQRT_ALL
                    str = 'POWER_SQRT_ALL';
                case Model.POWER_SQRT_GMM_ONLY
                    str = 'POWER_SQRT_GMM_ONLY';
                case Model.POWER_EXPERIMENTAL
                    str = 'POWER_EXPERIMENTAL';
                otherwise
                    str = 'UNKNOWN';
            end
        end

        function str = normalize2str(normalize_option)
            switch normalize_option
                case Model.NORMALIZE_OFF
                    str = 'NORMALIZE_OFF';
                case Model.NORMALIZE_L2_ALL
                    str = 'NORMALIZE_L2_ALL';
                case Model.NORMALIZE_L1_L2_SELECTIVE
                    str = 'NORMALIZE_L1_L2_SELECTIVE';
                otherwise
                    str = 'UNKNOWN';
            end
        end

    end

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_Prosix_C3_A601C(robot, T)
%   Solves the inverse kinematic problem for the EPSON Prosix_C3_A601C 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__Prosix_C3_A601C returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   epson=load_robot('EPSON', 'Prosix_C3_A601C');
%   q = [0 0 0 0 0 0];
%   T = directkinematic(epson, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(epson, T);
%   check that all of them are feasible solutions!
%   and every Ti equals T
%   for i=1:8,
%        Ti = directkinematic(epson, qinv(:,i))
%   end
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function [q] = inversekinematic_Prosix_C3_A601C(robot, T)

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

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

%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);

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%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),
    % use solve_spherical_wrist2 for the particular orientation
    % of the systems in this ABB robot
    % use either the geometric or algebraic method.
    % the function solve_spherical_wrist2 is used due to the relative
    % orientation of the last three DH reference systems.

    %This robot uses a different function to compute the last three angles,
    %since the relative orientation of the systems S4, S5 and S6 differs
    %from that of the rest of the robots
    qtemp = solve_spherical_wrist_Prosix(robot, q(:,i), T, 1); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solve_spherical_wrist_Prosix(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=a(2);
L3=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_Prosix_C3_A601C: 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=a(2);
L3=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_Prosix_C3_A601C: 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/2 - eta);
q3(2) = -(eta - 3*pi/2);

% 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_Prosix(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);