% This program implements the Kinect IMU integration as discussed by Huai
% Formulated in a local frame with camera measurements
% the output trajectory is that of the IMU

% we face the problem that some IMU records are missing, but it is
% generally very small gap, so it is reasonable to use the received
% measurements for Kalman update once the imu epoch exceeds their epoch.

% Test cases (1) start IMU and Kinect in stationary mode, do ZUPT for IMU
% for at least 1 minutes, the initial frame of IMU and the camera are s0
% and c0 frame, then when Kinect is moved smoothly in predefined
% trajectory, the filter is used to estimate gravity in s0 frame, the
% calibration terms of camera and IMU

function imu_cam_calib()
addpath('..\instk'); % imu functions
addpath('..\voicebox\'); % for rotro2qr and rotqr2eu, they are more robuts
% than dcm2quat_v000 and dcm2euler_v000
import java.util.LinkedList
clear variables;
clc; close all; format longg;
fprintf('\n IMU_Kinect_Calib.m to test EKF filtering in calibrating mems IMU and Kinect!\n\n');
rng('default');

experim=4; %test case for IMU CAMera calibration

switch experim

    case 4
        % test on Steval MKI062V2 data integration with camera, Mar 2014
        % output options
        % test observations: block camera measurement in the ZUPT period,
        % or turn off ZUPT, does not change the results much of turn-on ZUPT and camera
        % measurement at the same time.
        % the strange profile of Cs2c has to do with the data,
        % Ts2c wrong becuase of the filter mechanism and data.
        % use of gravity magnitude constraint does not show much improvement
        % it is advised to turn off scale factor estimate, let gravity and
        % RS2C float
        isOutNED=true;
        resdir='C:\Users\huai.3\kinectfusion\KinectFusionExplorer-D2D\temp\';
        filresfile=[resdir, 'filresult.bin']; % navigation states
        imuresfile=[resdir, 'imuresult.bin']; % imu error terms
        % imu options
        options.startTime=2;
        options.endTime=130;
        options.imuErrorModel=4; % how bias and scale factor is modeled
        options.mechanization=2; % 1 for wander azimuth
        options.imutype=6;      %  Steval MKI062V2
        options.dt=1/50;  %sampling interval
        options.maxCovStep=options.dt; %maximum covariance propagation step, if equal to dt, means single speed mode
        options.Cb2imu=eye(3);
        options.Timu2body=zeros(3,1); % h764G is the body frame
        options.imufile=[resdir,'iNemo_02092014_061806.tsv'];

       % Toc2s=[[  0.0970;   -0.2459;    0.9432];-[   -0.0037;   -0.0537;   -0.0680]]; % PoseIMUKinectx302010
       % Toc2s=[[   0.0886;   -0.2499;    0.9400]; -[  -0.0006;   -0.0526;   -0.0681]]; % for PoseIMUKinecty102030
      %  Toc2s=[[0.1194;   -0.2479;    0.9359]; -[  -0.0022;   -0.0530;   -0.0676]];% for PoseIMUKinectz201306
      %  Toc2s=[[   0.177695680320824;-0.00680201661415064;0.962107115439917]; -[  0.0362812169298294;-0.0710746054253426;0.0478464512175324]]; % for PoseIMUCircle
       % Toc2s=[[   -0.219889874055416; -0.230975516372796;0.907607304785909]; -[ -0.0084376;   -0.057877;       -0.068204]]; % for PoseIMUKinect3
       Toc2s=[0.285567755532141, -0.0225589251844046, 1.0874587565859,  0, 3/180*pi, 4/180*pi]'; % for iNEMO2_20140603_160228 anticlock
        % ZUPT start and end Time, determine before the filter
        % format n x 2
        % n is the number of segment
        % first  column is the start time
        % second column is the end time, for example [575908      576243; 576502      576602]
        options.zuptSE=[2, 50]; % for iNEMO2_20140603_160228 anticlock
        imuFileType=3; % iNemo data
       % imuFileType=2; % file provided by Yujia, mix of steval and kinect output
        %Initial PVA of IMU
        options.inillh_ant=[40.00311241687*pi/180; -83.01529294250*pi/180; 227.901];
        % lat, log, height of the antenna, in this case, identical to IMU,
        % guessed from http://www.daftlogic.com/sandbox-google-maps-find-altitude.htm
        options.imuErrors=zeros(12,1);
        options.imuErrors(1:6)=Toc2s; % for the Steval case, comment this line if not steval
        options.Vn=[0;0;0];
        options.Ve=[0;0;0];
        options.qb2n=rotro2qr(R3(0.1477)*[0, -1,0; -1,0, 0; 0,0, -1]);
        % 0.1477 is roughly estimated from earth surface triangulation using data from daftlogic
        options.InvalidateIMUerrors=false;
        options.initAttVar=0.5*pi/180; % 1 deg std for roll and pitch, 2 times 5 deg for yaw std
        % gps options
        useGPS=false;

        % ZUPT options
        options.sigmaZUPTPos = 0.02;% unit m
        options.sigmaZUPTVel = 0.02;% unit m/s
        options.sigmaZUPTAng = 1*180/pi;% unit rad
        rateZUPT=round(sqrt(1/options.dt)); % from tests, we see zupt has adverse effect in this case
        % NHC options
        options.sigmaNHC = 0.1;% unit m/s
        rateNHC=inf; %round(sqrt(1/options.dt));
        % minimum velocity before applying the NHC, this option decouples ZUPT and NHC
        options.minNHCVel=2.0;

        rateGravNorm=inf; %rateZUPT;
        sigmaGravMag=3e-3; % unit m/s^2

        options.useCam=false;
        camFile=options.imufile; % where the camera measurements come from
        options.camPoseFile=[resdir, 'kinectPose.txt']; % output camera position and attitude
        deltaPhi=[0;0;0]; % [-0.487; 1.106; -0.518]/180*pi;
        options.Cimu2cam=rotqr2ro(rvec2quat_v000(deltaPhi))*[0,0,1;-1,0,0;0,-1,0]'; % (Rc2s)'
        options.Tcam2body=[3; -6.2; -4.05]*1e-2; % unit m
        % depth camera in body frame whichStep is assumed to be the imu frame,

        sigmaEuler=[1; 1; 1]*pi/180; % unit rad
        sigmaTrans=[2;2;2]*1e-3; % unit m
    otherwise
        error('Unsupported testing case!');
end
numPrevImuDataToKeep=40; % the number of data put in preimudata
% Initialize the model state and covariance of state, process noise and
% measurment noise
filter =EKF_filter_s0framelet(options);
preimudata=LinkedList();% record the previous imu data

% read in imu data
[fimu, imudata, preimudata]=readimuheader(options.imufile, preimudata, options.startTime, numPrevImuDataToKeep, imuFileType);
lastimu=preimudata.getLast();
preimutime=lastimu(1,end);
imuctr=1;   % to count how many IMU data after the latest GPS observations

%load the camera measurements with timestamp
if(options.useCam)
    camdata=load_KinectDataset(camFile, options.startTime);
    euc2c0imu=zeros(size(camdata,1),4);
    euc2c0imu(:,1)=camdata(:,1);
    imgepoch=camdata(1,1);
    whichStep=1;
else imgepoch=inf;
end
% Start the main INS
initime=preimutime;
imuaccum=zeros(6,1);    % record the accumulated imu measurements
curimutime=imudata(1,end);
covupt_time=preimutime; %the time that we last updated the covariance

fprintf('Estimating trajectory...\n');
ffilres=fopen(filresfile,'Wb'); % navigation states, 'W' use buffer and binary fwrite()
fimures=fopen(imuresfile,'Wb'); % imu errors

while (~feof(fimu)&&curimutime<options.endTime)
    %Write IMU's position and velocity, rpy and accel and gyro bias to the files
    filter.SaveToFile([], preimutime, ffilres);
    if (imudata(1)-initime)>60
        disp(['Process Time:' num2str(imudata(1))]);
        initime=imudata(1);
    end
    % time propagation of the IMU
    imuaccum=filter.ffun_state(imuaccum,[imudata(2:7);preimutime;curimutime]);

    %Update the covariance
    if (((curimutime-covupt_time)>=options.maxCovStep) ||(curimutime>=imgepoch))
        %propagate the covariance
        filter.ffun_covariance(imuaccum, covupt_time, curimutime);
        covupt_time=curimutime;
        imuaccum=zeros(6,1);
        %Record covariance and navigation solution for the smoother
        %Note that I am recording the predicted solutions. That is why the
        %backward part must use filtered solution.
        imuerrors=filter.imuErrors;
        adjcoeff=ones(size(imuerrors));
        % since the scale factor is in ppt rather than in ppm, we need to
        % convert it to ppm
        adjcoeff(filter.imuScaleFactorSIP-filter.imuBiasDriftSIP+(1:6))=1e-3;
        outimuerrors=imuerrors.*adjcoeff;
        fwrite(fimures,[curimutime;outimuerrors;...
            full(sqrt(diag(filter.p_k_k(filter.imuBiasDriftSIP+(0:11),...
            filter.imuBiasDriftSIP+(0:11))))).*adjcoeff],'double');
    end
    %Apply ZUPT. Don't apply zupt for each imu sample. Zupt
    %for each sample must be performed on nominal trajectory, not with
    %the Kalman filter.
    isStatic =~isempty(options.zuptSE) && ~isempty(find(((options.zuptSE(:,1)<=curimutime)&(options.zuptSE(:,2)>=curimutime))==1,1));
    isZUPT =mod(imuctr, rateZUPT)==0;
    if (isStatic&&isZUPT)
        measure=zeros(9,1);
        predict=[filter.rvqs0(1:6); unskew(eye(3)-rotqr2ro(filter.rvqs0(7:10)))];
        H=sparse([eye(9) zeros(9,size(filter.p_k_k,1)-9)]);
        R=zeros(9);
        R(1:3,1:3)=eye(3)*options.sigmaZUPTPos^2;
        R(4:6,4:6)=eye(3)*options.sigmaZUPTVel^2;
        R(7:9,7:9)=eye(3)*options.sigmaZUPTAng^2;
        filter.correctstates(predict,measure, H,R);
    end
    % apply gravity norm measurement
    isGravNorm =mod(imuctr, rateGravNorm)==0;
    if (isStatic&&isGravNorm)
        %Gravity (most time consuming part of ecef implementations)
        xyz_imu=filter.rqs02e(1:3)+quatrot_v000(filter.rqs02e(4:7),filter.rvqs0(1:3),0);
        Llh=ecef2geo_v000(xyz_imu,0);
        [Rn, Re, gn, sL, cL, WIE_E]=geoparam_v000(Llh);
        predict=sqrt(imudata(2:4)'*imudata(2:4));
        gs0=quatrot_v000(filter.rvqs0(7:10), imudata(2:4),1); % predicted gravity in s0 frame
        H=[zeros(1,9) 1/predict*gs0' zeros(1,filter.covDim-12)];
        measure=norm(gn,2);
        R=sigmaGravMag^2;
        filter.correctstates(predict,measure, H,R);
    end

    % image measurement
    if(curimutime>=imgepoch)
        % there are some gap in the imu recordings
        if(abs(curimutime-imgepoch)>options.dt)
            disp(['abs(curimutime-imgepoch)>options.dt at ' num2str(curimutime)...
                ' GTOW sec and image index ' num2str(whichStep) '!']);
        end
        % predict the image cooridnates of points in the states
        RTcnc0=camdata(whichStep,2:7)';
        % 6x1 vector. RTcnc0(1:3) euler angle of Rc2c0, RTcnc0(4:6), Tc0 2c
        [Rc2c0, Tc02c , H]=filter.computeH();
        predict=[unskew(eye(3)-Rc2c0*(roteu2ro('xyz',RTcnc0(1:3)))', 0); Tc02c-RTcnc0(4:6)];
        euc2c0imu(whichStep, :)=[imgepoch; rotro2eu('xyz', Rc2c0)]';
        % predicted error =predicted value-measured value,
        % measured error= measured value- measured value= 0, dummy
        measure=zeros(6,1); % dummy

        R=diag([sigmaEuler.^2;sigmaTrans.^2]);
        filter.correctstates(predict,measure, H,R);
        filter.SaveCamPoseandRqs02e(imgepoch,whichStep);

        % take the next image time
        if(whichStep<size(camdata,1))
            whichStep=whichStep+1;
            imgepoch=camdata(whichStep,1);
        else
            imgepoch=inf; % stop using images
        end
    end
    %     filter.SaveCamPoseandRqs02e(curimutime,whichStep);
    %Read the next imu data
    if(preimudata.size()==numPrevImuDataToKeep)
        preimudata.removeFirst();
    end
    preimudata.addLast(imudata(:,end));    % record previous imudata
    preimutime=curimutime;
    [fimu, imudata]=grabnextimudata(fimu, preimutime, imuFileType);
    if (isempty(imudata))
        break;
    else
        curimutime=imudata(1,end);
        imuctr=imuctr+1;
    end
end
fprintf(' done.\n\n');
% draw Rc2c0 predicted and measured values
% nextFig=1;
% f(nextFig) = figure;
% plot(camdata(:,1)-camdata(1,1),camdata(:, 2:4)*180/pi,'marker','+');
% hold on
% plot(euc2c0imu(:,1)-euc2c0imu(1,1),euc2c0imu(:, 2:4)*180/pi,'marker','v');
% grid
% xlabel('Time [s]')
% ylabel('degree')
% legend('Roll cam','Pitch cam','Yaw/Heading cam','Roll imu','Pitch imu','Yaw/Heading imu');
% title('Cc2c0 by Kinect and predicted by IMU');

fclose all;
% display the navigation results
plotnav;
end

function RoTrKinect=load_KinectDataset(dataFile, startTime)
% output time, euler angle of Rc2c0, T c0 in c
fid = load(dataFile); %'PoseIMUKinect3.tsv');
DataIMU = fid((1:2:size(fid,1)-1),:);
EulerKin = fid((2:2:size(fid,1)),2:4);
TranKin = fid((2:2:size(fid,1)),5:7);
obsindex = zeros(size(EulerKin,1),1);
for i = 2:size(EulerKin,1)
    if EulerKin(i,1) ~= EulerKin(i-1,1)
        obsindex(i) = 1;
    end
end
whom=logical(obsindex)& (DataIMU(:,1)>=startTime);
RoTrKinect=[DataIMU(whom,1), EulerKin(whom, :), TranKin(whom, :)];
end


function [ patches, pdm, clmParams ] = Load_CLNF_wild()
%LOAD_CLNF_GENERAL Summary of this function goes here
%   Detailed explanation goes here
    clmParams = struct;

    clmParams.window_size = [25,25; 23,23; 21,21;21,21];
    clmParams.numPatchIters = size(clmParams.window_size,1);

    [patches] = Load_Patch_Experts( '../models/wild/', 'ccnf_patches_*_wild.mat', [], [], clmParams);

    % the default PDM to use
    pdmLoc = ['../models/pdm/pdm_68_aligned_wild.mat'];
    load(pdmLoc);

    pdm = struct;
    pdm.M = double(M);
    pdm.E = double(E);
    pdm.V = double(V);

    % the default model parameters to use
    clmParams.regFactor = [35, 27, 20, 20];
    clmParams.sigmaMeanShift = [1.25, 1.375, 1.5, 1.5];
    clmParams.tikhonov_factor = [2.5, 5, 7.5, 7.5];

    clmParams.startScale = 1;
    clmParams.num_RLMS_iter = 10;
    clmParams.fTol = 0.01;
    clmParams.useMultiScale = true;
    clmParams.use_multi_modal = 1;
    clmParams.multi_modal_types  = patches(1).multi_modal_types;

end


% *************************************************************************
% Video Super-Resolution with Convolutional Neural Networks
%
% This function calculates the motion compensated, bicubically upsampled
% input frames
%
%
% Version 1.0
%
% Created by:   Armin Kappeler
% Date:         02/19/2016
%
% *************************************************************************
function [input_data,idx_gt] = preprocess_frames(im_LR,upscale,MOTIONCOMPENSATION,ADAPTIVEMOTIONCOMPENSATION)

%% parameters
DParam.prescaling = 1;
DParam.upscaleFactor = upscale;
DParam.patchSize = 36;
DParam.stride = 36;
framesPerBlock = 5;

if ADAPTIVEMOTIONCOMPENSATION
    MOTIONCOMPENSATION = 1;
    ADAPT_WEIGHT = 8;
else
    ADAPT_WEIGHT = 0;
end

%% initialization
image_dims = [size(im_LR,1) size(im_LR,2)]*DParam.upscaleFactor;
nrFrames = size(im_LR,3);
framesOrder = 1:framesPerBlock;
framesOrder(floor(framesPerBlock/2)+1) = [];
framesOrder = [floor(framesPerBlock/2)+1 framesOrder];

nrSets = nrFrames-framesPerBlock+1;
tmpImgH5 = zeros([image_dims(2) image_dims(1) framesPerBlock+2 nrSets],'single');

%% loop through framesets (one frameset consists of 5 frames)
for i=1:nrSets
    display(['preprocessing frame ' num2str(i) ' of ' num2str(nrSets)])
    tic;
    %% loop through frames
    imgHi_bic_center = [];
    for layer=framesOrder

        %% load image
        fileIdx = i+layer-1;
        imgLo = im_LR(:,:,fileIdx);
        if isinteger(imgLo)
            imgLo = im2double(imgLo);
        end

        imgHi_bic = imresize(imgLo,DParam.upscaleFactor,'bicubic');

        %% motion compensation
        if MOTIONCOMPENSATION
            if layer == floor(framesPerBlock/2)+1 %center frame
                imgHi_bic_center = imgHi_bic;
            else

                [~,mc_err,~,~,imgHi_mc]=track_CLG_TV_adv(imgHi_bic_center,imgHi_bic,DParam,DParam);

                if ADAPT_WEIGHT > 0 %adaptive motion compensation
                    ratio_mc = exp(-mc_err./ADAPT_WEIGHT);
                    ratio_bic = ones(size(imgHi_bic)) - ratio_mc;

                    imgHi_bic = ratio_bic.*imgHi_bic_center + ratio_mc.*imgHi_mc;

                else
                    imgHi_bic = imgHi_mc;
                end
            end
        end

        %% convert into caffe-shape
        Xl_tmp = imgHi_bic;

        Xl_tmp = permute(Xl_tmp,[2 1 3 4]);
        tmpImgH5(:,:,layer,i) = Xl_tmp;

        %% calculate temporal image gradients -> not used for VSRnet
        if layer == framesPerBlock
            %first derriv.
            tmpImgH5(:,:,6,i) = tmpImgH5(:,:,2,i) - tmpImgH5(:,:,4,i);
            %second derriv.g
            tmpImgH5(:,:,7,i) = (tmpImgH5(:,:,1,i) + tmpImgH5(:,:,5,i) - 2*tmpImgH5(:,:,3,i));
        end

    end
    toc;
end

%% return values
input_data = tmpImgH5;
input_data = {input_data};
idx_gt = (1:nrSets) + floor(framesPerBlock/2);

end


function [mustUSet, posMustU] = findMustUWithGAMS(model, minFluxesW, maxFluxesW, varargin)
% This function runs the second step of `optForce`, that is to solve a
% bilevel mixed integer linear programming  problem to find a first order
% `MustU` set.
%
% USAGE:
%
%    [mustUSet, posMustU] = findMustUWithGAMS(model, minFluxesW, maxFluxesW, varargin)
%
%
% INPUTS:
%    model:                      (structure) a metabolic model with at
%                                least the following fields:
%
%                                     * .rxns - Reaction IDs in the model
%                                     * .mets - Metabolite IDs in the model
%                                     * .S - Stoichiometric matrix (sparse)
%                                     * .b - RHS of `Sv = b` (usually zeros)
%                                     * .c - Objective coefficients
%                                     * .lb - Lower bounds for fluxes
%                                     * .ub - Upper bounds for fluxes
%    minFluxesW:                 (double array of size `n_rxns x 1`) minimum
%                                fluxes for each reaction in the model for
%                                wild-type strain. This can be obtained by
%                                running the function `FVAOptForce` e.g.:
%                                `minFluxesW = [-90; -56];`
%    maxFluxesW:                 (double array of size `n_rxns x 1`) maximum
%                                fluxes for each reaction in the model for
%                                wild-type strain. This can be obtained by
%                                running the function `FVAOptForce` e.g.:
%                                `maxFluxesW = [90; 56];`
%
% OPTIONAL INPUTS:
%    constrOpt:                  (Structure) structure containing
%                                additional contraints. Include here only
%                                reactions whose flux is fixed, i.e.,
%                                reactions whose lower and upper bounds
%                                have the same value. Do not include here
%                                reactions whose lower and upper bounds
%                                have different values. Such contraints
%                                should be defined in the lower and upper
%                                bounds of the model. The structure has the
%                                following fields:
%
%                                     * .rxnList - Reaction list (cell array)
%                                     * .values - Values for constrained reactions (double array)
%                                       e.g.: `struct('rxnList',{{'EX_gluc','R75','EX_suc'}},'values',[-100,0,155.5]');`
%    solverName:                 (string) Name of the solver used in GAMS.
%                                Default = 'cplex'
%    runID:                      (string) ID for identifying this run
%    outputFolder:               (string) name for folder in which
%                                results will be stored
%    outputFileName:             (string) name for files in which results
%                                will be stored
%    printExcel:                 (double) boolean to describe wheter data
%                                must be printed in an excel file or not
%    printText:                  (double) boolean to describe wheter data
%                                must be printed in an plaint text file or
%                                not
%    printReport:                (double) 1 to generate a report in a
%                                plain text file. 0 otherwise.
%    keepInputs:                 (double) 1 to mantain folder with inputs
%                                to run `findMustUU.gms`. 0 otherwise.
%    keepGamsOutputs:            (double) 1 to mantain files returned by
%                                `findMustUU.gms`. 0 otherwise.
%    verbose:                    (double) 1 to print results in console.
%                                0 otherwise.
% OUTPUTS:
%    mustUSet:                   (cell array of size number of reactions
%                                found X 1) Cell array containing the
%                                reactions ID which belong to the `Must_U`
%                                Set
%    posMustU:                   (double array of size number of reactions
%                                found X 1)  double array containing the
%                                positions of reactions in the model.
%    outputFileName.xls:         (file) File containing one column array
%                                with identifiers for reactions in MustU.
%                                This file will only be generated if the
%                                user entered `printExcel = 1`. Note that the
%                                user can choose the name of this file
%                                entering the input `outputFileName = 'PutYourOwnFileNameHere';`
%    outputFileName.txt:         (file) File containing one column array
%                                with identifiers for reactions in MustU.
%                                This file will only be generated if the
%                                user entered `printText = 1`. Note that the
%                                user can choose the name of this file
%                                entering the input `outputFileName = 'PutYourOwnFileNameHere';`
%    outputFileName_Info.xls:    (file) File containing five column
%                                arrays.
%
%                                  * C1: identifiers for reactions in `MustU`
%                                  * C2: min fluxes for reactions according to FVA
%                                  * C3: max fluxes for reactions according to FVA
%                                  * C4: min fluxes achieved for reactions,
%                                    according to `findMustU.gms`
%                                  * C5: max fluxes achieved for reactions,
%                                    according to `findMustU.gms`
%                                This file will only be generated if the
%                                user entered `printExcel = 1`. Note that the
%                                user can choose the name of this file
%                                entering the input `outputFileName = 'PutYourOwnFileNameHere';`
%    outputFileName_Info.txt:    (file) File containing five column
%                                arrays.
%
%                                  * C1: identifiers for reactions in MustU
%                                  * C2: min fluxes for reactions according to FVA
%                                  * C3: max fluxes for reactions according to FVA
%                                  * C4: min fluxes achieved for reactions,
%                                    according to `findMustU.gms`
%                                  * C5: max fluxes achieved for reactions,
%                                    according to `findMustU.gms`
%                                This file will only be generated if the
%                                user entered `printText = 1`. Note that the
%                                user can choose the name of this file
%                                entering the input `outputFileName = 'PutYourOwnFileNameHere';`
%    findMustU.lst:              (file) file autogenerated by GAMS. It
%                                contains information about equations,
%                                variables, parameters as well as
%                                information about the running (values at
%                                each iteration). This file only will be
%                                saved in the output folder is the user
%                                entered `keepGamsOutputs = 1`
%    GtoMU.gdx:                  (file) file containing values for
%                                variables, parameters, etc. which were
%                                found by GAMS when solving `findMustU.gms`.
%                                This file only will be saved in the output
%                                folder is the user entered `keepInputs = 1`
%
% NOTE:
%
%    This function is based in the GAMS files written by Sridhar
%    Ranganathan which were provided by the research group of Costas D.
%    Maranas. For a detailed description of the `optForce` procedure, please
%    see: `Ranganathan S, Suthers PF, Maranas CD (2010) OptForce: An
%    Optimization Procedure for Identifying All Genetic Manipulations
%    Leading to Targeted Overproductions. PLOS Computational Biology 6(4):
%    e1000744`. https://doi.org/10.1371/journal.pcbi.1000744
%
% .. Author: - Sebastian Mendoza, May 30th 2017, Center for Mathematical Modeling, University of Chile, snmendoz@uc.cl

optionalParameters = {'constrOpt', 'solverName', 'runID', 'outputFolder', 'outputFileName',  ...
    'printExcel', 'printText', 'printReport', 'keepInputs', 'keepGamsOutputs', 'verbose'};

if (numel(varargin) > 0 && (~ischar(varargin{1}) || ~any(ismember(varargin{1},optionalParameters))))

    tempargin = cell(1,2*(numel(varargin)));
    for i = 1:numel(varargin)

        tempargin{2*(i-1)+1} = optionalParameters{i};
        tempargin{2*(i-1)+2} = varargin{i};
    end
    varargin = tempargin;

end

parser = inputParser();
parser.addRequired('model',@(x) isstruct(x) && isfield(x, 'S') && isfield(model, 'rxns')...
    && isfield(model, 'mets') && isfield(model, 'lb') && isfield(model, 'ub') && isfield(model, 'b')...
    && isfield(model, 'c'))
parser.addRequired('minFluxesW',@isnumeric)
parser.addRequired('maxFluxesW',@isnumeric)
parser.addParamValue('constrOpt', struct('rxnList', {{}}, 'values', []), @(x) isstruct(x) && isfield(x, 'rxnList') && isfield(x, 'values') ...
    && length(x.rxnList) == length(x.values) && length(intersect(x.rxnList, model.rxns)) == length(x.rxnList))
solvers = checkGAMSSolvers('MIP');
if isempty(solvers)
    error('there is no GAMS solvers available to solver Mixed Integer Programming problems') ;
else
    if ismember('cplex', lower(solvers))
        defaultSolverName = 'cplex';
    else
        defaultSolverName = lower(solvers(1));
    end
end
parser.addParamValue('solverName', defaultSolverName, @(x) ischar(x))
hour = clock; defaultRunID = ['run-' date '-' num2str(hour(4)) 'h' '-' num2str(hour(5)) 'm'];
parser.addParamValue('runID', defaultRunID, @(x) ischar(x))
parser.addParamValue('outputFolder', 'OutputsFindMustU', @(x) ischar(x))
parser.addParamValue('outputFileName', 'MustUSet', @(x) ischar(x))
parser.addParamValue('printExcel', 1, @(x) isnumeric(x) || islogical(x));
parser.addParamValue('printText', 1, @(x) isnumeric(x) || islogical(x));
parser.addParamValue('printReport', 1, @(x) isnumeric(x) || islogical(x));
parser.addParamValue('keepInputs', 1, @(x) isnumeric(x) || islogical(x));
parser.addParamValue('keepGamsOutputs', 1, @(x) isnumeric(x) || islogical(x));
parser.addParamValue('verbose', 1, @(x) isnumeric(x) || islogical(x));

parser.parse(model, minFluxesW, maxFluxesW, varargin{:})
model = parser.Results.model;
minFluxesW = parser.Results.minFluxesW;
maxFluxesW = parser.Results.maxFluxesW;
constrOpt= parser.Results.constrOpt;
solverName = parser.Results.solverName;
runID = parser.Results.runID;
outputFolder = parser.Results.outputFolder;
outputFileName = parser.Results.outputFileName;
printExcel = parser.Results.printExcel;
printText = parser.Results.printText;
printReport = parser.Results.printReport;
keepInputs = parser.Results.keepInputs;
keepGamsOutputs = parser.Results.keepGamsOutputs;
verbose = parser.Results.verbose;

% correct size of constrOpt
if ~isempty(constrOpt.rxnList)
    if size(constrOpt.rxnList, 1) > size(constrOpt.rxnList,2); constrOpt.rxnList = constrOpt.rxnList'; end;
    if size(constrOpt.values, 1) > size(constrOpt.values,2); constrOpt.values = constrOpt.values'; end;
end

% first, verify that GAMS is installed in your system
gamsPath = which('gams');
if isempty(gamsPath); error('OptForce: GAMS is not installed in your system. Please install GAMS.'); end;

%name of the function to solve the optimization problem in GAMS
gamsMustUFunction = 'findMustU.gms';
%path of that function
pathGamsFunction = which(gamsMustUFunction);
if isempty(pathGamsFunction); error(['optForce: ' gamsMustUFunction ' not in MATLAB path.']); end;
%current path
workingPath = pwd;
%go to the path associate to the ID for this run.
if ~isdir(runID); mkdir(runID); end; cd(runID);

% if the user wants to generate a report.
if printReport
    %create name for file.
    hour = clock;
    reportFileName = ['report-' date '-' num2str(hour(4)) 'h' '-' num2str(hour(5)) 'm.txt'];
    freport = fopen(reportFileName, 'w');
    reportClosed = 0;
    % print date of running.
    fprintf(freport, ['findMustUWithGAMS.m executed on ' date ' at ' num2str(hour(4)) ':' num2str(hour(5)) '\n\n']);
    % print matlab version.
    fprintf(freport, ['MATLAB: Release R' version('-release') '\n']);
    % print gams version.
    fprintf(freport, ['GAMS: ' regexprep(gamsPath, '\\', '\\\') '\n']);
    % print solver used in GAMS to solve optForce.
    fprintf(freport, ['GAMS solver: ' solverName '\n']);

    %print each of the inputs used in this running.
    fprintf(freport, '\nThe following inputs were used to run OptForce: \n');
    fprintf(freport, '\n------INPUTS------\n');
    %print model.
    fprintf(freport, '\nModel:\n');
    for i = 1:length(model.rxns)
        rxn = printRxnFormula(model, model.rxns{i}, false);
        fprintf(freport, [model.rxns{i} ': ' rxn{1} '\n']);
    end
    %print lower and upper bounds, minimum and maximum values for each of
    %the reactions in wild-type and mutant strain
    fprintf(freport, '\nLB\tUB\tMin_WT\tMax_WT\n');
    for i = 1:length(model.rxns)
        fprintf(freport, '%6.4f\t%6.4f\t%6.4f\t%6.4f\n', model.lb(i), model.ub(i), minFluxesW(i), maxFluxesW(i));
    end

    %print constraints
    fprintf(freport,'\nConstrained reactions:\n');
    for i = 1:length(constrOpt.rxnList)
        fprintf(freport,'%s: fixed in %6.4f\n', constrOpt.rxnList{i}, constrOpt.values(i));
    end

    fprintf(freport,'\nrunID(Main Folder): %s \n\noutputFolder: %s \n\noutputFileName: %s \n',...
        runID, outputFolder, outputFileName);

    fprintf(freport,'\nprintExcel: %1.0f \n\nprintText: %1.0f \n\nprintReport: %1.0f \n\nkeepInputs: %1.0f  \n\nkeepGamsOutputs: %1.0f \n\nverbose: %1.0f \n',...
        printExcel, printText, printReport, keepInputs, keepGamsOutputs, verbose);

end

copyfile(pathGamsFunction);

% export inputs for running the optimization problem in GAMS to find the
% MustU Set
inputFolder = 'InputsMustU';
exportInputsMustToGAMS(model, 'U', minFluxesW, maxFluxesW, constrOpt,inputFolder)

% create a directory to save results if this don't exist
if ~exist(outputFolder, 'dir')
    mkdir(outputFolder);
end

%run
if verbose
    run = system(['gams ' gamsMustUFunction ' lo=3 --myroot=' inputFolder '/ --solverName=' solverName ' gdx=GtoMU']);
else
    run = system(['gams ' gamsMustUFunction ' --myroot=' inputFolder '/ --solverName=' solverName ' gdx=GtoMU']);
end

if printReport; fprintf(freport, '\n------RESULTS------\n'); end;

%if user decide not to show inputs files for findMustU.gms
if ~keepInputs; rmdir(inputFolder,'s'); end;

%if findMustU.gms was executed correctly "run" should be 0
if run == 0
    if printReport; fprintf(freport, '\nGAMS was executed correctly\n'); end;
    if verbose; fprintf('GAMS was executed correctly\nSummary of information exported by GAMS:\n'); end;
    %show GAMS report in MATLAB console
    if verbose; gdxWhos GtoMU; end;

    %if the problem was solved correctly, a variable named findMustU should be
    %inside of GtoMU. Otherwise, the wrong file is being read.
    try
        findMustU.name = 'findMustU';
        rgdx('GtoMU', findMustU);
        if printReport; fprintf(freport, '\nGAMS variables were read by MATLAB correctly\n'); end;
        if verbose; fprintf('GAMS variables were read by MATLAB correctly\n'); end;

        %Using GDXMRW to read solutions found by findMustU.gms
        %extract must U set found by findMustU.gms
        must.name = 'must';
        must.compress = 'true';
        must = rgdx('GtoMU', must);
        uelsMust = must.uels{1};

        %if the set is not empty
        if ~isempty(uelsMust)
            if printReport; fprintf(freport, '\na MustU set was found\n'); end;
            if verbose; fprintf('a MustU set was found\n'); end;
            mustUSet = uelsMust';
            %find position for reactions of the MustU set in model.rxns
            posMustU = cell2mat(arrayfun(@(x)find(strcmp(x, model.rxns)), mustUSet, 'UniformOutput', false))';

            %find values of fluxes achieved by each reaction in MustU set
            minFlux = zeros(size(mustUSet));
            maxFlux = zeros(size(mustUSet));

            %extract minimum values
            vmin.name = 'vmin';
            vmin.compress = 'true';
            vmin = rgdx('GtoMU', vmin);
            uels_vmin = vmin.uels{1};
            if ~isempty(uels_vmin)
                val_vmin = vmin.val(:,2);
                for i = 1:length(uels_vmin)
                    pos = strcmp(mustUSet, uels_vmin{i});
                    minFlux(pos == 1) = val_vmin(i);
                end
            end

            %extract miximum values
            vmax.name = 'vmax';
            vmax.compress = 'true';
            vmax = rgdx('GtoMU', vmax);
            uels_vmax = vmax.uels{1};
            if ~isempty(uels_vmax)
                val_vmax = vmax.val(:,2);
                for i = 1:length(uels_vmax)
                    pos = strcmp(mustUSet, uels_vmax{i});
                    maxFlux(pos == 1) = val_vmax(i);
                end
            end
        else
            if printReport; fprintf(freport, '\na MustU set was not found\n'); end;
            if verbose; fprintf('a MustU set was not found\n'); end;
        end

        % print info into an excel file if required by the user
        if printExcel
            if ~isempty(mustUSet)
                currentFolder = pwd;
                cd(outputFolder);
                Info = [{'Reactions'},{'Min Flux in Wild-type strain'},{'Max Flux in Wild-type strain'},{'Min Flux in Mutant strain'},{'Max Flux in Mutant strain'}];
                Info = [Info; [mustUSet, num2cell(minFluxesW(posMustU)), num2cell(maxFluxesW(posMustU)), num2cell(minFlux) ,num2cell(maxFlux)]];
                xlswrite('MustU_Info', Info);
                xlswrite(outputFileName, mustUSet);
                cd(currentFolder);
                if verbose; fprintf(['MustU set was printed in ' outputFileName '.xls  \n']); end;
                if printReport; fprintf(freport, ['\nMustU set was printed in ' outputFileName '.xls  \n']); end;
            else
                if verbose; fprintf('No mustU set was found. Therefore, no excel file was generated\n'); end;
                if printReport; fprintf(freport, '\nNo mustU set was found. Therefore, no excel file was generated\n'); end;
            end
        end

        % print info into a plain text file if required by the user
        if printText
            if ~isempty(mustUSet)
                currentFolder = pwd;
                cd(outputFolder);
                f = fopen('MustU_Info.txt','w');
                fprintf(f, 'Reactions\tMin Flux in Wild-type strain\tMax Flux in Wild-type strain\tMin Flux in Mutant strain\tMax Flux in Mutant strain\n');
                for i = 1:length(posMustU)
                    fprintf(f, '%s\t%4.4f\t%4.4f\t%4.4f\t%4.4f\n', mustUSet{i}, minFluxesW(posMustU(i)), maxFluxesW(posMustU(i)), minFlux(i), maxFlux(i));
                end
                fclose(f);
                f = fopen([outputFileName '.txt'], 'w');
                for i = 1:length(posMustU)
                    fprintf(f, '%s\n', mustUSet{i});
                end
                fclose(f);
                cd(currentFolder);
                if verbose; fprintf(['MustU set was printed in ' outputFileName '.txt  \n']); end;
                if printReport; fprintf(freport, ['\nMustU set was printed in ' outputFileName '.txt  \n']); end;
            else
                if verbose; fprintf('No mustU set was found. Therefore, no excel file was generated\n'); end;
                if printReport; fprintf(freport, '\nNo mustU set was found. Therefore, no excel file was generated\n'); end;
            end
        end

        %close file for saving report
        if printReport; fclose(freport); reportClosed = 1; end;
        if printReport; movefile(reportFileName, outputFolder); end;
        delete(gamsMustUFunction);

        %remove or move additional files that were generated during running
        if keepGamsOutputs
            if ~isdir(outputFolder); mkdir(outputFolder); end;
            movefile('GtoMU.gdx',outputFolder);
            movefile(regexprep(gamsMustUFunction, 'gms', 'lst'), outputFolder);
        else
            delete('GtoMU.gdx');
            delete(regexprep(gamsMustUFunction, 'gms', 'lst'));
        end

        %go back to the original path
        cd(workingPath);

    catch
        %GAMS variables were not read correctly by MATLAB
        if verbose; fprintf('GAMS variables were not read by MATLAB corretly\n'); end;
        if printReport && ~reportClosed; fprintf(freport, '\nGAMS variables were not read by MATLAB corretly\n'); fclose(freport); end;
        cd(workingPath);
        error('OptForce: GAMS variables were not read by MATLAB corretly');
    end
else
    %if GAMS was not executed correcttly
    if printReport && ~reportClosed; fprintf(freport, '\nGAMS was not executed correctly\n'); fclose(freport); end;
    if verbose; fprintf('GAMS was not executed correctly\n'); end;
    cd(workingPath);
    error('OptForce: GAMS was not executed correctly');
end
end


function [f,J,Q] = spm_fx_gen(x,u,P,M)
% generic state equations for a neural mass models
% FORMAT [f,J,D] = spm_fx_gen(x,u,P,M)
% FORMAT [f,J]   = spm_fx_gen(x,u,P,M)
% FORMAT [f]     = spm_fx_gen(x,u,P,M)
% x  - neuronal states
% u  - exogenous input
% P  - model parameters
% M  - model structure
%
% This routine compiles equations of motion for multiple nodes or neural
% masses in the cell array of hidden states. To include a new sort of node,
% it is necessary to update the following routines:
%
% spm_dcm_neural_priors: to specify the intrinsic parameters of a new model
% spm_dcm_x_neural:      to specify its initial states
% spm_L_priors:          to specify which hidden states generate signal
% spm_fx_gen (below):    to specify how different models interconnect
%
% This routine deal separately with the coupling between nodes (that depend
% upon extrinsic connectivity, sigmoid activation functions and delays -
% and coupling within nodes (that calls on the model specific equations of
% motion).
%
% In generic schemes one can mix and match different types of sources;
% furthermore, they can have different condition-specific modulation of
% intrinsic connectivity parameters and different, source-specific-
% contribution to the lead field (or electrode gain). Source-specific
% models are specified by a structure array model, For the i-th source:
%
% model(i).source  = 'ECD','CMC',...  % source model
% model(i).B       = [i j k ,...]     % free parameters that have B effects
% model(i).J       = [i j k ,...]     % cardinal states contributing to L
% model(i).K       = [i j k ,...]     % other states contributing to L
% ...
%__________________________________________________________________________
% David O, Friston KJ (2003) A neural mass model for MEG/EEG: coupling and
% neuronal dynamics. NeuroImage 20: 1743-1755
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_fx_gen.m 7409 2018-08-27 11:39:00Z bernadette $

% model-specific parameters
%==========================================================================

% model or node-specific state equations of motions
%--------------------------------------------------------------------------
fx{1} = @spm_fx_erp;                     % ERP model
fx{2} = @spm_fx_cmc;                     % CMC model
fx{3} = @spm_fx_bgt;                     % basal ganglia circuit
fx{4} = @spm_fx_mmc;                     % motor micro circuit

% indices of extrinsically coupled hidden states
%--------------------------------------------------------------------------
efferent(1,:) = [9 9 9 9];               % sources of ERP connections
afferent(1,:) = [4 8 5 8];               % targets of ERP connections

efferent(2,:) = [3 3 7 7];               % sources of CMC connections
afferent(2,:) = [2 8 4 6];               % targets of CMC connections

efferent(3,:) = [9 9 9 9];               % sources of BGT connections (thalamus)
afferent(3,:) = [2 6 2 6];               % targets of BGT connections (striatum & STN)

efferent(4,:) = [3 3 7 7];               % sources of MMC connections
afferent(4,:) = [2 4 8 0];               % targets of MMC connections

% scaling of afferent extrinsic connectivity (Hz)
%--------------------------------------------------------------------------
E(1,:) = [64 2 32 8]*1000;               % ERP connections
E(2,:) = [64 4 64 8]*1000;               % CMC connections
E(3,:) = [1.8 1.2 1.8 1.2]*100000;       % BGC connections
E(4,:) = [.9 .9 .11 0]*100000;           % MMC connections

if isfield(M,'ERP_E'); E(1,:)= M.ERP_E; end
if isfield(M,'CMC_E'); E(2,:)= M.CMC_E; end
if isfield(M,'BGT_E'); E(3,:)= M.BGT_E; end
if isfield(M,'MMC_E'); E(4,:)= M.MMC_E; end

% intrinsic delays ms (scaling)
%--------------------------------------------------------------------------
D(1) = 2;                                % ERP connections
D(2) = 1;                                % CMC connections
D(3) = 4;                                % BGT connections
D(4) = 1;                                % MMC connections

% get model specific operators
%==========================================================================
if isvector(x)
    x = spm_unvec(x,M.x);                               % neuronal states
end

% get the neural mass models {'ERP','CMC'}
%--------------------------------------------------------------------------
n     = numel(x);
model = M.dipfit.model;
for i = 1:n
    if     strcmp(model(i).source,'ERP')
        nmm(i) = 1;
    elseif strcmp(model(i).source,'CMC')
        nmm(i) = 2;
    elseif strcmp(model(i).source,'BGT')
        nmm(i) = 3;
    elseif strcmp(model(i).source,'MMC')
        nmm(i) = 4;
    end
end

% synaptic activation function
%--------------------------------------------------------------------------
R     = 2/3;                      % gain of sigmoid activation function
B     = 0;                        % bias or background (sigmoid)
R     = R.*exp(P.S);              % posterior gain
S     = @(x,R,B)1./(1 + exp(-R*x(:) + B)) - 1/(1 + exp(B));
dSdx  = @(x,R,B)(R*exp(B - R*x(:)))./(exp(B - R*x(:)) + 1).^2;
for i = 1:n
    Sx{i} = S(x{i},R,B);          % sigmod firing rate function
end

% Extrinsic connections
%--------------------------------------------------------------------------
for i = 1:numel(P.A)
    A{i} = exp(P.A{i});
end

% detect and reduce the strength of reciprocal (lateral) connections
%--------------------------------------------------------------------------
TOL   = exp(-8);
for i = 1:numel(A)
    L    = (A{i} > TOL) & (A{i}' > TOL);
    A{i} = A{i}./(1 + 4*L);
end

% and scale of extrinsic connectivity (Hz)
%--------------------------------------------------------------------------
for i = 1:n
    for k = 1:numel(P.A)
        A{k}(i,:) = E(nmm(i),k)*A{k}(i,:);
    end
end

% assemble flow
%==========================================================================
N     = M;
for i = 1:n

    % intrinsic flow
    %----------------------------------------------------------------------
    N.x  = M.x{i};
    if isfield(M,'u')
        ui = u(i,:);
    else
        ui = u;
    end
    Q    = P.int{i};
    Q.C  = P.C(i,:);
    f{i} = fx{nmm(i)}(x{i},ui,Q,N);

    % extrinsic flow
    %----------------------------------------------------------------------
    for j = 1:n
        for k = 1:numel(P.A)
            if A{k}(i,j) > TOL
                ik       = afferent(nmm(i),k);
                jk       = efferent(nmm(j),k);
                f{i}(ik) = f{i}(ik) + A{k}(i,j)*Sx{j}(jk);
            end
        end
    end
end

% concatenate flow
%--------------------------------------------------------------------------
f  = spm_vec(f);

if nargout < 2; return, end

% Jacobian
%==========================================================================
for i = 1:n
    for j = 1:n

        J{i,j} = zeros(numel(x{i}),numel(x{j}));
        if i == j

            % intrinsic flow
            %--------------------------------------------------------------
            N.x    = M.x{i};
            if isfield(M,'u')
                ui = u(i,:);
            else
                ui = u;
            end
            Q      = P.int{i};
            Q.C    = P.C(i,:);
            J{i,i} = spm_diff(fx{nmm(i)},x{i},ui,Q,N,1);

        else

            % extrinsic flow
            %--------------------------------------------------------------
            for k = 1:numel(P.A)
                if A{k}(i,j) > TOL
                    ik  = afferent(nmm(i),k);
                    jk  = efferent(nmm(j),k);
                    dS  = dSdx(x{j}(jk),R,B);
                    J{i,j}(ik,jk) = J{i,j}(ik,jk) + A{k}(i,j)*dS;
                end
            end
        end
    end
end

J  = spm_cat(J);

if nargout < 3; return, end

% delays
%==========================================================================
% Delay differential equations can be integrated efficiently (but
% approximately) by absorbing the delay operator into the Jacobian
%
%    dx(t)/dt     = f(x(t - d))
%                 = Q(d)f(x(t))
%
%    J(d)         = Q(d)df/dx
%--------------------------------------------------------------------------
% Implement: dx(t)/dt = f(x(t - d)) = inv(1 + D.*dfdx)*f(x(t))
%                     = Q*f = Q*J*x(t)
%--------------------------------------------------------------------------

% source specific (intrinsic) delays
%--------------------------------------------------------------------------
for i = 1:n
    P.D(i,i) = P.D(i,i) + log(D(nmm(i)));
end

% N-th order Taylor approximation to delay
%--------------------------------------------------------------------------
N     = 256;
for i = 1:8
    Q = spm_dcm_delay(P,M,J,N);
    if isfinite(norm(Q,'inf')) && (norm(Q*J,'inf') < norm(J,'inf')*4)
        break
    else
        N = N/2;
    end
end

return


function [s, cfg] = ft_statfun_indepsamplesT(cfg, dat, design)

% FT_STATFUN_INDEPSAMPLEST calculates the independent samples T-statistic on the
% biological data in dat (the dependent variable), using the information on the
% independent variable (ivar) in design.
%
% Use this function by calling one of the high-level statistics functions as
%   [stat] = ft_timelockstatistics(cfg, timelock1, timelock2, ...)
%   [stat] = ft_freqstatistics(cfg, freq1, freq2, ...)
%   [stat] = ft_sourcestatistics(cfg, source1, source2, ...)
% with the following configuration option
%   cfg.statistic = 'ft_statfun_indepsamplesT'
%
% Configuration options
%   cfg.computestat    = 'yes' or 'no', calculate the statistic (default='yes')
%   cfg.computecritval = 'yes' or 'no', calculate the critical values of the test statistics (default='no')
%   cfg.computeprob    = 'yes' or 'no', calculate the p-values (default='no')
%
% The following options are relevant if cfg.computecritval='yes' and/or
% cfg.computeprob='yes'.
%   cfg.alpha = critical alpha-level of the statistical test (default=0.05)
%   cfg.tail  = -1, 0, or 1, left, two-sided, or right (default=1)
%               cfg.tail in combination with cfg.computecritval='yes'
%               determines whether the critical value is computed at
%               quantile cfg.alpha (with cfg.tail=-1), at quantiles
%               cfg.alpha/2 and (1-cfg.alpha/2) (with cfg.tail=0), or at
%               quantile (1-cfg.alpha) (with cfg.tail=1).
%
% Design specification
%   cfg.ivar  = row number of the design that contains the labels of the conditions that must be
%               compared (default=1). The labels are the numbers 1 and 2.
%
% See also FT_TIMELOCKSTATISTICS, FT_FREQSTATISTICS or FT_SOURCESTATISTICS

% Copyright (C) 2006, Eric Maris
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% set the defaults
cfg.computestat    = ft_getopt(cfg, 'computestat', 'yes');
cfg.computecritval = ft_getopt(cfg, 'computecritval', 'no');
cfg.computeprob    = ft_getopt(cfg, 'computeprob', 'no');
cfg.alpha          = ft_getopt(cfg, 'alpha', 0.05);
cfg.tail           = ft_getopt(cfg, 'tail', 1);

% perform some checks on the configuration
if strcmp(cfg.computeprob,'yes') && strcmp(cfg.computestat,'no')
  % probabilities can only be calculated if the test statistics are calculated
  cfg.computestat = 'yes';
end
if isfield(cfg,'uvar') && ~isempty(cfg.uvar)
  ft_error('cfg.uvar should not exist for an independent samples statistic');
end

% perform some checks on the design and data
sel1     = design(cfg.ivar,:)==1;
sel2     = design(cfg.ivar,:)==2;
nreplc1  = sum(~isnan(dat(:,sel1)), 2);
nreplc2  = sum(~isnan(dat(:,sel2)), 2);
nrepl    = nreplc1 + nreplc2;
hasnans1 = any(nreplc1<sum(sel1));
hasnans2 = any(nreplc2<sum(sel2));

if any(nrepl<size(design,2))
  ft_warning('Not all replications are used for the computation of the statistic.');
end
df = nrepl - 2;

if strcmp(cfg.computestat, 'yes')
  % compute the statistic use nanmean only if necessary
  if hasnans1
    avg1 = nanmean(dat(:,sel1), 2);
    var1 = nanvar(dat(:,sel1), 0, 2);
  else
    avg1 = mean(dat(:,sel1), 2);
    % var1 = var(dat(:,sel1), 0, 2);
    % the following achieves the same as the line above, but faster
    var1 = (sum(dat(:,sel1).^2,2)-(avg1.^2).*nreplc1)./(nreplc1-1);
  end
  if hasnans2
    avg2 = nanmean(dat(:,sel2), 2);
    var2 = nanvar(dat(:,sel2), 0, 2);
  else
    avg2 = mean(dat(:,sel2), 2);
    % var2 = var(dat(:,sel2), 0, 2);
    % the following achieves the same as the line above, but faster
    var2 = (sum(dat(:,sel2).^2,2)-(avg2.^2).*nreplc2)./(nreplc2-1);
  end

  varc = (1./nreplc1 + 1./nreplc2).*((nreplc1-1).*var1 + (nreplc2-1).*var2)./df;

  % in the case of non-equal triallengths, and tfrs as input-data nreplc are
  % vectors with different values. when the triallengths are equal, and the
  % input is a tfr, nreplc are vectors with either zeros (all trials contain nan
  % meaning that t_ftimwin did not fit around data), or the number of trials
  s.stat = (avg1 - avg2)./sqrt(varc);
end

if strcmp(cfg.computecritval,'yes')
  % also compute the critical values
  s.df      = df;
  if all(df==df(1)), df = df(1); end
  if cfg.tail==-1
    s.critval = tinv(cfg.alpha,df);
  elseif  cfg.tail==0
    s.critval = [tinv(cfg.alpha/2,df),tinv(1-cfg.alpha/2,df)];
  elseif cfg.tail==1
    s.critval = tinv(1-cfg.alpha,df);
  end
end

if strcmp(cfg.computeprob,'yes')
  % also compute the p-values
  s.df      = df;
  if cfg.tail==-1
    s.prob = tcdf(s.stat,s.df);
  elseif  cfg.tail==0
    s.prob = 2*tcdf(-abs(s.stat),s.df);
  elseif cfg.tail==1
    s.prob = 1-tcdf(s.stat,s.df);
  end
end


function  [isnr, y_hat] = Demo_IDDBM3D(experiment_number, test_image_name)
% ------------------------------------------------------------------------------------------
%
%     Demo software for BM3D-frame based image deblurring
%               Public release ver. 0.8 (beta) (June 03, 2011)
%
% ------------------------------------------------------------------------------------------
%
%  This function implements the IDDBM3D image deblurring algorithm proposed in:
%
%  [1] A.Danielyan, V. Katkovnik, and K. Egiazarian, "BM3D frames and
%   variational image deblurring," submitted to IEEE TIP, May 2011
%
% ------------------------------------------------------------------------------------------
%
% authors:               Aram Danielyan
%                        Vladimir Katkovnik
%
% web page:              http://www.cs.tut.fi/~foi/GCF-BM3D/
%
% contact:               firstname.lastname@tut.fi
%
% ------------------------------------------------------------------------------------------
% Copyright (c) 2011 Tampere University of Technology.
% All rights reserved.
% This work should be used for nonprofit purposes only.
% ------------------------------------------------------------------------------------------
%
% Disclaimer
% ----------
%
% Any unauthorized use of these routines for industrial or profit-oriented activities is
% expressively prohibited. By downloading and/or using any of these files, you implicitly
% agree to all the terms of the TUT limited license (included in the file Legal_Notice.txt).
% ------------------------------------------------------------------------------------------

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  FUNCTION INTERFACE:
%
%  [psnr, y_hat] = Demo_IDDBM3D(experiment_number, test_image_name)
%
%  INPUT:
%   1) experiment_number: 1 -> PSF 1, sigma^2 = 2
%                         2 -> PSF 1, sigma^2 = 8
%                         3 -> PSF 2, sigma^2 = 0.308
%                         4 -> PSF 3, sigma^2 = 49
%                         5 -> PSF 4, sigma^2 = 4
%                         6 -> PSF 5, sigma^2 = 64
%                         7-13 -> experiments 7-13 are not described in [1].
%                         see this file for the blur and noise parameters.
%   2) test_image_name:   a valid filename of a grayscale test image
%
%  OUTPUT:
%   1) isnr           the output improvement in SNR, dB
%   2) y_hat:         the restored image
%
%  ! The function can work without any of the input arguments,
%   in which case, the internal default ones are used !
%
%   To run this demo functions within the BM3D package should be accessible to Matlab
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
addpath('../')

if ~exist('experiment_number','var'), experiment_number=3; end
if ~exist('test_image_name','var'), test_image_name='Cameraman256.png'; end

filename=test_image_name;

if 1 %
    initType = 'bm3ddeb'; %use output of the BM3DDEB to initialize the algorithm
else
	initType = 'zeros'; %use zero image to initialize the algorithm
end

matchType = 'bm3ddeb'; %build groups using output of the BM3DDEB algorithm
numIt = 200;

fprintf('Experiment number: %d\n', experiment_number);
fprintf('Image: %s\n', filename);

%% ------- Generating bservation ---------------------------------------------
disp('--- Generating observation ----');
y=im2double(imread(filename));

[yN,xN]=size(y);

switch experiment_number
    case 1
        sigma=sqrt(2)/255;
        for x1=-7:7; for x2=-7:7; h(x1+8,x2+8)=1/(x1^2+x2^2+1); end, end; h=h./sum(h(:));
    case 2
        sigma=sqrt(8)/255;
        s1=0; for a1=-7:7; s1=s1+1; s2=0; for a2=-7:7; s2=s2+1; h(s1,s2)=1/(a1^2+a2^2+1); end, end;  h=h./sum(h(:));
    case 3
        BSNR=40;
        sigma=-1; % if "sigma=-1", then the value of sigma depends on the BSNR
        h=ones(9); h=h./sum(h(:));
    case 4
        sigma=7/255;
        h=[1 4 6 4 1]'*[1 4 6 4 1]; h=h./sum(h(:));  % PSF
    case 5
        sigma=2/255;
        h=fspecial('gaussian', 25, 1.6);
    case 6
        sigma=8/255;
        h=fspecial('gaussian', 25, .4);
    %extra experiments
    case 7
        BSNR=30;
        sigma=-1;
        h=ones(9); h=h./sum(h(:));
    case 8
        BSNR=20;
        sigma=-1;
        h=ones(9); h=h./sum(h(:));
    case 9
        BSNR=40;
        sigma=-1;
        h=fspecial('gaussian', 25, 1.6);
    case 10
        BSNR=20;
        sigma=-1;
        h=fspecial('gaussian', 25, 1.6);
    case 11
        BSNR=15;
        sigma=-1;
        h=fspecial('gaussian', 25, 1.6);
    case 12
        BSNR=40;
        sigma=-1; % if "sigma=-1", then the value of sigma depends on the BSNR
        h=ones(19); h=h./sum(h(:));
    case 13
        BSNR=25;
        sigma=-1; % if "sigma=-1", then the value of sigma depends on the BSNR
        h=ones(19); h=h./sum(h(:));
end

y_blur = imfilter(y, h, 'circular'); % performs blurring (by circular convolution)

if sigma == -1;   %% check whether to use BSNR in order to define value of sigma
    sigma=sqrt(norm(y_blur(:)-mean(y_blur(:)),2)^2 /(yN*xN*10^(BSNR/10)));
    %     Xv% compute sigma from the desired BSNR
end

%%%% Create a blurred and noisy observation
randn('seed',0);
z = y_blur + sigma*randn(yN, xN);

bsnr=10*log10(norm(y_blur(:)-mean(y_blur(:)),2)^2 /sigma^2/yN/xN);
psnr_z =PSNR(y,z,1,0);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

fprintf('Observation BSNR: %4.2f, PSNR: %4.2f\n', bsnr, psnr_z);

%% ----- Computing initial estimate ---------------------
disp('--- Computing initial estimate  ----');

[dummy, y_hat_RI,y_hat_RWI,zRI] = BM3DDEB_init(experiment_number, y, z, h, sigma);

switch lower(initType)
    case 'zeros'
        y_hat_init=zeros(size(z));
    case 'zri'
        y_hat_init=zRI;
    case 'ri'
        y_hat_init=y_hat_RI;
    case 'bm3ddeb'
        y_hat_init=y_hat_RWI;

end

switch lower(matchType)
    case 'z'
        match_im = z;
    case 'y'
        match_im = y;
    case 'zri'
        match_im = zRI;
    case 'ri'
        match_im = y_hat_RI;
    case 'bm3ddeb'
        match_im = y_hat_RWI;
end

psnr_init = PSNR(y, y_hat_init,1,0);

fprintf('Initialization method: %s\n', initType);
fprintf('Initial estimate ISNR: %4.2f, PSNR: %4.2f\n', psnr_init-psnr_z, psnr_init);

%% ------- Core algorithm ---------------------
%------ Description of the parameters of the IDDBM3D function ----------
%y - true image (use [] if true image is unavaliable)
%z - observed
%h - blurring PSF
%y_hat_init - initial estimate y_0
%match_im - image used to constuct groups and calculate weights g_r
%sigma - standard deviation of the noise
%threshType = 'h'; %use 's' for soft thresholding
%numIt - number of iterations
%gamma - regularization parameter see [1]
%tau - regularization parameter see [1] (thresholding level)
%xi - regularization parameter see [1], it is always set to 1 in this implementation
%showFigure - set to True to display figure with current estimate
%--------------------------------------------------------------------

threshType = 'h';
showFigure = true;

switch threshType
    case {'s'}
        gamma_tau_xi_inits= [
            0.0004509 0.70 1;%1
            0.0006803 0.78 1;%2
            0.0003485 0.65 1;%3
            0.0005259 0.72 1;%4
            0.0005327 0.82 1;%5
            7.632e-05 0.25 1;%6
            0.0005818 0.81 1;%7
            0.001149  1.18 1;%8
            0.0004155 0.74 1;%9
            0.0005591 0.74 1;%10
            0.0007989 0.82 1;%11
            0.0006702 0.75 1;%12
            0.001931  1.83 1;%13
        ];
    case {'h'}
        gamma_tau_xi_inits= [
            0.00051   3.13 1;%1
            0.0006004 2.75 1;%2
            0.0004573 2.91 1;%3
            0.0005959 2.82 1;%4
            0.0006018 3.63 1;%5
            0.0001726 2.24 1;%6
            0.00062   2.98 1;%7
            0.001047  3.80 1;%8
            0.0005125 3.00 1;%9
            0.0005685 2.80 1;%10
            0.0005716 2.75 1;%11
            0.0005938 2.55 1;%12
            0.001602  4.16 1;%13
        ];
end

gamma = gamma_tau_xi_inits(experiment_number,1);
tau   = gamma_tau_xi_inits(experiment_number,2)/255*2.7;
xi    = gamma_tau_xi_inits(experiment_number,3);

disp('-------- Start ----------');
fprintf('Number of iterations to perform: %d\n', numIt);
fprintf('Thresholding type: %s\n', threshType);

y_hat = IDDBM3D(y, h, z, y_hat_init, match_im, sigma, threshType, numIt, gamma, tau, xi, showFigure);
psnr = PSNR(y,y_hat,1,0);
isnr = psnr-psnr_z;

disp('-------- Results --------');
fprintf('Final estimate ISNR: %4.2f, PSNR: %4.2f\n', isnr, psnr);
return;

end

function PSNRdb = PSNR(x, y, maxval, borders)
    if ~exist('borders', 'var'), borders = 0; end
    if ~exist('maxval', 'var'), maxval = 255; end

    xx=borders+1:size(x,1)-borders;
    yy=borders+1:size(x,2)-borders;

    PSNRdb = zeros(1,size(x,3));
    for fr=1:size(x,3)
        err = x(xx,yy,fr) - y(xx,yy,fr);
        PSNRdb(fr) = 10 * log10((maxval^2)/mean2(err.^2));
    end
end


% LENGTH - length of memory mapped underlying array
%
% Author: Arnaud Delorme, SCCN, INC, UCSD, Nov. 2008

% Copyright (C) 2008 Arnaud Delorme, SCCN, INC, UCSD
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

function s = length(a)

    s = size(a,1);


function [] = showMultiSuperquadrics(x, c, arclength)

n = size(x, 1);
rng(5);
% color = rand(100, 3);

for i = 1 : n
%     c = color(i, :);
    showSuperquadrics(x(i, :), 'Color', c, ...
        'FaceAlpha', 1, 'ShowAxis', 1, ...
        'Light', false, 'Arclength', arclength);
    hold on
end
hold off
rng('shuffle')
end


function [scoreF,score0] = argmax_fit_type(Minit,lib,list_sid,verbose)
% ARGMAX_FIT_TYPE... Fit a parse to an image, with type-level parameters
% only
%
% Input
%   Minit : instance of MotorProgram class
%   list_sid: (default=all) list of stroke-ids which we want to optimize
%    while the others are frozen
%   verbose: (true/false) describe algorithm progress
%
% Output:
%  scoreF : final score
%  score0 : initial score
%

    M = Minit.copy();
    if ~exist('list_sid','var')
       list_sid = 1:M.ns;
    end
    if ~exist('verbose','var')
       verbose = false;
    end

    has_relations = M.has_relations(list_sid);
    if has_relations
        fmin = @(theta) myscore_HasRel(theta,M,lib,list_sid);
    else
        all_R = cache_enum_all_relations(lib,M);
        fmin = @(theta) myscore_NoRel(theta,M,lib,list_sid,all_R);
    end

    % Set-up the optimization problem
    [theta0,lb,ub] = model_to_vec_fit_type(M,list_sid);
    options = optimset('Display','off');
    if verbose
        options = optimset(options,'Display','iter');
    end
    options = optimset(options,'TolFun',1e-4,'Algorithm','active-set');

    % Run the optimization
    %tic
    score0 = -fmin(theta0);
    try
        thetaF = fmincon(fmin,theta0,[],[],[],[],lb,ub,[],options);
    catch
        fprintf(1,'*Warning*: optimization failed. Error caught.\n');
        thetaF = theta0;
    end
    scoreF = -fmin(thetaF);
    %t = toc;

    refill(thetaF,Minit,list_sid);

    % make sure we have not added/removed relations
    assert(Minit.has_relations==has_relations);

end

function refill(theta,M,list_sid)
    vec_to_model_fit_type(theta,M,list_sid);
end

% fill the MotorProgram with parameters
% and then score
function minscore = myscore_HasRel(theta,M,lib,list_sid)
    Q = M.copy(); % we don't want to modify the shared MotoProgram base
    refill(theta,Q,list_sid);
    ll = scoreMP(Q,lib,'strokes',list_sid,'type',true,'token',true,'stat',false,'image',true);
    minscore = -ll;
end

% fill the MotorProgram with parameters
% and then optimize the relations.
% Finally, return the score "minscore"
function minscore = myscore_NoRel(theta,M,lib,list_sid,all_R)
    Q = M.copy(); % we don't want to modify the shared MotoProgram base
    refill(theta,Q,list_sid);
    argmax_relations(lib,Q,all_R,list_sid);
    ll = scoreMP(Q,lib,'strokes',list_sid,'type',true,'token',true,'stat',false,'image',true);
    minscore = -ll;
end


function [cl,cl2] = hewma_plot_bivariate(varargin)
% Visualize a change_point map stored in hewma_cp.img
% and a run-length map stored in hewma_runlen.img
% (output from hewma2)
%
% :Usage:
% ::
%
%     [cl,cl2] = hewma_plot_bivariate([Method],[optional overlay image])
%
% and classify voxels into groupings based on locations in the bivariate
% space

Method = 'group';
if length(varargin), Method = varargin{1};, end
if length(varargin) > 1, ovl = varargin{2}; else, ovl = which('scalped_single_subj_T1.img');, end

doplot = 1;

% ----------------------------------------------------
% *
% *
% * get data, x, and list of all voxels to go with each data point, CLU
% *
% *
% ----------------------------------------------------
switch Method

    case 'group'

        mask = 'hewma_sig.img';
        t = 'hewma_t.img';

        name1 = 'hewma_cp.img';
        name2 = 'hewma_runlen.img';
        name2 = 'gausslongest.img';

    case 'difference'
        mask = 'hewma_sigdiff.img';
        t = 'hewma_tdiff.img';

        name1 = 'hewma_cpdiff.img';
        name2 = 'hewma_runlendiff.img';

    otherwise
        error('Method (first arg) must be group or difference.');
end

% ----------------------------------------------------
% initial viewing of sig map
% ----------------------------------------------------

cl = mask2clusters(mask,t);

if doplot, cluster_orthviews(cl,'overlay',ovl,'bivalent'); %,{[1 0 0]});,
    %cluster_orthviews(cl,{[1 0 0]},'add');,
    %cluster_orthviews(cl,{[1 0 0]},'add');,
end

CLU = clusters2CLU(cl);

% ----------------------------------------------------
% get mask for pos and neg activation
% ----------------------------------------------------
v = spm_read_vols(spm_vol(t));

% ----------------------------------------------------
% load data, get data for each voxel in CLU
% ----------------------------------------------------

v1 = spm_read_vols(spm_vol(name1));
v2 = spm_read_vols(spm_vol(name2));

for i = 1:size(CLU.XYZ,2)
    % data in voxel list
    x(i,1) = v1(CLU.XYZ(1,i),CLU.XYZ(2,i),CLU.XYZ(3,i));
    x(i,2) = v2(CLU.XYZ(1,i),CLU.XYZ(2,i),CLU.XYZ(3,i));
end

x = round(x);

go = input('Make tables?');
if go, mincs = input('Min cluster size?  (in voxels): ');, end

% ----------------------------------------------------
% POSITIVE
% ----------------------------------------------------

% select positive points and CLU
wh = find(CLU.Z > 0);
dat = x; dat = dat(wh,:);
CLU2 = CLU; CLU2.XYZ = CLU2.XYZ(:,wh); CLU2.XYZmm = CLU2.XYZmm(:,wh); CLU2.Z = CLU2.Z(:,wh);

[cl2,nclasses,colors] = cluster_kmeans_parcel(dat,CLU2,1);

% -------------------------------------------------------------------
% * tables
% -------------------------------------------------------------------

if go

    fprintf(1,'\nPositive deviations\n-----------------------------------\n');

    for i = 1:length(cl2);

        whomit = cat(1,cl2{i}.numVox); whomit(whomit >= mincs) = 0; cl2{i}(find(whomit)) = [];

        fprintf(1,'Class %3.0f, color = %s\n-----------------------------------\n',i,num2str(colors{i}));
        if isempty(cl2{i})
            fprintf(1,'No clusters large enough.\n');
        else
            cluster_table(cl2{i});
        end
        fprintf(1,'\n-----------------------------------\n')
    end
end

cl = cl2;

% ----------------------------------------------------
% NEGATIVE
% ----------------------------------------------------

dodecrease = input('Look at deactivations?');

if dodecrease

    % select negative points and CLU
    wh = find(CLU.Z < 0);
    dat = x; dat = dat(wh,:);
    CLU2 = CLU; CLU2.XYZ = CLU2.XYZ(:,wh); CLU2.XYZmm = CLU2.XYZmm(:,wh); CLU2.Z = CLU2.Z(:,wh);

    [cl2,nclasses,colors] = cluster_kmeans_parcel(dat,CLU2,1);

    % -------------------------------------------------------------------
    % * tables
    % -------------------------------------------------------------------

    if go

        fprintf(1,'\nNegative deviations\n-----------------------------------\n');

        for i = 1:length(cl2);

           whomit = cat(1,cl2{i}.numVox); whomit(whomit >= mincs) = 0; cl2{i}(find(whomit)) = [];

            fprintf(1,'Class %3.0f, color = %s\n-----------------------------------\n',i,num2str(colors{i}));
            if isempty(cl2{i})
                fprintf(1,'No clusters large enough.\n');
            else
                cluster_table(cl2{i});
            end
            fprintf(1,'\n-----------------------------------\n')
        end
    end

    cl = [cl cl2];

end

cl2 = cl; % cl2 has output in cell array, sep. by class
cl = cat(2,cl{:}); % cl is single vector

% -------------------------------------------------------------------
% * cluster extraction
% -------------------------------------------------------------------

go = input('Save timeseries in clusters?');

if go
    nm= 'hewma_timeseries.mat'; ctr = 1;

    while exist(nm) == 2
        nm = [nm(1:end-4) num2str(ctr) '.mat'];
        ctr = ctr+1;
    end
    fprintf(1,'Extracting cluster data and saving in %s\n',nm);
    mincs = input('Min cluster size?  (in voxels): ');
    whomit = cat(1,cl.numVox); whomit(whomit >= mincs) = 0; cl(find(whomit>0)) = [];

    cl = hewma_save_timeseries(cl,mincs);
    eval(['save ' nm ' cl']);

end

% -------------------------------------------------------------------
% * timeseries plotting
% -------------------------------------------------------------------

go = input('Plot timeseries interactively?');

if go
    if ~(exist('EXPT') == 1)
        file = spm_get(1,'*mat','Select EXPT.mat',pwd);
        [dd,ff,ee] = fileparts(file);
        cd(dd)
        load(file)
    end

    % used in button-up fcn callback
    E2 = EXPT;
    clear EXPT

    global VOL
    global f
    global f2
    global EXPT
    EXPT = E2;

    set(gcf,'WindowButtonUpFcn','[dat,files,stats,mycov] = hewma_plot_coord_btnupfcn;')

    % get coordinate mapping matrix
    VOL = struct('M',cl(1).M);

    % prepare figure
    f1 = figure('Color','w','Name','Hewma plots');
    f = f1;     % button callback uses figure f

    stop = input('Click on figure to plot.  Press return to exit.');

end

return


classdef TestBitwiseNot
    %TestBitwiseNot

    properties (Constant)
        im = fullfile(mexopencv.root(),'test','img001.jpg');
    end

    methods (Static)
        function test_rgb_image
            img = cv.imread(TestBitwiseNot.im, 'ReduceScale',2);

            % rectangular mask
            [h,w,~] = size(img);
            mask = false([h w]);
            mask(100:h-100,100:w-100) = true;

            out = cv.bitwise_not(img);
            validateattributes(out, {class(img)}, {'size',size(img)});
            expected = my_bitwise_not(img);
            assert(isequal(out, expected));

            out = cv.bitwise_not(img, 'Mask',mask);
            validateattributes(out, {class(img)}, {'size',size(img)});
            expected = my_bitwise_not(img, mask);
            assert(isequal(out, expected));

            out = cv.bitwise_not(img, 'Mask',mask, 'Dest',img);
            validateattributes(out, {class(img)}, {'size',size(img)});
            expected = my_bitwise_not(img, mask, img);
            assert(isequal(out, expected));
        end

        function test_float_image
            img = cv.imread(TestBitwiseNot.im, 'ReduceScale',2);
            img = single(img) ./ 255;

            % circular mask
            [h,w,~] = size(img);
            [X,Y] = meshgrid(1:w,1:h);
            c = fix([w h]/2); r = 50;
            mask = ((X-c(1)).^2 + (Y-c(2)).^2) < r^2;

            out = cv.bitwise_not(img);
            validateattributes(out, {class(img)}, {'size',size(img)});
            expected = my_bitwise_not(img);
            assert(isequaln(out, expected));

            out = cv.bitwise_not(img, 'Mask',mask);
            validateattributes(out, {class(img)}, {'size',size(img)});
            expected = my_bitwise_not(img, mask);
            assert(isequaln(out, expected));

            out = cv.bitwise_not(img, 'Mask',mask, 'Dest',img);
            validateattributes(out, {class(img)}, {'size',size(img)});
            expected = my_bitwise_not(img, mask, img);
            assert(isequaln(out, expected));
        end

        function test_vector
            src = uint8([1 2 3]);
            mask = [true false true];

            % uint8([254 253 252])
            out = cv.bitwise_not(src);
            validateattributes(out, {class(src)}, {'size',size(src)});
            expected = bitcmp(src);
            assert(isequal(out, expected));

            % uint8([254 0 252])
            out = cv.bitwise_not(src, 'Mask',mask);
            validateattributes(out, {class(src)}, {'size',size(src)});
            expected = bitcmp(src);
            expected(~mask) = 0;
            assert(isequal(out, expected));

            % uint8([254 2 252])
            out = cv.bitwise_not(src, 'Mask',mask, 'Dest',src);
            validateattributes(out, {class(src)}, {'size',size(src)});
            expected = bitcmp(src);
            expected(~mask) = src(~mask);
            assert(isequal(out, expected));
        end

        function test_error_argnum
            try
                cv.bitwise_not();
                throw('UnitTest:Fail');
            catch e
                assert(strcmp(e.identifier,'mexopencv:error'));
            end
        end
    end

end

function out = my_bitwise_not(src, mask, dst)
    %MY_BITWISE_NOT  Similar to cv.bitwise_not using core MATLAB functions

    if nargin < 2, mask = true(size(src,1), size(src,2)); end
    if nargin < 3, dst = zeros(size(src), class(src)); end

    % calculate bitwise NOT
    if isinteger(src)
        out = bitcmp(src);
    elseif isfloat(src)
        if isa(src, 'double')
            klass = 'uint64';
        elseif isa(src, 'single')
            klass = 'uint32';
        end
        out = bitcmp(typecast(src(:), klass));
        out = reshape(typecast(out, class(src)), size(src));
    end

    % apply masking
    for k=1:size(out,3)
        out_slice = out(:,:,k);
        dst_slice = dst(:,:,k);
        out_slice(~mask) = dst_slice(~mask);
        out(:,:,k) = out_slice;
    end
end


function planC = createPCimage(scanNum,structNumV,pcaParamsFile,planC)
% function createPCimage(scanNum,structNumV,pcaParamsFile,planC)
%
% APA, 2/4/2017

if ~iscell(scanNum)

    rowMargin = 100; % extend rows by this amount
    colMargin = 512; % extend cols by this amount
    slcMargin = 7; % extend slcss by this amount  %Changed from 15

    if ~exist('planC','var')
        global planC
    end

    indexS = planC{end};

    % get ROI
    randomFlg = 1;
    [volToEval,maskBoundingBox3M,mask3M,minr,maxr,minc,maxc,mins,maxs,uniqueSlices] = ...
        getROI(structNumV,rowMargin,colMargin,slcMargin,planC,randomFlg);

    sliceThickNessV = ...
        [planC{indexS.scan}(scanNum).scanInfo(mins:maxs).sliceThickness];
    [xVals, yVals, zVals] = getScanXYZVals(planC{indexS.scan}(scanNum));

else

    volToEval = scanNum{1};
    maskBoundingBox3M = scanNum{2};
    mask3M = scanNum{3};
    minr = scanNum{4};
    maxr = scanNum{5};
    minc = scanNum{6};
    maxc = scanNum{7};
    mins = scanNum{8};
    maxs = scanNum{9};
    uniqueSlices = scanNum{10};
    sliceThickNessV = scanNum{11};
    xVals = scanNum{12};
    yVals = scanNum{13};
    zVals = scanNum{14};
    rowMargin = [];
    colMargin = [];
    slcMargin = [];
end

minIntensity = -200;   % Clipping min
maxIntensity = 400; % Clipping max
harOnlyFlg = 1;
featFlagsV = [1,1,1,1,1,1,1,1,1];
numLevsV = 64;
patchRadiusV = [1,2];
compNumV = 1:2;

load(pcaParamsFile)

% Clip intensities
% volToEval(volToEval < minIntensity) = minIntensity;
% volToEval(volToEval > maxIntensity) = maxIntensity;
nanIntenityV = volToEval < -400;

%%% AA commented for writing cropped CT scans

% Get Law's and Haralick features for this structure
newFeaturesM = getLawsAndHaralickFeatures({volToEval,maskBoundingBox3M},...
    rowMargin,colMargin,slcMargin,minIntensity,maxIntensity,planC);
% rowMargin = [];
% colMargin = [];
% slcMargin = [];
% newFeaturesM = gpuGetLawsAndHaralickFeatures({volToEval,maskBoundingBox3M},...
%         rowMargin,colMargin,slcMargin,minIntensity,maxIntensity,planC,...
%         harOnlyFlg,featFlagsV,numLevsV,patchRadiusV);
newFeaturesM = bsxfun(@minus,newFeaturesM, featureMeanV);
newFeaturesM = bsxfun(@rdivide,newFeaturesM, featureStdV);
%newFeaturesM = newFeaturesM(:,indSignifV);

siz = size(maskBoundingBox3M);

% Parameters for storing component image to planC
deltaXYZv = [abs(xVals(1)-xVals(2)) abs(yVals(1)-yVals(2)) abs(zVals(1)-zVals(2))];
zV = zVals(mins:maxs);
regParamsS.horizontalGridInterval = deltaXYZv(1);
regParamsS.verticalGridInterval   = deltaXYZv(2); %(-)ve for dose
regParamsS.coord1OFFirstPoint   = xVals(minc);
%regParamsS.coord2OFFirstPoint   = yVals(minr); % for dose
regParamsS.coord2OFFirstPoint   = yVals(maxr);
regParamsS.zValues  = zV;
regParamsS.sliceThickness = sliceThickNessV;
assocTextureUID = '';
for compNum = compNumV

    %% Plot components, raw image and segmentation
    compV = zeros(prod(siz),1);

    compV(~nanIntenityV) = newFeaturesM * coeff(:,compNum) * 1.0 + newFeaturesM * coeff(:,2) * 0.0 + ...
        newFeaturesM * coeff(:,3) * 0.0 + newFeaturesM * coeff(:,4) * 0.0;

    %compV = max(compV) - compV; % reverse intensities

    compV(nanIntenityV) = min(compV);
    comp3M = reshape(compV,siz);

    % figure, imagesc(comp3M(:,:,30))
    % figure, imagesc(volToEval(:,:,30))

    %%% AA commented for writing cropped CT scans ends

    %%
    % Save comp3M to planC
    %dose2CERR(entropy3M,[], 'entropy3voxls_Ins3_NI14','test','test','non CT',regParamsS,'no',assocScanUID)
    planC = scan2CERR(comp3M,['PC_',num2str(compNum)],'Passed',regParamsS,assocTextureUID,planC);
    % planC = scan2CERR(volToEval,'PC','Passed',regParamsS,assocTextureUID,planC);

end

% for iFeat = 1:size(newFeaturesM,2)
%     compV = zeros(prod(siz),1);
%     compV(nanIntenityV) = min(compV);
%     compV(~nanIntenityV) = newFeaturesM(:,iFeat);
%         planC = scan2CERR(reshape(compV,siz),['Feat_',num2str(iFeat)],'Passed',regParamsS,assocTextureUID,planC);
% end


function sgmgg_print ( gg_ma, gg_mi, gg_mo, gg_na, gg_nd, gg_ni, gg_no, ...
  gg_a, gg_b, gg_f, gg_g, gg_i, gg_o, gg_s )

%*****************************************************************************80
%
%% SGMGG_PRINT prints out an SGMGG data structure.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 August 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer GG_MA, the maximum dimension for GG_A.
%
%    Input, integer GG_MI, the maximum dimension for GG_I.
%
%    Input, integer GG_MO, the maximum dimension for GG_O.
%
%    Input, integer GG_NA, the current dimension for GG_A.
%
%    Input, integer GG_ND, the spatial dimension.
%
%    Input, integer GG_NI, the current dimension for GG_I.
%
%    Input, integer GG_NO, the current dimension for GG_O.
%
%    Input, integer GG_A(GG_MA), the active indices.
%
%    Input, integer GG_B(GG_ND,GG_MI), the forward neighbors.
%
%    Input, integer GG_F(GG_ND,GG_MI), the backward neighbors.
%
%    Input, real GG_G(GG_MA), the error estimators.
%
%    Input, integer GG_I(GG_ND,GG_MI), the index set.
%
%    Input, integer GG_O(GG_MO), the old indices.
%
%    Input, integer GG_S(GG_MI), 0 if index I is old, 1
%    if it is active.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'GG DATA STRUCTURE:\n' );
  fprintf ( 1, '  ND = %d\n', gg_nd );
  fprintf ( 1, '  NI = %d\n', gg_ni );
  fprintf ( 1, '  NO = %d\n', gg_no );
  fprintf ( 1, '  NA = %d\n', gg_na );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Indices:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     I       A/O    G(I)        Index values\n' );
  fprintf ( 1, '\n' );
  for i = 1 : gg_ni
    if ( gg_s(i) == 0 )
      s = 'o';
    elseif ( gg_s(i) == 1 )
      s = 'a';
    else
      s = '?';
    end
    fprintf ( 1, '  %4d        %c  %14g', i, s, gg_g(i) );
    for j = 1 : gg_nd
      fprintf ( 1, '  %4d', gg_i(j,i) );
    end
    fprintf ( 1, '\n' );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Backward neighbors:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : gg_ni
    fprintf ( 1, '  %4d      ', i );
    for j = 1 : gg_nd
      fprintf ( 1, '  %4d', gg_b(j,i) );
    end
    fprintf ( 1, '\n' );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Forward neighbors:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : gg_ni
    fprintf ( 1, '  %4d      ', i );
    for j = 1 : gg_nd
      fprintf ( 1, '  %4d', gg_f(j,i) );
    end
    fprintf ( 1, '\n' );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Active Heap:\n' );
  fprintf ( 1, '     I     A      G\n' );
  fprintf ( 1, '\n' );

  for i = 1 : gg_na
    fprintf ( 1, '  %4d  %4d  %14g\n', i, gg_a(i), gg_g(gg_a(i)) );
  end

  return
end


function C = getcolor01(A)
%------------------------------------------------------------------------------
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: June 6, 1999.
% (c) 1999-2002 Stichting CWI, Amsterdam
%------------------------------------------------------------------------------
[n, m] = size(A);
C=A(1:2:n, 2:2:m);
%------------------------------------------------------------------------------


% This script stores in a file the standard configuration of RP for
% sampling from the 4 segmentations discussed in the paper. The parameters
% were trained with the VOC 2007 dataset. Further details can be found in
% the paper:
%
%"S. Manen, M. Guillaumin, and L. Van Gool. Prime Object Proposals with
%Randomized Prim's Algorithm. In ICCV, 2013."

function generateRPConfig_4segs(rppath)

	%% Parameter specification:
	params.approxFinalNBoxes = 10000;                                    %Approximate number of proposals
	params.rSeedForRun = -1;                                            %Random seed to be used (-1 to generate it with a hashing function)
	params.q = 10;                                                      %Parameter to eliminate near duplicates (raise it to eliminate more duplicates)

	% LAB segmentation
	params.segmentations{1}.colorspace = 'LAB';                         %Colorspace: 'RGB', 'LAB', 'opponent', 'rg', 'HSV'
	% --> Segmentation parameters:
	params.segmentations{1}.superpixels.sigma = 0.8;
	params.segmentations{1}.superpixels.c = 100;
	params.segmentations{1}.superpixels.min_size = 100;
	% --> Parameters trained from VOC07:
	%   --> Feature weights:
	params.segmentations{1}.simWeights.wBias = 3.0017;
	params.segmentations{1}.simWeights.wCommonBorder = -1.0029;
	params.segmentations{1}.simWeights.wLABColorHist = -2.6864;
	params.segmentations{1}.simWeights.wSizePer = -2.3655;
	%   --> Size term alpha, as explained in the paper sec. 4.2.
	%       It is quantized to contain exactly 2^16 elements for speed
	%       purposes.
	params.segmentations{1}.alpha = dlmread(fullfile(rppath,'config','alpha/alpha_voc07.dat'));
	params.segmentations{1}.verbose = false;                            %Set to true to display more information during execution

	% Opponent
	params.segmentations{2} = params.segmentations{1};
	params.segmentations{2}.colorspace = 'opponent';                    %Colorspace: 'RGB', 'LAB', 'opponent', 'rg', 'HSV'

	% rg
	params.segmentations{3} = params.segmentations{1};
	params.segmentations{3}.colorspace = 'rg';                          %Colorspace: 'RGB', 'LAB', 'opponent', 'rg', 'HSV'

	% Hue
	params.segmentations{4} = params.segmentations{1};
	params.segmentations{4}.colorspace = 'HSV';                         %Colorspace: 'RGB', 'LAB', 'opponent', 'rg', 'HSV'

	%% Save parameters:
	save(fullfile(rppath, 'config', 'rp_4segs.mat'),'params');
end


function [net, info] = cnn_train_adagrad_ms(net, imdb, getBatch, varargin)
% CNN_TRAIN   Demonstrates training a CNN
%    CNN_TRAIN() is an example learner implementing stochastic gradient
%    descent with momentum to train a CNN for image classification.
%    It can be used with different datasets by providing a suitable
%    getBatch function.

opts.train = [] ;
opts.val = [] ;
opts.numEpochs = 300 ;
opts.batchSize = 256 ;
opts.useGpu = false ;
opts.learningRate = 0.001 ;
opts.continue = false ;
opts.expDir = fullfile('data','exp') ;
opts.conserveMemory = false ;
opts.sync = true ;
opts.prefetch = false ;
opts.weightDecay = 0.0005 ;
opts.errorType = 'multiclass' ;
opts.plotDiagnostics = false ;
opts.delta = 1e-8;
opts.display = 1;
opts.snapshot = 1;
opts.test_interval = 1;
opts = vl_argparse(opts, varargin) ;

if ~exist(opts.expDir, 'dir'), mkdir(opts.expDir); end
if isempty(opts.train), opts.train = find(imdb.images.set==1); end
if isempty(opts.val), opts.val = find(imdb.images.set==2); end
if isnan(opts.train), opts.train = []; end

% -------------------------------------------------------------------------
%                                                    Network initialization
% -------------------------------------------------------------------------

for i=1:numel(net.layers)
    if ~strcmp(net.layers{i}.type,'conv'), continue; end
    net.layers{i}.filtersMomentum = zeros(size(net.layers{i}.filters), ...
        class(net.layers{i}.filters)) ;
    net.layers{i}.biasesMomentum = zeros(size(net.layers{i}.biases), ...
        class(net.layers{i}.biases)) ; %#ok<*ZEROLIKE>
    if ~isfield(net.layers{i}, 'filtersLearningRate')
        net.layers{i}.filtersLearningRate = 1 ;
    end
    if ~isfield(net.layers{i}, 'biasesLearningRate')
        net.layers{i}.biasesLearningRate = 1 ;
    end
    if ~isfield(net.layers{i}, 'filtersWeightDecay')
        net.layers{i}.filtersWeightDecay = 1 ;
    end
    if ~isfield(net.layers{i}, 'biasesWeightDecay')
        net.layers{i}.biasesWeightDecay = 1 ;
    end
end

if opts.useGpu
    net = vl_simplenn_move(net, 'gpu') ;
    for i=1:numel(net.layers)
        if ~strcmp(net.layers{i}.type,'conv'), continue; end
        net.layers{i}.filtersMomentum = gpuArray(net.layers{i}.filtersMomentum) ;
        net.layers{i}.biasesMomentum = gpuArray(net.layers{i}.biasesMomentum) ;
    end
end

G_f = cell(numel(net.layers), 1);
G_b = cell(numel(net.layers), 1);

for l=1:numel(net.layers)

    if ~strcmp(net.layers{l}.type, 'conv'), continue ; end

    G_f{l} = zeros(size(net.layers{l}.filters), 'single');
    G_b{l} = zeros(size(net.layers{l}.biases), 'single');

end

% -------------------------------------------------------------------------
%                                                        Train and validate
% -------------------------------------------------------------------------

rng(0) ;

if opts.useGpu
    one = gpuArray(single(1)) ;
else
    one = single(1) ;
end

info.train.objective = [] ;
info.train.error = [] ;
info.train.topFiveError = [] ;
info.train.speed = [] ;
info.val.objective = [] ;
info.val.error = [] ;
info.val.topFiveError = [] ;
info.val.speed = [] ;

lr = opts.learningRate ;
res = [] ;
for epoch=1:opts.numEpochs

    % fast-forward to where we stopped
    modelPath = @(ep) fullfile(opts.expDir, sprintf('net-epoch-%d.mat', ep));
    modelFigPath = fullfile(opts.expDir, 'net-train.pdf') ;
    if opts.continue
        if exist(modelPath(epoch),'file')
            if epoch == opts.numEpochs
                load(modelPath(epoch), 'net', 'info') ;
            end
            continue ;
        end
        if epoch > 1
            fprintf('resuming by loading epoch %d\n', epoch-1) ;
            load(modelPath(epoch-1), 'net', 'info') ;
        end
    end

    train = opts.train(randperm(numel(opts.train))) ;
    val = opts.val ;

    info.train.objective(end+1) = 0 ;
    info.train.error(end+1) = 0 ;
    info.train.topFiveError(end+1) = 0 ;
    info.train.speed(end+1) = 0 ;
    info.val.objective(end+1) = 0 ;
    info.val.error(end+1) = 0 ;
    info.val.topFiveError(end+1) = 0 ;
    info.val.speed(end+1) = 0 ;

    for t=1:opts.batchSize:numel(train)
        % get next image batch and labels
        batch = train(t:min(t+opts.batchSize-1, numel(train))) ;
        batch_time = tic ;
        fprintf('training: epoch %02d: processing batch %3d of %3d ...', epoch, ...
            fix(t/opts.batchSize)+1, ceil(numel(train)/opts.batchSize)) ;
        [im_ori, labels_ori] = getBatch(imdb, batch) ;
        if opts.prefetch
            nextBatch = train(t+opts.batchSize:min(t+2*opts.batchSize-1, numel(train))) ;
            getBatch(imdb, nextBatch) ;
        end

        for ss = 1 : numel(imdb.scales)

            [im, labels] = rescale_im(im_ori, labels_ori, imdb.scales(ss),...
                imdb.mask, imdb.half_size, imdb.meanPixel);

            if opts.useGpu
                im = gpuArray(im) ;
            end

            % backprop
            net.layers{end}.class = labels ;
            res = vl_simplenn(net, im, one, res, ...
                'conserveMemory', opts.conserveMemory, ...
                'sync', opts.sync) ;

            % gradient step
            for l=1:numel(net.layers)
                if ~strcmp(net.layers{l}.type, 'conv'), continue ; end

                g_f = (net.layers{l}.filtersLearningRate) * ...
                    (opts.weightDecay * net.layers{l}.filtersWeightDecay) * net.layers{l}.filters + ...
                    (net.layers{l}.filtersLearningRate) / numel(batch) * res(l).dzdw{1};
                g_b = (net.layers{l}.biasesLearningRate) * ...
                    (opts.weightDecay * net.layers{l}.biasesWeightDecay) * net.layers{l}.biases + ...
                    (net.layers{l}.biasesLearningRate) / numel(batch) * res(l).dzdw{2};

                G_f{l} = G_f{l} + g_f .^ 2;
                G_b{l} = G_b{l} + g_b .^ 2;

                net.layers{l}.filters = net.layers{l}.filters - lr ./ (opts.delta + sqrt(G_f{l})) .* g_f;
                net.layers{l}.biases  = net.layers{l}.biases - lr ./ (opts.delta + sqrt(G_b{l})) .* g_b;
            end
        end

        % print information
        batch_time = toc(batch_time) ;
        speed = numel(batch)/batch_time ;
        info.train = updateError(opts, info.train, net, res, batch_time) ;

        fprintf(' %.2f s (%.1f images/s)', batch_time, speed) ;
        n = t + numel(batch) - 1 ;
        switch opts.errorType
            case 'multiclass'
                fprintf(' err %.1f err5 %.1f', ...
                    info.train.error(end)/n*100, info.train.topFiveError(end)/n*100) ;
                fprintf('\n') ;
            case 'binary'
                fprintf(' err %.1f', ...
                    info.train.error(end)/n*100) ;
                fprintf('\n') ;
            case 'euclideanloss'
                fprintf(' err %.1f', info.train.error(end) / n);
                fprintf('\n') ;
        end

        % debug info
        if opts.plotDiagnostics
            figure(2) ; vl_simplenn_diagnose(net,res) ; drawnow ;
        end
    end % next batch

    % evaluation on validation set
    if epoch == 1 || rem(epoch, opts.test_interval) == 0 || epoch == opts.numEpochs
        for t=1:opts.batchSize:numel(val)
            batch_time = tic ;
            batch = val(t:min(t+opts.batchSize-1, numel(val))) ;
            fprintf('validation: epoch %02d: processing batch %3d of %3d ...', epoch, ...
                fix(t/opts.batchSize)+1, ceil(numel(val)/opts.batchSize)) ;
            [im, labels] = getBatch(imdb, batch) ;
            if opts.prefetch
                nextBatch = val(t+opts.batchSize:min(t+2*opts.batchSize-1, numel(val))) ;
                getBatch(imdb, nextBatch) ;
            end
            if opts.useGpu
                im = gpuArray(im) ;
            end

            net.layers{end}.class = labels ;
            res = vl_simplenn(net, im, [], res, ...
                'disableDropout', true, ...
                'conserveMemory', opts.conserveMemory, ...
                'sync', opts.sync) ;

            % print information
            batch_time = toc(batch_time) ;
            speed = numel(batch)/batch_time ;
            info.val = updateError(opts, info.val, net, res, batch_time) ;

            fprintf(' %.2f s (%.1f images/s)', batch_time, speed) ;
            n = t + numel(batch) - 1 ;
            switch opts.errorType
                case 'multiclass'
                    fprintf(' err %.1f err5 %.1f', ...
                        info.val.error(end)/n*100, info.val.topFiveError(end)/n*100) ;
                    fprintf('\n') ;
                case 'binary'
                    fprintf(' err %.1f', ...
                        info.val.error(end)/n*100) ;
                    fprintf('\n') ;
                case 'euclideanloss'
                    fprintf(' err %.1f', info.val.error(end) / n);
                    fprintf('\n') ;
            end
        end
    end

    % save
    info.train.objective(end) = info.train.objective(end) / numel(train) ;
    info.train.error(end) = info.train.error(end) / numel(train)  ;
    info.train.topFiveError(end) = info.train.topFiveError(end) / numel(train) ;
    info.train.speed(end) = numel(train) / info.train.speed(end) ;
    info.val.objective(end) = info.val.objective(end) / numel(val) ;
    info.val.error(end) = info.val.error(end) / numel(val) ;
    info.val.topFiveError(end) = info.val.topFiveError(end) / numel(val) ;
    info.val.speed(end) = numel(val) / info.val.speed(end) ;
    if epoch == 1 || rem(epoch, opts.snapshot) == 0 || epoch == opts.numEpochs
        save(modelPath(epoch), 'net', 'info') ;
    end

    if epoch == 1 || rem(epoch, opts.display) == 0 || epoch == opts.numEpochs
        figure(1) ; clf ;
        subplot(1,2,1) ;
        semilogy(1:epoch, info.train.objective, 'k') ; hold on ;
        semilogy([1 opts.test_interval : opts.test_interval : epoch epoch], info.val.objective([1 opts.test_interval : opts.test_interval : epoch epoch]), 'b') ;
        xlabel('training epoch') ; ylabel('energy') ;
        grid on ;
        h=legend('train', 'val') ;
        set(h,'color','none');
        title('objective') ;
        subplot(1,2,2) ;
        switch opts.errorType
            case 'multiclass'
                plot(1:epoch, info.train.error, 'k') ; hold on ;
                plot(1:epoch, info.train.topFiveError, 'k--') ;
                plot([1 opts.test_interval : opts.test_interval : epoch epoch], info.val.error([1 opts.test_interval : opts.test_interval : epoch epoch]), 'b') ;
                plot([1 opts.test_interval : opts.test_interval : epoch epoch], info.val.topFiveError([1 opts.test_interval : opts.test_interval : epoch epoch]), 'b--') ;
                h=legend('train','train-5','val','val-5') ;
            case 'binary'
                plot(1:epoch, info.train.error, 'k') ; hold on ;
                plot([1 opts.test_interval : opts.test_interval : epoch epoch], info.val.error([1 opts.test_interval : opts.test_interval : epoch epoch]), 'b') ;
                h=legend('train','val') ;
            case 'euclideanloss'
                plot(1 : epoch, info.train.error, 'k'); hold on;
                plot([1 opts.test_interval : opts.test_interval : epoch epoch], info.val.error([1 opts.test_interval : opts.test_interval : epoch epoch]), 'b') ;
                h = legend('train', 'val') ;
        end
        grid on ;
        xlabel('training epoch') ; ylabel('error') ;
        set(h,'color','none') ;
        title('error') ;
        drawnow ;
        print(1, modelFigPath, '-dpdf') ;
    end

end
end

% -------------------------------------------------------------------------
function info = updateError(opts, info, net, res, speed)
% -------------------------------------------------------------------------
predictions = gather(res(end-1).x) ;
sz = size(predictions) ;
n = prod(sz(1:2)) ;

labels = net.layers{end}.class ;
info.objective(end) = info.objective(end) + sum(double(gather(res(end).x))) ;
info.speed(end) = info.speed(end) + speed ;
switch opts.errorType
    case 'multiclass'
        [~,predictions] = sort(predictions, 3, 'descend') ;
        error = ~bsxfun(@eq, predictions, reshape(labels, 1, 1, 1, [])) ;
        info.error(end) = info.error(end) +....
            sum(sum(sum(error(:,:,1,:))))/n ;
        info.topFiveError(end) = info.topFiveError(end) + ...
            sum(sum(sum(min(error(:,:,1:5,:),[],3))))/n ;
    case 'binary'

        labels = labels(:,:,1,:);
        [~,predictions] = sort(predictions, 3, 'descend') ;

        predictions = predictions(:,:,1,:);
        error = ~bsxfun(@eq, predictions, labels) ;
        info.error(end) = info.error(end) + sum(error(:))/n ;
    case 'euclideanloss'
        error = euclideanloss(sigmoid(predictions), labels);
        info.error(end) = info.error(end) + error;
end
end

function [im, labels] = rescale_im(im_ori, label_ori, scale, mask, half_size, meanPixel)

mask = imresize(mask, scale, 'nearest');

for i = 1:size(im_ori,4)
    im_ii = imresize(im_ori(:,:,:,i), scale, 'nearest');
    im_large = padarray(im_ii, [half_size, half_size], 'symmetric');
    im_ii = bsxfun(@minus, im_large, meanPixel);

    label_ii = imresize(label_ori(:,:,:,i), scale, 'nearest');

    im(:, :, :, i) = im_ii;
    labels(:, :, 1, i) = label_ii;
    labels(:, :, 2, i) = double(mask);

end
end