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

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

        nameAbbreviation = 'LogDisc'  % LogDisc

        isNativeMary = false;  % True
    end

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

        w = [];  % Regression weights

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

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

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

        irlsStepSize = .05; % The stepsize

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

        maxIter = 500;  % Maxmimuum number of iterations

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

        wInitTechnique = 'FLD';  % Weight initialization technique

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

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

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

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

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

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

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

    end

    methods

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

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

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

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

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

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

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

    end

    methods (Access=protected, Hidden = true)

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

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

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

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

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

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

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

            converged = 0;
            nIter = 1;

            while ~converged
                Obj.nIterations = nIter;

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

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

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

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

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

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

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

        function self = trainActionBig(self,ds)

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

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

            for iter = 1:nMaxIterations

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

                % cBlockDs is now our dataset

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

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

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

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

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

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

                wOld = cW;

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

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

            self.w = cW;
        end

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

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


function [lin_fun, max_lin_fun] = RobertsonCRF(stack, stack_exposure, max_iterations, err_threshold, bNormalize)
%
%       [lin_fun, max_lin_fun] = RobertsonCRF(stack, stack_exposure, max_iterations, err_threshold, bNormalize)
%
%       This function computes camera response function using Mitsunaga and
%       Nayar method.
%
%        Input:
%           -stack: a stack of LDR images. If the stack is a single or
%           double values are assumed to be in [0,1].
%           -stack_exposure: an array containg the exposure time of each
%           image. Time is expressed in second (s)
%           -max_iterations: max number of iterations
%           -err_threshold: threshold after which the function can stop
%           -bNormalize: if 1 it enables function normalization
%
%        Output:
%           -lin_fun: tabled function
%           -max_lin_fun: maximum value of the inverse CRF
%
%     Copyright (C) 2016  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

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

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

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

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

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

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

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

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

col = size(stack, 3);

lin_fun = zeros(256, col);

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

global minM;
global maxM;

x = (0:255) / 255;

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

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

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

global stack_min;
global stack_max;

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

global minSatTime;

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

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

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

for i=1:max_iterations
    lin_fun_prev = lin_fun;

    %normalization CRF
    lin_fun = Normalization(lin_fun);

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

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

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

    %disp([i, err]);

    if(err < err_threshold)
        break;
    end
end

max_lin_fun = max(lin_fun(:));

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

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

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

end

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

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

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

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

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

        mid = lf(index);

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

            mid = lf(index);
        end

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

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

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

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

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

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

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

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

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

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

        tmpStack = tabledFunction(tmpStack, lin_fun);

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

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

    imgOut = imgOut ./ totWeight;

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

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

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

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

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

end

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

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

    global minSatTime;

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

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

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

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

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

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

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

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

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

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

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

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

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

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


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    % use solve_spherical_wrist2 for the particular orientation
    % of the systems in this ABB robot
    % use either the geometric or algebraic method.
    % the function solve_spherical_wrist2 is used due to the relative
    % orientation of the last three DH reference systems.

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

    qtemp = solve_spherical_wrist_Prosix(robot, q(:,i), T, -1); %wrist down

    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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


function wss_dist= comp_wss(cleanFile, enhancedFile);
% ----------------------------------------------------------------------
%
%     Weighted Spectral Slope (WSS) Objective Speech Quality Measure
%
%     This function implements the Weighted Spectral Slope (WSS)
%     distance measure originally proposed in [1].  The algorithm
%     works by first decomposing the speech signal into a set of
%     frequency bands (this is done for both the test and reference
%     frame).  The intensities within each critical band are
%     measured.  Then, a weighted distances between the measured
%     slopes of the log-critical band spectra are computed.
%     This measure is also described in Section 2.2.9 (pages 56-58)
%     of [2].
%
%     Whereas Klatt's original measure used 36 critical-band
%     filters to estimate the smoothed short-time spectrum, this
%     implementation considers a bank of 25 filters spanning
%     the 4 kHz bandwidth.
%
%   Usage:  wss_dist=comp_wss(cleanFile.wav, enhancedFile.wav)
%
%         cleanFile.wav - clean input file in .wav format
%         enhancedFile  - enhanced output file in .wav format
%         wss_dist      - computed spectral slope distance
%
%  Example call:  ws =comp_wss('sp04.wav','enhanced.wav')
%
%  References:
%
%     [1] D. H. Klatt, "Prediction of Perceived Phonetic Distance
%	    from Critical-Band Spectra: A First Step", Proc. IEEE
%	    ICASSP'82, Volume 2, pp. 1278-1281, May, 1982.
%
%     [2] S. R. Quackenbush, T. P. Barnwell, and M. A. Clements,
%	    Objective Measures of Speech Quality.  Prentice Hall
%	    Advanced Reference Series, Englewood Cliffs, NJ, 1988,
%	    ISBN: 0-13-629056-6.
%
%  Authors: Bryan L. Pellom and John H. L. Hansen (July 1998)
%  Modified by: Philipos C. Loizou  (Oct 2006)
%
% Copyright (c) 2006 by Philipos C. Loizou
% $Revision: 0.0 $  $Date: 10/09/2006 $
%
% ----------------------------------------------------------------------
if nargin~=2
    fprintf('USAGE: WSS=comp_wss(cleanFile.wav, enhancedFile.wav)\n');
    fprintf('For more help, type: help comp_wss\n\n');
    return;
end

alpha= 0.95;

[data1, Srate1, Nbits1]= wavread(cleanFile);
[data2, Srate2, Nbits2]= wavread(enhancedFile);
if ( Srate1~= Srate2) | ( Nbits1~= Nbits2)
    error( 'The two files do not match!\n');
end

len= min( length( data1), length( data2));
data1= data1( 1: len)+eps;
data2= data2( 1: len)+eps;

wss_dist_vec= wss( data1, data2,Srate1);
wss_dist_vec= sort( wss_dist_vec);
wss_dist= mean( wss_dist_vec( 1: round( length( wss_dist_vec)*alpha)));

function distortion = wss(clean_speech, processed_speech,sample_rate)

% ----------------------------------------------------------------------
% Check the length of the clean and processed speech.  Must be the same.
% ----------------------------------------------------------------------

clean_length      = length(clean_speech);
processed_length  = length(processed_speech);

if (clean_length ~= processed_length)
  disp('Error: Files  musthave same length.');
  return
end

% ----------------------------------------------------------------------
% Global Variables
% ----------------------------------------------------------------------

winlength   = round(30*sample_rate/1000); 	   % window length in samples
skiprate    = floor(winlength/4);		   % window skip in samples
max_freq    = sample_rate/2;	   % maximum bandwidth
num_crit    = 25;		   % number of critical bands

USE_FFT_SPECTRUM = 1;		   % defaults to 10th order LP spectrum
n_fft       = 2^nextpow2(2*winlength);
n_fftby2    = n_fft/2;		   % FFT size/2
Kmax        = 20;		   % value suggested by Klatt, pg 1280
Klocmax     = 1;		   % value suggested by Klatt, pg 1280

% ----------------------------------------------------------------------
% Critical Band Filter Definitions (Center Frequency and Bandwidths in Hz)
% ----------------------------------------------------------------------

cent_freq(1)  = 50.0000;   bandwidth(1)  = 70.0000;
cent_freq(2)  = 120.000;   bandwidth(2)  = 70.0000;
cent_freq(3)  = 190.000;   bandwidth(3)  = 70.0000;
cent_freq(4)  = 260.000;   bandwidth(4)  = 70.0000;
cent_freq(5)  = 330.000;   bandwidth(5)  = 70.0000;
cent_freq(6)  = 400.000;   bandwidth(6)  = 70.0000;
cent_freq(7)  = 470.000;   bandwidth(7)  = 70.0000;
cent_freq(8)  = 540.000;   bandwidth(8)  = 77.3724;
cent_freq(9)  = 617.372;   bandwidth(9)  = 86.0056;
cent_freq(10) = 703.378;   bandwidth(10) = 95.3398;
cent_freq(11) = 798.717;   bandwidth(11) = 105.411;
cent_freq(12) = 904.128;   bandwidth(12) = 116.256;
cent_freq(13) = 1020.38;   bandwidth(13) = 127.914;
cent_freq(14) = 1148.30;   bandwidth(14) = 140.423;
cent_freq(15) = 1288.72;   bandwidth(15) = 153.823;
cent_freq(16) = 1442.54;   bandwidth(16) = 168.154;
cent_freq(17) = 1610.70;   bandwidth(17) = 183.457;
cent_freq(18) = 1794.16;   bandwidth(18) = 199.776;
cent_freq(19) = 1993.93;   bandwidth(19) = 217.153;
cent_freq(20) = 2211.08;   bandwidth(20) = 235.631;
cent_freq(21) = 2446.71;   bandwidth(21) = 255.255;
cent_freq(22) = 2701.97;   bandwidth(22) = 276.072;
cent_freq(23) = 2978.04;   bandwidth(23) = 298.126;
cent_freq(24) = 3276.17;   bandwidth(24) = 321.465;
cent_freq(25) = 3597.63;   bandwidth(25) = 346.136;

bw_min      = bandwidth (1);	   % minimum critical bandwidth

% ----------------------------------------------------------------------
% Set up the critical band filters.  Note here that Gaussianly shaped
% filters are used.  Also, the sum of the filter weights are equivalent
% for each critical band filter.  Filter less than -30 dB and set to
% zero.
% ----------------------------------------------------------------------

min_factor = exp (-30.0 / (2.0 * 2.303));       % -30 dB point of filter

for i = 1:num_crit
  f0 = (cent_freq (i) / max_freq) * (n_fftby2);
  all_f0(i) = floor(f0);
  bw = (bandwidth (i) / max_freq) * (n_fftby2);
  norm_factor = log(bw_min) - log(bandwidth(i));
  j = 0:1:n_fftby2-1;
  crit_filter(i,:) = exp (-11 *(((j - floor(f0)) ./bw).^2) + norm_factor);
  crit_filter(i,:) = crit_filter(i,:).*(crit_filter(i,:) > min_factor);
end

% ----------------------------------------------------------------------
% For each frame of input speech, calculate the Weighted Spectral
% Slope Measure
% ----------------------------------------------------------------------

num_frames = clean_length/skiprate-(winlength/skiprate); % number of frames
start      = 1;					% starting sample
window     = 0.5*(1 - cos(2*pi*(1:winlength)'/(winlength+1)));

for frame_count = 1:num_frames

   % ----------------------------------------------------------
   % (1) Get the Frames for the test and reference speech.
   %     Multiply by Hanning Window.
   % ----------------------------------------------------------

   clean_frame = clean_speech(start:start+winlength-1);
   processed_frame = processed_speech(start:start+winlength-1);
   clean_frame = clean_frame.*window;
   processed_frame = processed_frame.*window;

   % ----------------------------------------------------------
   % (2) Compute the Power Spectrum of Clean and Processed
   % ----------------------------------------------------------

    if (USE_FFT_SPECTRUM)
       clean_spec     = (abs(fft(clean_frame,n_fft)).^2);
       processed_spec = (abs(fft(processed_frame,n_fft)).^2);
    else
       a_vec = zeros(1,n_fft);
       a_vec(1:11) = lpc(clean_frame,10);
       clean_spec     = 1.0/(abs(fft(a_vec,n_fft)).^2)';

       a_vec = zeros(1,n_fft);
       a_vec(1:11) = lpc(processed_frame,10);
       processed_spec = 1.0/(abs(fft(a_vec,n_fft)).^2)';
    end

   % ----------------------------------------------------------
   % (3) Compute Filterbank Output Energies (in dB scale)
   % ----------------------------------------------------------

   for i = 1:num_crit
      clean_energy(i) = sum(clean_spec(1:n_fftby2) ...
		            .*crit_filter(i,:)');
      processed_energy(i) = sum(processed_spec(1:n_fftby2) ...
			        .*crit_filter(i,:)');
   end
   clean_energy = 10*log10(max(clean_energy,1E-10));
   processed_energy = 10*log10(max(processed_energy,1E-10));

   % ----------------------------------------------------------
   % (4) Compute Spectral Slope (dB[i+1]-dB[i])
   % ----------------------------------------------------------

   clean_slope     = clean_energy(2:num_crit) - ...
		     clean_energy(1:num_crit-1);
   processed_slope = processed_energy(2:num_crit) - ...
		     processed_energy(1:num_crit-1);

   % ----------------------------------------------------------
   % (5) Find the nearest peak locations in the spectra to
   %     each critical band.  If the slope is negative, we
   %     search to the left.  If positive, we search to the
   %     right.
   % ----------------------------------------------------------

   for i = 1:num_crit-1

       % find the peaks in the clean speech signal

       if (clean_slope(i)>0) 		% search to the right
	  n = i;
          while ((n<num_crit) & (clean_slope(n) > 0))
	     n = n+1;
 	  end
	  clean_loc_peak(i) = clean_energy(n-1);
       else				% search to the left
          n = i;
	  while ((n>0) & (clean_slope(n) <= 0))
	     n = n-1;
 	  end
	  clean_loc_peak(i) = clean_energy(n+1);
       end

       % find the peaks in the processed speech signal

       if (processed_slope(i)>0) 	% search to the right
	  n = i;
          while ((n<num_crit) & (processed_slope(n) > 0))
	     n = n+1;
	  end
	  processed_loc_peak(i) = processed_energy(n-1);
       else				% search to the left
          n = i;
	  while ((n>0) & (processed_slope(n) <= 0))
	     n = n-1;
 	  end
	  processed_loc_peak(i) = processed_energy(n+1);
       end

   end

   % ----------------------------------------------------------
   %  (6) Compute the WSS Measure for this frame.  This
   %      includes determination of the weighting function.
   % ----------------------------------------------------------

   dBMax_clean       = max(clean_energy);
   dBMax_processed   = max(processed_energy);

   % The weights are calculated by averaging individual
   % weighting factors from the clean and processed frame.
   % These weights W_clean and W_processed should range
   % from 0 to 1 and place more emphasis on spectral
   % peaks and less emphasis on slope differences in spectral
   % valleys.  This procedure is described on page 1280 of
   % Klatt's 1982 ICASSP paper.

   Wmax_clean        = Kmax ./ (Kmax + dBMax_clean - ...
		 	    clean_energy(1:num_crit-1));
   Wlocmax_clean     = Klocmax ./ ( Klocmax + clean_loc_peak - ...
				clean_energy(1:num_crit-1));
   W_clean           = Wmax_clean .* Wlocmax_clean;

   Wmax_processed    = Kmax ./ (Kmax + dBMax_processed - ...
			        processed_energy(1:num_crit-1));
   Wlocmax_processed = Klocmax ./ ( Klocmax + processed_loc_peak - ...
			            processed_energy(1:num_crit-1));
   W_processed       = Wmax_processed .* Wlocmax_processed;

   W = (W_clean + W_processed)./2.0;

   distortion(frame_count) = sum(W.*(clean_slope(1:num_crit-1) - ...
		       processed_slope(1:num_crit-1)).^2);

   % this normalization is not part of Klatt's paper, but helps
   % to normalize the measure.  Here we scale the measure by the
   % sum of the weights.

   distortion(frame_count) = distortion(frame_count)/sum(W);

   start = start + skiprate;

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 1. use vectorization to generate face and face2elem data structure
%% this vectorization code does not work for uniform tets

% tic
%
% matlabTri = 1;
% N = size(mesh.p,1);
% node = [mesh.p;mesh.eIntP];
% allCutElem = mesh.t(mesh.tLoc<0,:);
% isCutElem = (mesh.tLoc<0);
% vSign = [mesh.pLoc;zeros(size(mesh.eIntP,1),1)];
%
% isInterfaceNode = false(size(node,1), 1);
% isInterfaceNode(allCutElem(:)) = true; % include the vertices of allCutElem
% isInterfaceNode(N+1:end) = true;    % and the cut points and the aux points
% interfaceNode = node(isInterfaceNode,:);
% matlabversion = version();
% if matlabTri == 1
%     DT = delaunayTriangulation(interfaceNode);
%     tetElem = DT.ConnectivityList;
% elseif matlabTri == 0
%     constrant1 = [mesh.e(mesh.eLoc<0,1),N-mesh.eLoc(mesh.eLoc<0)];
%     constrant2 = [N-mesh.eLoc(mesh.eLoc<0),mesh.e(mesh.eLoc<0,2)];
%     constrant = [constrant1;constrant2];
%     DT = constrainedDelaunayTetGen(interfaceNode,constrant);
%     tetElem = DT.ConnectivityList;
% end
% [tetElem, ortidx, volume] = fixorder3(interfaceNode, tetElem);
%
% localidx2globalidx = find(isInterfaceNode); % tetElem points to interfaceNode
% tetElem = localidx2globalidx(tetElem);  % map to the global index of node
% bc = (node(tetElem(:, 1), :) + node(tetElem(:, 2), :) ...
%     + node(tetElem(:, 3), :) + node(tetElem(:, 4), :))/4.0;
% idx2cube = FindElemId(bc, mesh); % Get the cube index of each tetElem
% idx2cubeBad = (idx2cube<=0);
% idx2cube = idx2cube(~idx2cubeBad);
% tetElem = tetElem(isCutElem(idx2cube), :); % Just keep the tets in cut elem
% volume = volume(isCutElem(idx2cube), :);
% idx2cube = idx2cube(isCutElem(idx2cube));  % keep cube idx in cut elements
% % volumetet0 =  accumarray(idx2cube,volume);
% % volumetet0 = volumetet0(volumetet0>0);
% %[tetElemOld, ortidxOld, volumeOld] = fixorder3(interfaceNode, mesh.t(mesh.tLoc<0,:));
%
% % There might exist some bad tet elements whose vertices are in
% % different cubes. We just need to keep the tet in every cut elem. So
% % here we get rid of them. (but do not handle different tets)
% NT = size(tetElem,1);
% X = reshape(node(tetElem,1), NT, 4);
% Y = reshape(node(tetElem,2), NT, 4);
% Z = reshape(node(tetElem,3), NT, 4);
% isBadTet =  (max(X,[],2) - min(X,[],2)) > 2*h - eps ... % d > 2h means
%           | (max(Y,[],2) - min(Y,[],2)) > 2*h - eps ... % in differnt cube
%           | (max(Z,[],2) - min(Z,[],2)) > 2*h - eps;
% tetElem = tetElem(~isBadTet,:);
%
% toc

% plot to check
%showmesh3(node,tetElem);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% this approach is based on forlopp which is slow
tic

Np = size(mesh.p,1);
node = [mesh.p;mesh.eIntP];
allCutElem = mesh.t(mesh.tLoc<0,:);
isCutElem = (mesh.tLoc<0);
vSign = [mesh.pLoc;zeros(size(mesh.eIntP,1),1)];
isInterfaceNode = false(size(node,1), 1);
isInterfaceNode(allCutElem(:)) = true; % include the vertices of allCutElem
numCutElemV1 = sum(isInterfaceNode);
isInterfaceNode(Np+1:end) = true;    % and the cut points and the aux points
allCutElemReoderTmp = zeros(size(node,1),1);
numCutElemV2 = sum(isInterfaceNode);
allCutElemReoderTmp(isInterfaceNode) = 1:numCutElemV2;
allCutElemReoder = allCutElemReoderTmp(allCutElem);
interfaceNode = node(isInterfaceNode,:);
ntI = -min(mesh.tLoc);
intID = find(mesh.tLoc<0);

tetElem = zeros(12*ntI,4);
tetElemLoc = zeros(12*ntI,1);
idx2cube = zeros(12*ntI,1);
tetcount = 0;

for i = 1:ntI
    tID = intID(i);
    t_e = mesh.t_e(tID,:);
    pLocK = mesh.pLoc(mesh.t(tID,:));
    vert0 = mesh.p(mesh.t(tID,:),:);
    nodeid = [allCutElemReoder(i,:),numCutElemV1-mesh.eLoc(t_e(mesh.eLoc(t_e)<0))'];
    vert2 = vert0(pLocK>0,:); % plus domain
    vert1 = vert0(pLocK<0,:); % minus domain
    intpt0 = mesh.eIntP(-mesh.eLoc(t_e(mesh.eLoc(t_e)<0)),:);
    id1 = [find(pLocK<0)', 5:4+size(intpt0,1)];
    id2 = [find(pLocK>0)', 5:4+size(intpt0,1)];
    p1 = [vert1;intpt0]; DT = delaunayTriangulation(p1); t1 = DT.ConnectivityList;
    p2 = [vert2;intpt0]; DT = delaunayTriangulation(p2); t2 = DT.ConnectivityList;
    tetElem(tetcount+1:tetcount+size(t1,1),:) = nodeid(id1(t1));
    idx2cube(tetcount+1:tetcount+size(t1,1)) = tID;
    tetElemLoc(tetcount+1:tetcount+size(t1,1)) = 1; % inside subdomain
    tetcount = tetcount+size(t1,1);
    tetElem(tetcount+1:tetcount+size(t2,1),:) = nodeid(id2(t2));
    idx2cube(tetcount+1:tetcount+size(t2,1)) = tID;
    tetElemLoc(tetcount+1:tetcount+size(t2,1)) = 2; % outside subdomain
    tetcount = tetcount+size(t2,1);
end
tetElem(tetcount+1:end,:) = [];
tetElemLoc(tetcount+1:end,:) = [];
idx2cube(tetcount+1:end,:) = [];
[tetElem, ortidx, volume] = fixorder3(interfaceNode, tetElem);
% there are some tets which are coplane, need to get rid of them
idPlane = (volume<=10^(-12));
tetElem(idPlane,:) = [];
volume(idPlane,:) = [];
tetElemLoc(idPlane,:) = [];
idx2cube(idPlane,:) = [];
localidx2globalidx = find(isInterfaceNode); % tetElem points to interfaceNode
tetElem = localidx2globalidx(tetElem);  % map to the global index of node
% there are some tets which are on the interface, but contained in the surrounding tets
% need to get rid of them (but the following algorithm may not be robust)
tetSign = vSign(tetElem);
idSliver = (sum(abs(tetSign),2)==0);
tetElem(idSliver,:) = [];
volume(idSliver,:) = [];
tetElemLoc(idSliver,:) = [];
idx2cube(idSliver,:) = [];
PolyVolume = accumarray([idx2cube,tetElemLoc],volume);
PolyVolume = PolyVolume(mesh.tLoc<0,:);
h = (PolyVolume(:,1) + PolyVolume(:,2)).^(1/3);
% the first component contains the volume of the polyhedron on the
% subdomain 1, the first component contains the volume of the polyhedron
% on the subdomain 2

toc

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% try another approach for vectorization

% tic
%
% Np = size(mesh.p,1);
% Ncube = size(mesh.t,1)/6;
% node = [mesh.p;mesh.eIntP];
% allCutElem = mesh.t(mesh.tLoc<0,:);
% isCutElem = (mesh.tLoc<0);
% vSign = [mesh.pLoc;zeros(size(mesh.eIntP,1),1)];
% intID = find(mesh.tLoc<0);
% isInterfaceNode = false(size(node,1), 1);
% isInterfaceNode(allCutElem(:)) = true; % include the vertices of allCutElem
% isInterfaceNode(Np+1:end) = true;    % and the cut points and the aux points
%
% for j = 1:6
%
%     Indj = (j-1)*Ncube + 1:Ncube;
%     Indj = intersect(intID,Indj);
%     allCutElemj = mesh.t(Indj,:);
%     isInterfaceNodej = false(size(node,1), 1);
%     isInterfaceNodej(allCutElemj(:)) = true; % include the vertices of allCutElem
%     tmp = mesh.eLoc(mesh.t_e(Indj,:));
%     tmp = tmp(tmp<0);
%     %isInterfaceNodej(Np-tmp) = true;    % and the cut points and the aux points
%     interfaceNodej = node(isInterfaceNodej,:);
%
%     DT = delaunayTriangulation(interfaceNodej);
%     tetElemj = DT.ConnectivityList;
%     [tetElemj, ortidx, volume] = fixorder3(interfaceNodej, tetElemj);
%
%     localidx2globalidxj = find(isInterfaceNodej); % tetElem points to interfaceNode
%     tetElemj = localidx2globalidxj(tetElemj);  % map to the global index of node
%     bcj = (node(tetElemj(:, 1), :) + node(tetElemj(:, 2), :) ...
%         + node(tetElemj(:, 3), :) + node(tetElemj(:, 4), :))/4.0;
%     idx2cubej = FindElemId(bcj, mesh); % Get the cube index of each tetElem
%     idx2cubeBadj = (idx2cubej<=0);
%     idx2cubej = idx2cubej(~idx2cubeBadj);
%     tetElemj = tetElemj(~idx2cubeBadj,:);
%     [godElemj,godElemjIDa,godElemjIDb] = intersect(idx2cubej,Indj);
%     idx2cubej = idx2cubej(godElemjIDa);
%     tetElemj = tetElemj(godElemjIDa,:);
%
% %     tetElem = tetElem(isCutElem(idx2cube), :); % Just keep the tets in cut elem
% %     volume = volume(isCutElem(idx2cube), :);
% %     idx2cube = idx2cube(isCutElem(idx2cube));  % keep cube idx in cut elements
%
% end
%
% toc

%% Get triangular faces for interrior elements and interface
tic
localFace = [2 3 4; 1 4 3; 1 2 4; 1 3 2];
NT = size(tetElem,1);
tface = zeros(4*NT, 3);
tface2elem = zeros(4*NT, 1);
iface = zeros(4*NT, 3);
iface2elem = zeros(4*NT, 1);
% find the interior tet elements
isIntTet = min(vSign(tetElem),[], 2) == -1; % can not be == -1 as there is sliver
intTet = tetElem(isIntTet,:);
% find the corresponding cube indices
intIdx2cube = idx2cube(isIntTet);
% find triangular interface
T = auxstructure3(intTet);
neighbor = T.neighbor; % if a face is on the boundary, then the neighbor element index is itself
clear T;
tmp = (1:size(intTet, 1))';
ct = 0;
ci = 0;
for i = 1:4
    face = intTet(:, localFace(i,:));
    % find the triangle faces of polyhedron
    % 1. face and its neighbor associated to different cubes, and
    % 2. face is not on the boundary of all cut elements (which are squares)
    isPolyTriFace = ((neighbor(:, i) == tmp) | (intIdx2cube ~= intIdx2cube(neighbor(:,i)))) &...
        (sum(abs(vSign(face)), 2) > 0);% & (sum(abs(vSign(face)), 2) > 0);
    c2 = ct + sum(isPolyTriFace);
    tface((ct+1):c2,:) = face(isPolyTriFace,:);
    tface2elem((ct+1):c2,:) = intIdx2cube(isPolyTriFace); % the indices of the polyhedron
    ct = c2;
    % note that only interior elements are treated

    % find the triangle faces on interface with normal points to exterior
    % 1. face is on the boundary and
    % 2. all vertices are on the interface
    isInterface = (sum(abs(vSign(face)), 2) == 0);

    % add to interface
    c4 = ci + sum(isInterface);
    iface((ci+1):c4,:) = face(isInterface,:); % the interface tri faces.
    iface2elem((ci+1):c4,:) = intIdx2cube(isInterface); % the indices of the polyhedron
    ci = c4;
end
iface((ci+1):end,:) = [];
iface2elem((ci+1):end,:) = [];
c2old = c2;
% plot to check
%trisurf(tface(1:c2,:),node(:,1),node(:,2),node(:,3))
%trisurf(iface(1:ci,:),node(:,1),node(:,2),node(:,3))

% Find the triangular faces for exterior elements
extTet = tetElem(~isIntTet, :);
extIdx2cube = idx2cube(~isIntTet);

T = auxstructure3(extTet);
neighbor = T.neighbor;
clear T;
tmp = (1:size(extTet,1))';
for i = 1:4
    face = extTet(:, localFace(i,:));
    % find the triangle faces of polyhedron
    % 1. face and its neighbor associated to different cubes, and
    % 2. face is not on the boundary of all cut elements (which are squares)
    isPolyTriFace = ((neighbor(:, i) == tmp) | (extIdx2cube ~= extIdx2cube(neighbor(:,i)))) &...
        (sum(abs(vSign(face)), 2) > 0);
    c2 = ct + sum(isPolyTriFace);
    tface((ct+1):c2,:) = face(isPolyTriFace,:);
    tface2elem((ct+1):c2,:) = extIdx2cube(isPolyTriFace);
    % index of exterior polyhedron is append to the end of elem
    ct = c2;
end
toc

% plot to check
%trisurf(tface(c2old+1:c2,:),node(:,1),node(:,2),node(:,3))
%trisurf(iface,node(:,1),node(:,2),node(:,3))

tface((ct+1):end,:) = [];    % remove empty meomory
tface2elem((ct+1):end) = [];

face2elem = tface2elem;
face2elemLoc = [ones(c2old,1);2*ones(c2-c2old,1)]; % face2elemLoc is the same as face location
face = tface;

interfaceData.tface = tface;
interfaceData.tface2elem = tface2elem;
interfaceData.iface = iface;
interfaceData.iface2elem = iface2elem;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 2.use face and face2elem data structure to compute ife functions

vSign = [mesh.pLoc;zeros(size(mesh.eIntP,1),1)];
node = [mesh.p;mesh.eIntP];
face = interfaceData.tface;
face2elem = interfaceData.tface2elem;
N = size(node, 1); Ndof = N;
NF = size(face,1); NC = size(mesh.t, 1);

NP = max(face2elem);
isCutPoly = false(NP, 1);
isCutPoly(face2elem) = true;
% this is the same as:
% isCutPoly = (mesh.tLoc<0);
% isCutPoly = isCutPoly(1:max(face2elem));
cutPolyIdx = zeros(NP, 1);
isExtPoly = false(NP,1);
isExtPoly(NC+1:NP) = true;

NP = sum(isCutPoly); % this new NP is max(-mesh.tLoc(mesh.tLoc<0));
cutPolyIdx(isCutPoly) = 1:NP;
% this is the same as:
% cutPolyIdx = zeros(size(mesh.tLoc,1), 1);
% cutPolyIdx(mesh.tLoc<0) = -mesh.tLoc(mesh.tLoc<0);
% cutPolyIdx = cutPolyIdxNew(1:max(face2elem));
face2elem = cutPolyIdx(face2elem);
% the old face2elem is the global index of interface elements
% the new one is only the index of interface elements
isExtPoly = isExtPoly(isCutPoly);
%%%clear isCutPoly cutPolyIdx

% prepare new data structure to compute projections
poly2node = sparse(face2elem(:)*ones(1,3), face(:), 1, NP,N);
poly2node = (poly2node > 0);
NV = poly2node*ones(N, 1);
%centroid = poly2node*node./[NV, NV, NV];
%KP = pde.A(centroid);

% generate local IFE basis functions which are not associated with any DoFs
% IFEbasis(:,i,:,:) for minus subdomain (i=1) or plus domain (i=2)
iface = interfaceData.iface;
tface = interfaceData.tface;
iface2elem = interfaceData.iface2elem;
[iface2elemReduce, uniID] = unique(iface2elem);
% length(uniID) should be NP
ifaceReduce = iface(uniID,:);
[IntFnormal,IntFarea,unitNormal,unittgt1,unittgt2] = facenormaltgt(node,ifaceReduce);
[Fnormal,Farea,FunitNormal,Funittgt1,Funittgt2] = facenormaltgt(node,tface);
% IFEbasis contains the associated IFE function on each face
IFEbasis = zeros(size(face2elem,1),3,3);
% for two pieces of an ife function, only the normal vector is different
% on the minus subdomain (1) ife = n; on the plus subdomain (2) ife = n*bm/bp
IFEbasis(:,1,:) = unitNormal(face2elem,:);
IFEbasis(:,2,:) = unittgt1(face2elem,:);
IFEbasis(:,3,:) = unittgt2(face2elem,:);
piece1tmp = (face2elemLoc==1);
IFEbasis(~piece1tmp,1,:) = IFEbasis(~piece1tmp,1,:)*bm/bp;
% generate boundary integral for computing projections
BIFE =  zeros(length(face2elem),3);
% BIFE contains  (beta*grad vi.n)*|f|/3 for i=1,2,3 (vi is the test function)
BIFE(:,1) = sum(squeeze(IFEbasis(:,1,:)).*Fnormal,2)/2;
BIFE(:,2) = sum(squeeze(IFEbasis(:,2,:)).*Fnormal,2)/2;
BIFE(:,3) = sum(squeeze(IFEbasis(:,3,:)).*Fnormal,2)/2;
% devided by 2 is because Fnormal is unitnormal*(2*area)
BIFE(piece1tmp,:) = bm*BIFE(piece1tmp,:)/3.;
BIFE(~piece1tmp,:) = bp*BIFE(~piece1tmp,:)/3;
% generate the matrix for solving IFE projections which is a diagonal matrix
VdiagTmp1 = (bm*PolyVolume(:,1) + bm^2/bp*PolyVolume(:,2)).^(-1);
VdiagTmp2 = (bm*PolyVolume(:,1) + bp*PolyVolume(:,2)).^(-1);
VdiagTmp3 = (bm*PolyVolume(:,1) + bp*PolyVolume(:,2)).^(-1);
Vdiag1 = VdiagTmp1(face2elem);
Vdiag2 = VdiagTmp2(face2elem);
Vdiag3 = VdiagTmp3(face2elem);
Vdiag = zeros(3*length(face2elem),1);
Vdiag(1:3:end-2) = Vdiag1;
Vdiag(2:3:end-1) = Vdiag2;
Vdiag(3:3:end) = Vdiag3;
Mdiag = spdiags(Vdiag,0,3*length(face2elem),3*length(face2elem));
BIFE = reshape(Mdiag*reshape(BIFE',[],1),3,[])';
% this updated new BIFE(i,:) contains (old)BIFE(i,:)*M^(-1) where M is the
% coefficient matrix associated with the element for this face.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compute projections to IFE spaces and form the matrices
nnz = sum(NV.^2);
iiP = zeros(nnz, 1);
jjP = zeros(nnz, 1);
ssP = zeros(nnz, 1);
indexP = 0;
iiF = zeros(nnz, 1);
jjF = zeros(nnz, 1);
ssF = zeros(nnz, 1);
indexF = 0;
unv = unique(NV);
hF = h(face2elem);
b = zeros(Ndof, 1);

for kk = 1:length(unv) % group polys according to their # of nodes

    tic
    %% generate IFE functions including the IFE basis functions on each interface
    %% elements which are not associated with any DoFs, and projections of gradients
    %% and the IFE functions with matching face average
    nv = unv(kk);
    isCurrentFace = (NV(face2elem) == nv);
    CurrentFace = tface(isCurrentFace,:);
    CNF = size(CurrentFace,1);
    Currentfaceloc = face2elemLoc(isCurrentFace);
    Currentface2elem = face2elem(isCurrentFace);
    CurrentFnormal = FunitNormal(isCurrentFace,:);
    isCurrentElem = false(NP, 1);
    isCurrentElem(face2elem(isCurrentFace)) = true;
    CurrentPolyVolume = PolyVolume(isCurrentElem,:);
    % Current elem to node matrix
    currentPoly2node = poly2node(isCurrentElem, :);
    CNP = size(currentPoly2node, 1);
    currentPolyLocalIdx = zeros(NP, 1);
    currentPolyLocalIdx(isCurrentElem) = 1:CNP;

    % currentElem(i,:) contains the node index of the (current) i-th element
    [I, ~] = find(currentPoly2node');
    currentElem = reshape(I, nv, [])';
    % localIdx(i,j) contains the (current) i-th element having the
    % node(dof=1,2,3,4) on the j-th location (j is the global node index)
    localIdx = sparse(repmat((1:CNP)', 1, nv), currentElem, ones(CNP, 1)*(1:nv), CNP, N);
    clear currentPoly2node

    % Deal with current triangle face cases
    %isCTFace = isCurrentFace;
    tFace = face(isCurrentFace, :);
    NN = sum(isCurrentFace);
    subs1 = currentPolyLocalIdx(face2elem(isCurrentFace));
    subs2 = [ones(NN, 1); 2*ones(NN, 1); 3*ones(NN, 1)];
    val = BIFE(isCurrentFace,:);

    % compute the projection of gradients
    GP1 = zeros(CNP,3,nv);
    % coefficient of n t1 and t2 for the subelement on the subdomain 1
    for m = 1:3
        subs3 = full(localIdx((CurrentFace(:, m) - 1)*CNP + subs1));
        % subs1: new order (range in 1:CNP) of the element index
        % subs2: three components in the vector
        % subs3: the local index (DoF) (w.r.t. element) of the m-th node of this face
        GP1 = GP1 + accumarray([repmat(subs1,3,1), subs2, repmat(subs3, 3, 1)], val(:), [CNP, 3, nv]);
    end
    GP2 = GP1; GP2(:,1,:) = bm/bp*GP2(:,1,:);
    % generate vector basis associated with interface plane
    unitNormalCurrent = unitNormal(isCurrentElem,:);
    unittgt1Current = unittgt1(isCurrentElem,:);
    unittgt2Current = unittgt2(isCurrentElem,:);
    NTCurrent = zeros(CNP,3,3);
    NTCurrent(:,:,1) = unitNormalCurrent;
    NTCurrent(:,:,2) = unittgt1Current;
    NTCurrent(:,:,3) = unittgt2Current;
    GP1 = mytimes(NTCurrent,GP1);
    GP2 = mytimes(NTCurrent,GP2);
    % the updated GP contains three component values of the projection vector
    % generate IFE vectors on each face

    % generate some data structure on faces to compute IFE functions (not gradients)
    GP1tmp = zeros(NP,3,nv); GP2tmp = zeros(NP,3,nv);
    GP1tmp(isCurrentElem,:,:) = GP1; GP2tmp(isCurrentElem,:,:) = GP2;
    Gface = zeros(CNF,3,nv);
    Gface(Currentfaceloc==1,:,:) = GP1tmp(Currentface2elem(Currentfaceloc==1),:,:);
    Gface(Currentfaceloc==2,:,:) = GP2tmp(Currentface2elem(Currentfaceloc==2),:,:);
    clear GP1tmp GP2tmp
    % compute the IFE functions with the constant such that integral on
    % faces equal the virtual element function
    VPtmp1 = zeros(CNP,nv); % sum_i DoFi(u)|f|/3 on each face f
    VPtmp2 = zeros(CNP,nv); % grad*(xf-xm)|f|
    VPtmp3 = zeros(CNP,nv); % the surface area of each element repeated nv times
    FareaCurrent = Farea(isCurrentFace); %|f|/3
    for m = 1:3
        subs3 = full(localIdx((CurrentFace(:, m) - 1)*CNP + subs1));
        VPtmp1 = VPtmp1 + accumarray([subs1, subs3], FareaCurrent(:)/3, [CNP, nv]);
    end
    Xmf = (node(ifaceReduce(Currentface2elem,1),:) + node(ifaceReduce(Currentface2elem,2),:) +...
        node(ifaceReduce(Currentface2elem,3),:))/3; % the Xm point on the approximate interface plane
    Xf = (node(CurrentFace(:,1),:) + node(CurrentFace(:,2),:) +...
        node(CurrentFace(:,3),:))/3; % the centroid on each triangular face
    VPtmp2tmp = ((Xf - Xmf)).*FareaCurrent;
    for i = 1:nv
        tmp = sum(Gface(:,:,i).*VPtmp2tmp,2);
        VPtmp2(:,i) = accumarray(subs1, tmp,[CNP, 1]);
    end
    VPtmp3 = repmat(accumarray(subs1, FareaCurrent(:)),[1, nv]);
    IFEConst = (VPtmp1 - VPtmp2)./VPtmp3;
    clear currentPolyLocalIdx localIdx
    clear subs1 subs2 subs3
    toc

    %% form the stiffness matrices and stabilization matrices
    CurrentFace2DoF = zeros(NP,nv);
    CurrentFace2DoF(isCurrentElem,:) = currentElem;
    CurrentFace2DoF = CurrentFace2DoF(Currentface2elem,:);
    FunitNormalCurrent = FunitNormal(isCurrentFace,:);
    Funittgt1Current = Funittgt1(isCurrentFace,:);
    Funittgt2Current = Funittgt2(isCurrentFace,:);
    FNTCurrent = zeros(CNF,3,3);
    FNTCurrent(:,1,:) = Funittgt1Current;
    FNTCurrent(:,2,:) = Funittgt2Current;
    FNTCurrent(:,3,:) = FunitNormalCurrent;
    GfaceP = zeros(size(Gface));
    for n = 1:nv
        GfaceP(:,:,n) =  Gface(:,:,n) - sum(Gface(:,:,n).*CurrentFnormal,2).*CurrentFnormal;
    end
    GfaceP = mytimes(FNTCurrent,GfaceP);
    GfaceP = GfaceP(:,1:2,:);
    % compute the face gradients based on DoFs
    % generate vector basis associated with each face
    Fnode  = zeros(CNF,3,3);
    for l = 1:3
        Fnode(:,:,l) = node(CurrentFace(:,l),:);
    end
    FacePCoord = mytimes(FNTCurrent,Fnode);
    gradLambdatmp = zeros(CNF,2,3);  % barycentric coordinates
    gradLambda = zeros(CNF,2,nv);
    signtmp = ones(CNF,2); signtmp(:,1) = -1;
    gradLambdatmp(:,:,1) = (FacePCoord(:,[2,1],3) - FacePCoord(:,[2,1],2)).*signtmp./(2*Farea(isCurrentFace));
    gradLambdatmp(:,:,2) = (FacePCoord(:,[2,1],1) - FacePCoord(:,[2,1],3)).*signtmp./(2*Farea(isCurrentFace));
    gradLambdatmp(:,:,3) = -gradLambdatmp(:,:,1) - gradLambdatmp(:,:,2);
    for i = 1:nv
        for j = 1:3
            IDij = (CurrentFace(:,j) == CurrentFace2DoF(:,i));
            gradLambda(IDij,:,i) = gradLambdatmp(IDij,:,j);
        end
    end

%     for n = 1:nv
%         for m = 1:nv
%             Xn = node(currentElem(:, n), :) - centroid(isCurrentElem,:);
%             IminusP(:, n, m) = - 1/nv - dot(Xn, BP(:, :, m), 2)./volume(isCurrentElem); % stabilization
%             if(n == m)
%                 IminusP(:, n, m) = 1 + IminusP(:, n, m);
%             end
%         end
%     end

    for n = 1:nv
        for m = 1:nv
            iiP(indexP+1:indexP + CNP) = currentElem(:, n);
            jjP(indexP+1:indexP + CNP) = currentElem(:, m);
            ssP(indexP+1:indexP + CNP) = bm*dot(GP1(:,:,n), GP1(:,:,m),2).*CurrentPolyVolume(:,1)+...
                bp*dot(GP2(:,:,n), GP2(:,:,m),2).*CurrentPolyVolume(:,2);
            indexP = indexP + CNP;

            iiF(indexF+1:indexF + CNF) = CurrentFace2DoF(:, n);
            jjF(indexF+1:indexF + CNF) = CurrentFace2DoF(:, m);
            ssF(indexF+1:indexF + CNF) = hF(isCurrentFace).*...
                dot((gradLambda(:,:,m) - GfaceP(:,:,m)),(gradLambda(:,:,n) - GfaceP(:,:,n)),2).*...
                Farea(isCurrentFace);
            indexF = indexF + CNF;
        end
    end

    %% compute the RHS with only evaluating the integral at the centroid
    %% approach 1 (will give the optimal order but slightly increase the error)
%     currentElemSign = vSign(currentElem);
%     currentElemSign1 = currentElemSign<=0; nv1 = sum(currentElemSign1,2);
%     currentElemSign2 = currentElemSign>=0; nv2 = sum(currentElemSign2,2);
% %     nodeX = node(:,1); currentElemNodeX = nodeX(currentElem); clear nodeX
% %     nodeY = node(:,2); currentElemNodeY = nodeY(currentElem); clear nodeY
% %     nodeZ = node(:,1); currentElemNodeZ = nodeZ(currentElem); clear nodeZ
%     currentElemNodeX = reshape(node(currentElem',1),size(currentElem,2),[])';
%     currentElemNodeY = reshape(node(currentElem',2),size(currentElem,2),[])';
%     currentElemNodeZ = reshape(node(currentElem',3),size(currentElem,2),[])';
%     centroid1 = [sum(currentElemNodeX.*currentElemSign1,2)./nv1,...
%         sum(currentElemNodeY.*currentElemSign1,2)./nv1, ...
%         sum(currentElemNodeZ.*currentElemSign1,2)./nv1];
%     centroid2 = [sum(currentElemNodeX.*currentElemSign2,2)./nv2,...
%         sum(currentElemNodeY.*currentElemSign2,2)./nv2, ...
%         sum(currentElemNodeZ.*currentElemSign2,2)./nv2];
%     ft1 = CurrentPolyVolume(:,1).*pde.f(centroid1(:,1),centroid1(:,2),centroid1(:,3));
%     ft2 = CurrentPolyVolume(:,2).*pde.f(centroid2(:,1),centroid2(:,2),centroid2(:,3));
%
%     Xm = (node(ifaceReduce(isCurrentElem,1),:) + node(ifaceReduce(isCurrentElem,2),:) +...
%         node(ifaceReduce(isCurrentElem,3),:))/3;
%     ifeEvalcen1 = zeros(CNP,nv); ifeEvalcen2 = zeros(CNP,nv);
%     for n = 1:nv
%         ifeEvalcen1(:,n) = sum(GP1(:,:,n).*(centroid1-Xm),2) + IFEConst(:,n);
%         ifeEvalcen2(:,n) = sum(GP2(:,:,n).*(centroid2-Xm),2) + IFEConst(:,n);
%     end
%
%     Currentb = ft1.*ifeEvalcen1 + ft2.*ifeEvalcen2;
%     b = b + accumarray(currentElem(:), Currentb(:), [Ndof, 1]);

    %% approach 2
    idx2cubeNew = -mesh.tLoc(idx2cube);
    TetID = isCurrentElem(idx2cubeNew);
    currentvolume = volume(TetID);
    ElemDoF = zeros(sum(TetID),nv);
    ElemDoF(isCurrentElem,:) = currentElem;
    ElemDoF = ElemDoF(idx2cubeNew(TetID),:);
    X1 = node(tetElem(TetID,1),:);
    X2 = node(tetElem(TetID,2),:);
    X3 = node(tetElem(TetID,3),:);
    X4 = node(tetElem(TetID,4),:);
    ng = 1;
    [gx, gy, gz] = gaussPtetra(X1, X2, X3, X4, ng);
    gw = gaussWtetra(ng);
    Xm = (node(ifaceReduce(isCurrentElem,1),:) + node(ifaceReduce(isCurrentElem,2),:) +...
        node(ifaceReduce(isCurrentElem,3),:))/3;

    Bas1 = zeros(size(isCurrentElem,1),3,nv); Bas2 = Bas1;
    Bas1(isCurrentElem,:,:) = GP1;
    Bas2(isCurrentElem,:,:) = GP2;
    Bas1 = Bas1(idx2cubeNew(TetID),:,:); Bas2 = Bas2(idx2cubeNew(TetID),:,:);
    Bas = zeros(sum(TetID),3,nv);
    %%%
    IFEc0 = zeros(size(isCurrentElem,1),nv);
    IFEc0(isCurrentElem,:) = IFEConst;
    IFEc0 = IFEc0(idx2cubeNew(TetID),:);
    %%%
    Xmtet = zeros(size(isCurrentElem,1),3);
    Xmtet(isCurrentElem,:) = Xm;
    Xmtet = Xmtet(idx2cubeNew(TetID),:);
    %%%
    currentTetDoF = zeros(size(isCurrentElem,1),nv);
    currentTetDoF(isCurrentElem,:) = currentElem;
    currentTetDoF = currentTetDoF(idx2cubeNew(TetID),:);
    %%%
    piecetmp = tetElemLoc(TetID);
    Bas(piecetmp==1,:,:) = Bas1(piecetmp==1,:,:);
    Bas(piecetmp==2,:,:) = Bas2(piecetmp==2,:,:);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%
    % mass matrix

%     for i = 1:nv
%         for j = 1:nv
%             Basi = squeeze(Bas(:,:,i)); IFEc0i = IFEc0(:,i);
%             Basj = squeeze(Bas(:,:,j)); IFEc0j = IFEc0(:,j);
%             uhi = (gx-Xmtet(:,1)).*Basi(:,1) + (gy-Xmtet(:,2)).*Basi(:,2) +...
%                 (gz-Xmtet(:,3)).*Basi(:,3) + IFEc0i;
%             uhj = (gx-Xmtet(:,1)).*Basj(:,1) + (gy-Xmtet(:,2)).*Basj(:,2) +...
%                 (gz-Xmtet(:,3)).*Basj(:,3) + IFEc0j;
%             Currentb(:,i) = sum(gw*ft.*uhi.*uhj,2).*currentvolume;
%             bii(count+1:count+cnt) = currentTetDoF(:,i);
%             count = count+cnt;
%         end
%     end

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

    ft = pde.f(gx,gy,gz);
    Currentb = zeros(size(ft));
    cnt = size(ft,1);
    bii = nv*cnt;
    count = 0 ;
    for i = 1:nv
        Basi = squeeze(Bas(:,:,i)); IFEc0i = IFEc0(:,i);
        uhi = (gx-Xmtet(:,1)).*Basi(:,1) + (gy-Xmtet(:,2)).*Basi(:,2) +...
            (gz-Xmtet(:,3)).*Basi(:,3) + IFEc0i;
        Currentb(:,i) = sum(gw*ft.*uhi,2).*currentvolume;
        bii(count+1:count+cnt) = currentTetDoF(:,i);
        count = count+cnt;
    end
    b = b + sparse(bii,1,reshape(Currentb,[],1),Ndof,1);
    %b = b + accumarray([idx2cubeNew(TetID),currentTetDoF(:)], Currentb(:), [Ndof, 1]);

end
K = sparse(iiP, jjP, ssP, Ndof, Ndof);
S = sparse(iiF, jjF, ssF, Ndof, Ndof);


function solution = solveCobraMILP(MILPproblem, varargin)
% Solves constraint-based MILP problems
% The solver is defined in the CBT_MILP_SOLVER global variable
% (set using `changeCobraSolver`). Solvers currently available are
% 'tomlab_cplex' and 'glpk'
%
% USAGE:
%
%    solution = solveCobraMILP(MILPproblem, parameters)
%
% INPUT:
%    MILPproblem:     Structure containing the following fields describing the LP problem to be solved
%
%                       * .A - LHS matrix
%                       * .b - RHS vector
%                       * .c - Objective coeff vector
%                       * .lb - Lower bound vector
%                       * .ub - Upper bound vector
%                       * .osense - Objective sense (-1 max, +1 min)
%                       * .csense - Constraint senses, a string containting the constraint sense for
%                         each row in A ('E', equality, 'G' greater than, 'L' less than).
%                       * .vartype - Variable types ('C' continuous, 'I' integer, 'B' binary)
%                       * .x0 - Initial solution
%
% Optional parameters can be entered using parameters structure or as
% parameter followed by parameter value: i.e. ,'printLevel', 3)
% Setting `parameters` = 'default' uses default setting set in
% `getCobraSolverParameters`.
%
% OPTIONAL INPUTS:
%    varargin:        Additional parameters either as parameter struct, or as
%                     parameter/value pairs. A combination is possible, if
%                     the parameter struct is either at the beginning or the
%                     end of the optional input.
%                     All fields of the struct which are not COBRA parameters
%                     (see `getCobraSolverParamsOptionsForType`) for this
%                     problem type will be passed on to the solver in a
%                     solver specific manner. Some optional parameters which
%                     can be passed to the function as parameter value pairs,
%                     or as part of the options struct are listed below:
%
%    timeLimit:       Global solver time limit
%    intTol:          Integrality tolerance
%    relMipGapTol:    Relative MIP gap tolerance
%    logFile:         Log file (for CPLEX)
%    printLevel:      Printing level
%
%                       * 0 - Silent (Default)
%                       * 1 - Warnings and Errors
%                       * 2 - Summary information
%                       * 3 - More detailed information
%                       * > 10 - Pause statements, and maximal printing (debug mode)
%    saveInput:       Saves LPproblem to filename specified in field.
%                     i.e. parameters.saveInput = 'LPproblem.mat';
%
%
% OUTPUT:
%    solution:        Structure containing the following fields describing a MILP solution
%
%                       * .cont:        Continuous solution
%                       * .int:         Integer solution
%                       * .full:        Full MILP solution vector
%                       * .obj:         Objective value
%                       * .solver:      Solver used to solve MILP problem
%                       * .stat:        Solver status in standardized form (see below)
%
%                         * 1 - Optimal solution found
%                         * 2 - Unbounded solution
%                         * 0 - Infeasible MILP
%                         * -1 - No integer solution exists
%                         * 3 - Other problem (time limit etc, but integer solution exists)
%                       * .origStat:    Original status returned by the specific solver
%                       * .time:        Solve time in seconds
%
% .. Authors:
%       - Markus Herrgard 1/23/07
%       - Tim Harrington  05/18/12 Added support for the Gurobi 5.0 solver
%       - Ronan (16/07/2013) default MPS parameters are no longer global variables
%       - Meiyappan Lakshmanan  11/14/14 Added support for the cplex_direct solver
%       - cplex_direct solver accesible through CPLEX m-file and CPLEX C-interface
%       - Thomas Pfau (12/11/2015) Added support for ibm_cplex (the IBM Matlab
%       interface) to the solvers.

[cobraParams,solverParams] = parseSolverParameters('MILP',varargin{:}); % get the solver parameters

solver = cobraParams.solver;

% Save Input if selected
if ~isempty(cobraParams.saveInput)
    fileName = cobraParams.saveInput;
    if ~find(regexp(fileName, '.mat'))
        fileName = [fileName '.mat'];
    end
    display(['Saving MILPproblem in ' fileName]);
    save(fileName, 'MILPproblem')
end

% Defaults in case the solver does not return anything
x = [];
xInt = [];
xCont = [];
f = [];

if ~isfield(MILPproblem, 'x0')
    MILPproblem.x0 = [];
end

[A, b, c, lb, ub, csense, osense, vartype, x0] = ...
    deal(MILPproblem.A, MILPproblem.b, MILPproblem.c, MILPproblem.lb, MILPproblem.ub, ...
    MILPproblem.csense, MILPproblem.osense, MILPproblem.vartype, MILPproblem.x0);

if any(~(vartype == 'C' | vartype == 'B' | vartype == 'I'))
    display('vartype not C or B or I:  Assuming C');
    vartype(vartype ~= 'C' & vartype ~= 'I'& vartype ~= 'B') = 'C';
end

t_start = clock;
switch solver

    case 'glpk'
        %% glpk

        % Set up problem
        if (isempty(csense))
            clear csense
            csense(1:length(b), 1) = 'S';
        else
            csense(csense == 'L') = 'U';
            csense(csense == 'G') = 'L';
            csense(csense == 'E') = 'S';
            csense = columnVector(csense);
        end

        if ~isfield(solverParams,'msglev')
            solverParams.msglev = cobraParams.printLevel;
        end
        if ~isfield(solverParams,'tmlim')
            solverParams.tmlim = cobraParams.timeLimit;
        end

        % whos csense vartype
        csense = char(csense);
        vartype = char(vartype);
        % whos csense vartype

        % Solve problem
        [x, f, stat, extra] = glpk(c, A, b, lb, ub, csense, vartype, osense, solverParams);
        % Handle solution status reports
        if (stat == 5)
            solStat = 1;  % optimal
        elseif(stat == 6)
            solStat = 2;  % unbounded
        elseif(stat == 4)
            solStat = 0;  % infeasible

        elseif(stat == 171)
            solStat = 1;  % Opt integer within tolerance
        elseif(stat == 173)
            solStat = 0;  % Integer infeas
        elseif(stat == 184)
            solStat = 2;  % Unbounded
        elseif(stat == 172)
            solStat = 3;  % Other problem, but integer solution exists
        else
            solStat = -1;  % No integer solution exists
        end

    case 'cplex_direct'
        %% cplex_direct

        % Set up problem
        b = full(b);
        [m_lin, n] = size(MILPproblem.A);
        if ~isempty(csense)
            Aineq = [MILPproblem.A(csense == 'L', :); - MILPproblem.A(csense == 'G', :)];
            bineq = [b(csense == 'L', :); - b(csense == 'G', :)];
            %        min      c*x
            %        st.      Aineq*x <= bineq
            %                 Aeq*x    = beq
            %                 lb <= x <= ub
            A = MILPproblem.A(csense == 'E', :);
            b = b(csense == 'E', 1);
            [x, f, exitflag, output] = cplexmilp(c, Aineq, bineq, A, b, [], [], [], lb, ub, vartype');

            % primal
            solution.obj = osense * f;
            solution.full = x;
            % this is the dual to the equality constraints but it's not the chemical potential
            %             solution.dual=lambda.eqlin;
        else
            Aineq = [];
            bineq = [];
            [x, f, exitflag, output] = cplexmilp(c, Aineq, bineq, MILPproblem.A, b, lb, ub, vartype);
            solution.obj = osense * f;
            solution.full = x;
            % this is the dual to the equality constraints but it's not the chemical potential
            solution.dual = sparse(size(MILPproblem.A, 1), 1);
            %             solution.dual(csense == 'E')=lambda.eqlin;
            % this is the dual to the inequality constraints but it's not the chemical potential
            %             solution.dual(csense == 'L')=lambda.ineqlin(1:nnz(csense == 'L'),1);
            %             solution.dual(csense == 'G')=lambda.ineqlin(nnz(csense == 'L')+1:end,1);
        end
        solution.nInfeas = [];
        solution.sumInfeas = [];
        solution.origStat = output.cplexstatus;

        Inform = solution.origStat;
        stat = Inform;
        if (stat == 101 || stat == 102)
            solStat = 1;  % Opt integer within tolerance
        elseif(stat == 103)
            solStat = 0;  % Integer infeas
        elseif(stat == 118 || stat == 119)
            solStat = 2;  % Unbounded
        elseif(stat == 106 || stat == 106 || stat == 108 || stat == 110 || stat == 112 || stat == 114 || stat == 117)
            solStat = -1;  % No integer solution exists
        else
            solStat = 3;  % Other problem, but integer solution exists
        end

    case 'gurobi_mex'
        % Free academic licenses for the Gurobi solver can be obtained from
        % http://www.gurobi.com/html/academic.html
        %
        % The code below uses Gurobi Mex to interface with Gurobi. It can be downloaded from
        % http://www.convexoptimization.com/wikimization/index.php/Gurobi_Mex:_A_MATLAB_interface_for_Gurobi

        opts = solverParams;
        if cobraParams.printLevel == 0
            % Version v1.10 of Gurobi Mex has a minor bug. For complete silence
            % Remove Line 736 of gurobi_mex.c: mexPrintf("\n");
            if ~isfield(opts,'Display')
                opts.Display = 0;
            end
            if ~isfield(opts,'DisplayInterval')
                opts.DisplayInterval = 0;
            end
        else
            if ~isfield(opts,'Display')
                opts.Display = 1;
            end
        end

        if ~isfield(opts,'TimeLimit')
            opts.TimeLimit = solverParams.timeLimit;
        end
        if ~isfield(opts,'MIPGap')
            opts.MIPGap = solverParams.relMipGapTol;
        end
        if ~isfield(opts,'IntFeasTol')
            opts.IntFeasTol = solverParams.intTol;
        end
        if ~isfield(opts,'FeasibilityTol')
            % minimum intTol for gurobi = 1e-9
            opts.FeasibilityTol = max(solverParams.feasTol,1e-9);
        end
        if ~isfield(opts,'OptimalityTol')
            opts.OptimalityTol = solverParams.optTol;
        end

        if (isempty(csense))
            clear csense
            csense(1:length(b),1) = '=';
        else
            csense(csense == 'L') = '<';
            csense(csense == 'G') = '>';
            csense(csense == 'E') = '=';
            csense = csense(:);
        end
        % gurobi_mex doesn't automatically cast logicals to doubles
        c = double(c);
        [x,f,stat,output] = gurobi_mex(c,osense,sparse(A),b, ...
            csense,lb,ub,vartype,opts);
        if stat == 2
            solStat = 1; % Optimal solutuion found
        elseif stat == 3
            solStat = 0; % Infeasible
        elseif stat == 5
            solStat = 2; % Unbounded
        elseif stat == 4
            solStat = 0; % Gurobi reports infeasible *or* unbounded
        else
            solStat = -1; % Solution not optimal or solver problem
        end

    case 'ibm_cplex'
        % Free academic licenses for the IBM CPLEX solver can be obtained from
        % https://www.ibm.com/developerworks/community/blogs/jfp/entry/CPLEX_Is_Free_For_Students?lang=en
        cplexlp = buildCplexProblemFromCOBRAStruct(MILPproblem);
        [cplexlp, logFile, logToFile] = setCplexParametersForProblem(cplexlp,cobraParams,solverParams,'MILP');

        % Solve problem
        Result = cplexlp.solve();

        if logToFile
            % Close the output file
            fclose(logFile);
        end

        % Get results
        stat = Result.status;
        if (stat == 101 || stat == 102 || stat == 1)
            solStat = 1; % Opt integer within tolerance
            % Return solution if problem is feasible, bounded and optimal
            x = Result.x;
            f = Result.objval;
        elseif (stat == 103 || stat == 3)
            solStat = 0; % Integer infeas
        elseif (stat == 118 || stat == 119 || stat == 2)
            solStat = 2; % Unbounded
        elseif (stat == 106 || stat == 106 || stat == 108 || stat == 110 || stat == 112 || stat == 114 || stat == 117)
            solStat = -1; % No integer solution exists
        else
            solStat = 3; % Other problem, but integer solution exists
        end
        if exist([pwd filesep 'clone1.log'],'file')
            delete('clone1.log')
        end
    case 'gurobi'
        %% gurobi 5
        % Free academic licenses for the Gurobi solver can be obtained from
        % http://www.gurobi.com/html/academic.html
        MILPproblem.A = deal(sparse(MILPproblem.A));

        if cobraParams.printLevel == 0
            params.OutputFlag = 0;
            params.DisplayInterval = 1;
        else
            params.OutputFlag = 1;
            params.DisplayInterval = 5;
        end

        %return solution when time limit is reached and save the log file
        if ~isempty(cobraParams.logFile)
            params.LogFile = cobraParams.logFile;
        end
        params.TimeLimit = cobraParams.timeLimit;

        % set tolerances
        params.MIPGap = cobraParams.relMipGapTol;
        if cobraParams.intTol <= 1e-09
            params.IntFeasTol = 1e-09;
        else
            params.IntFeasTol = cobraParams.intTol;
        end
        params.FeasibilityTol = cobraParams.feasTol;
        params.OptimalityTol = cobraParams.optTol;

        if (isempty(csense))
            clear csense
            csense(1:length(b),1) = '=';
        else
            csense(csense == 'L') = '<';
            csense(csense == 'G') = '>';
            csense(csense == 'E') = '=';
            MILPproblem.csense = csense(:);
        end

        if osense == -1
            MILPproblem.osense = 'max';
        else
            MILPproblem.osense = 'min';
        end

        % overwrite default params with directParams
        fieldNames = fieldnames(solverParams);
        for i = 1:size(fieldNames,1)
            params.(fieldNames{i}) = solverParams.(fieldNames{i});
        end

        MILPproblem.vtype = vartype;
        MILPproblem.modelsense = MILPproblem.osense;
        [MILPproblem.A,MILPproblem.rhs,MILPproblem.obj,MILPproblem.sense] = deal(sparse(MILPproblem.A),MILPproblem.b,double(MILPproblem.c),MILPproblem.csense);
        if ~isempty(x0)
            MILPproblem.start = x0;
        end
        resultgurobi = gurobi(MILPproblem,params);

        stat = resultgurobi.status;
        if strcmp(resultgurobi.status,'OPTIMAL')
            solStat = 1; % Optimal solution found
            [x,f] = deal(resultgurobi.x,resultgurobi.objval);
        elseif strcmp(resultgurobi.status,'INFEASIBLE')
            solStat = 0; % Infeasible
        elseif strcmp(resultgurobi.status,'UNBOUNDED')
            solStat = 2; % Unbounded
        elseif strcmp(resultgurobi.status,'INF_OR_UNBD')
            solStat = 0; % Gurobi reports infeasible *or* unbounded
        elseif strcmp(resultgurobi.status,'TIME_LIMIT')
            solStat = 3; % Time limit reached
            warning('Time limit reached, solution might not be optimal (gurobi)')
            try
                [x,f] = deal(resultgurobi.x,resultgurobi.objval);
            catch
                %x and f could not be assigned, as there is no solution
                %yet
            end
        else
            solStat = -1; % Solution not optimal or solver problem
        end

    case 'tomlab_cplex'
        %% CPLEX through tomlab
        if (~isempty(csense))
            b_L(csense == 'E') = b(csense == 'E');
            b_U(csense == 'E') = b(csense == 'E');
            b_L(csense == 'G') = b(csense == 'G');
            b_U(csense == 'G') = inf;
            b_L(csense == 'L') = -inf;
            b_U(csense == 'L') = b(csense == 'L');
        elseif isfield(MILPproblem, 'b_L') && isfield(MILPproblem, 'b_U')
            b_L = MILPproblem.b_L;
            b_U = MILPproblem.b_U;
        else
            b_L = b;
            b_U = b;
        end
        intVars = (vartype == 'B') | (vartype == 'I');
        % intVars
        % pause;
        tomlabProblem = mipAssign(osense*c,A,b_L,b_U,lb,ub,x0,'CobraMILP',[],[],intVars);

        % Set parameters for CPLEX
        tomlabProblem.MIP.cpxControl.EPINT = cobraParams.intTol;
        tomlabProblem.MIP.cpxControl.EPGAP = cobraParams.relMipGapTol;
        tomlabProblem.MIP.cpxControl.TILIM = cobraParams.timeLimit;
        tomlabProblem.CPLEX.LogFile = cobraParams.logFile;
        tomlabProblem.PriLev = cobraParams.printLevel;
        tomlabProblem.MIP.cpxControl.THREADS = 1; % by default use only one thread

        % Strict numerical tolerances
        tomlabProblem.MIP.cpxControl.EPRHS = cobraParams.feasTol;
        tomlabProblem.MIP.cpxControl.EPOPT = cobraParams.optTol;
        tomlabProblem.MIP.cpxControl.EPAGAP = cobraParams.absMipGapTol;

        %Now, replace anything that is in the solver Specific field.
        tomlabProblem.cpxControl = updateStructData(tomlabProblem.MIP.cpxControl,solverParams);

        % Set initial solution
        tomlabProblem.MIP.xIP = x0;

        % Set up callback to print out intermediate solutions
        % only set this up if you know that you actually need these
        % results.  Otherwise do not specify intSolInd and contSolInd
        global cobraIntSolInd;
        global cobraContSolInd;
        if(~isfield(MILPproblem, 'intSolInd'))
            MILPproblem.intSolInd = [];
        else
            tomlabProblem.MIP.callback(14) = 1;
        end
        cobraIntSolInd = MILPproblem.intSolInd;
        if(~isfield(MILPproblem, 'contSolInd'))
            MILPproblem.contSolInd = [];
        end
        cobraContSolInd = MILPproblem.contSolInd;
        tomlabProblem.MIP.callbacks = [];
        tomlabProblem.PriLevOpt = 0;

        % Solve problem
        Result = tomRun('cplex', tomlabProblem);

        % Get results
        x = Result.x_k;
        f = osense*Result.f_k;
        stat = Result.Inform;
        if (stat == 101 || stat == 102)
            solStat = 1; % Opt integer within tolerance
        elseif (stat == 103)
            solStat = 0; % Integer infeas
        elseif (stat == 118 || stat == 119)
            solStat = 2; % Unbounded
        elseif (stat == 106 || stat == 106 || stat == 108 || stat == 110 || stat == 112 || stat == 114 || stat == 117)
            solStat = -1; % No integer solution exists
        else
            solStat = 3; % Other problem, but integer solution exists
        end
    case 'mps'
        fprintf(' > The interface to ''mps'' from solveCobraMILP will not be supported anymore.\n -> Use >> writeCbModel(model, ''mps'');\n');
        % temporary legacy support
        solverParams = updateStructData(cobraParams,solverParams);
        writeLPProblem(MILPproblem, 'problemName','COBRAMILPProblem','fileName','MILP.mps','solverParams',solverParams);
        return
    otherwise
        if isempty(solver)
            error('There is no solver for MILP problems available');
        else
            error(['Unknown solver: ' solver]);
        end
end
t = etime(clock, t_start);

%% Store results
if ~strcmp(solver,'mps')
    if (~isempty(x))
        % xInt = x(MILPproblem.intSolInd);
        % xCont = x(MILPproblem.contSolInd);
        xInt = x(vartype == 'B' | vartype == 'I');
        xCont = x(vartype == 'C');
    end

    solution.cont = xCont;
    solution.int = xInt;
    solution.obj = f;
    solution.solver = solver;
    solution.stat = solStat;
    solution.origStat = stat;
    solution.time = t;
    solution.full = x;
    if(isfield(MILPproblem, 'intSolInd'))
        solution.intInd = MILPproblem.intSolInd;
    end
end

%% Redirection function such that cplex redirects its output to the defined outputfile.
    function redirect(l)
        % Write the line of log output
        fprintf(outputfile, '%s\n', l);
    end

end