function [VMap,NMap,tMap,CMap] = raycastingMEX(camRtC2W, castingRange)

% VMap and NMap are in the world coordinate

% DDA based ray casting

global raycastingDirectionC;
global voxel;
global tsdf_value;
global tsdf_color;

camCenterW = getCameraCenter(camRtC2W);

raycastingDirectionW = transformRTdir(raycastingDirectionC,camRtC2W);

camCenterWgrid = (camCenterW - voxel.range(:,1)) / voxel.unit + 1;

tMap = NaN(1,640*480);
NMap = NaN(3,640*480);
CMap = NaN(3,640*480);

nearPlane = 0.3;
farPlane = 8.0;

step = voxel.unit;
largestep = 0.75 * voxel.mu;

tPrev = 0;

for i=1:(640*480)
    % Ray-Box Intersection to get the range

    origin = camCenterW;
    direction = raycastingDirectionW(:,i);

    % intersect ray with a box
    % http://www.siggraph.org/education/materials/HyperGraph/raytrace/rtinter3.htm
    % compute intersection of ray with all six bbox planes
    invR = 1 ./ direction;
    tbot = invR .* (voxel.range(:,1) + repmat(voxel.unit,3,1) - origin);
    ttop = invR .* (voxel.range(:,2) - repmat(voxel.unit,3,1) - origin);

    % re-order intersections to find smallest and largest on each axis
    tmin = min(ttop, tbot);
    tmax = max(ttop, tbot);

    % find the largest tmin and the smallest tmax
    largest_tmin = max(max(tmin(1), tmin(2)), max(tmin(1), tmin(3)));
    smallest_tmax = min(min(tmax(1), tmax(2)), min(tmax(1), tmax(3)));

    % check against near and far plane
    tnear = max(largest_tmin, nearPlane);
    tfar = min(smallest_tmax, farPlane);

    if tnear < tfar
        t = tnear;
        % first walk with largesteps until we found a hit
        stepsize = largestep;
        %f_t = interpolateTrilineary(origin + direction * t);
        f_t = interpolateTrilineary(camCenterWgrid + direction * t / voxel.unit);

        f_tt = 0;
        if f_t > 0     % ups, if we were already in it, then don't render anything here
            while t < tfar
                %f_tt = interpolateTrilineary(origin + direction * t);
                f_tt = interpolateTrilineary(camCenterWgrid + direction * t / voxel.unit);
                if f_tt < 0                               % got it, jump out of inner loop
                    break;
                end
                if f_tt < 0.8                            % coming closer, reduce stepsize
                    stepsize = step;
                end
                f_t = f_tt;
                t = t + stepsize;
            end
            if f_tt < 0                               % got it, calculate accurate intersection
                tMap(i) = t + stepsize * f_tt / (f_t - f_tt);
            end
        end
    end
end

% normalize normal map
NMap = NMap ./ repmat(sqrt(sum(NMap.^2,1)),3,1);

% computer vertex map
VMap = repmat(camCenterW,1,640*480) + raycastingDirectionW .* (repmat(tMap,3,1));

%imagesc(reshape(tMap,480,640)); axis equal; axis tight

return;

%camCenterWcopy = repmat( (camCenterW - voxel.range(:,1) ) / voxel.unit + 1,1,640*480);
%startPoints = camCenterWcopy + raycastingDirectionW * (castingRange(1)/ voxel.unit);
%endPoints = camCenterWcopy + raycastingDirectionW * (castingRange(2)/ voxel.unit);

% http://stellar.mit.edu/S/course/6/fa10/6.837/courseMaterial/topics/topic1/lectureNotes/13_RayTracing-Acceleration/13_RayTracing-Acceleration.pdf

% we assume the camera must be inside. otherwise, it is an error.
% so we don't test the camera

raycastingDirectionWinv = raycastingDirectionW.^-1;

maxTx = max((voxel.size_grid(1)-2-camCenterWgrid(1))*raycastingDirectionWinv(1,:),(2-camCenterWgrid(1))*raycastingDirectionWinv(1,:));
maxTy = max((voxel.size_grid(2)-2-camCenterWgrid(2))*raycastingDirectionWinv(2,:),(2-camCenterWgrid(2))*raycastingDirectionWinv(2,:));
maxTz = max((voxel.size_grid(3)-2-camCenterWgrid(3))*raycastingDirectionWinv(3,:),(2-camCenterWgrid(3))*raycastingDirectionWinv(3,:));
maxT = min(maxTx, min(maxTy, maxTz));

castingRangeGrid = castingRange/voxel.unit;
maxT = min(maxT, castingRangeGrid(2));

% parallel setting
parallelBlock = matlabpool('size');
blockDim = (640*480)/parallelBlock;
if blockDim ~= round(blockDim)
    error('thread does not align');
end

backMove = voxel.mu_grid;

tMap = NaN(1,640*480);
NMap = NaN(3,640*480);
CMap = NaN(3,640*480);

initDis = 3/ voxel.unit;

% http://www.cse.yorku.ca/~amana/research/grid.pdf
prevT = initDis;

for i=1:(640*480)
    % Ray-Box Intersection to get the range

    %{
    raycast( const Volume volume, const uint2 pos, const Matrix4 view, const float nearPlane, const float farPlane, const float step, const float largestep){
        const float3 origin = view.get_translation();
        const float3 direction = rotate(view, make_float3(pos.x, pos.y, 1.f));

        // intersect ray with a box
        // http://www.siggraph.org/education/materials/HyperGraph/raytrace/rtinter3.htm
        // compute intersection of ray with all six bbox planes
        const float3 invR = make_float3(1.0f) / direction;
        const float3 tbot = -1 * invR * origin;
        const float3 ttop = invR * (volume.dim - origin);

        // re-order intersections to find smallest and largest on each axis
        const float3 tmin = fminf(ttop, tbot);
        const float3 tmax = fmaxf(ttop, tbot);

        // find the largest tmin and the smallest tmax
        const float largest_tmin = fmaxf(fmaxf(tmin.x, tmin.y), fmaxf(tmin.x, tmin.z));
        const float smallest_tmax = fminf(fminf(tmax.x, tmax.y), fminf(tmax.x, tmax.z));

        // check against near and far plane
        const float tnear = fmaxf(largest_tmin, nearPlane);
        const float tfar = fminf(smallest_tmax, farPlane);

        if(tnear < tfar) {
            // first walk with largesteps until we found a hit
            float t = tnear;
            float stepsize = largestep;
            float f_t = volume.interp(origin + direction * t);
            float f_tt = 0;
            if( f_t > 0){     // ups, if we were already in it, then don't render anything here
                for(; t < tfar; t += stepsize){
                    f_tt = volume.interp(origin + direction * t);
                    if(f_tt < 0)                               // got it, jump out of inner loop
                        break;
                    if(f_tt < 0.8f)                            // coming closer, reduce stepsize
                        stepsize = step;
                    f_t = f_tt;
                }
                if(f_tt < 0){                               // got it, calculate accurate intersection
                    t = t + stepsize * f_tt / (f_t - f_tt);
                    return make_float4(origin + direction * t, t);
                }
            }
        }
        return make_float4(0);
    }
    %}

    tMin = castingRangeGrid(1);
    tMax = maxT(i);

    if tMin<tMax % max range > min range

        tStart = max(castingRangeGrid(1),min(tMax,prevT-backMove));

        rayDir = raycastingDirectionW(:,i);

        startPoint = camCenterWgrid + rayDir*tStart;

        % The initialization phase begins by identifying the voxel in which the ray origin, ?u, is found.
        % If the ray origin is outside the grid, we find the point in which the ray enters the grid and take the adjacent voxel.
        % The integer variables X and Y are initialized to the starting voxel coordinates.
        X = round(startPoint(1));
        Y = round(startPoint(2));
        Z = round(startPoint(3));
        valCurrentVoxel = tsdf_value(X,Y,Z);
        %valCurrentVoxel = interpValue(startPoint);

        tOptimal = NaN;

        if valCurrentVoxel == 0
            % lucky, you got the surface immediately!
            %tMap(i) = tStart;
            prevT = tStart;

            tOptimal = tStart;
        else
            if valCurrentVoxel>0
                localDir = rayDir;
                tMax = tMax-tStart;
                lookforSign = -1;
            else
                localDir = -rayDir;
                tMax = tStart-tMin;
                lookforSign = +1;
            end

            % In addition, the variables stepX and stepY are initialized to either 1 or -1 indicating
            % whether X and Y are incremented or decremented as the ray crosses voxel boundaries
            % (this is determined by the sign of the x and y components of ?v).
            stepX = sign(localDir(1));
            stepY = sign(localDir(2));
            stepZ = sign(localDir(3));

            % Next, we determine the value of t at which the ray crosses the ?rst vertical voxel boundary
            % and store it in variable tMaxX. We perform a similar computation in y and store the result in tMaxY.
            % The minimum of these two values will indicate how much we can travel along the ray and still remain in the current voxel.

            if localDir(1)>0
                tMaxX = (X+0.5 - startPoint(1))/localDir(1);
            elseif localDir(1)<0
                tMaxX = (X-0.5 - startPoint(1))/localDir(1);
            else
                tMaxX = Inf;
            end
            if localDir(2)>0
                tMaxY = (Y+0.5 - startPoint(2))/localDir(2);
            elseif localDir(2)<0
                tMaxY = (Y-0.5 - startPoint(2))/localDir(2);
            else
                tMaxY = Inf;
            end
            if localDir(3)>0
                tMaxZ = (Z+0.5 - startPoint(3))/localDir(3);
            elseif localDir(3)<0
                tMaxZ = (Z-0.5 - startPoint(3))/localDir(3);
            else
                tMaxZ = Inf;
            end

            if min(min(tMaxX,tMaxY),tMaxZ)<0
                error('tStart<0');
            end

            % Finally, we compute tDeltaX and tDeltaY.
            % tDeltaX indicates how far along the ray we must move (in units of t) for the horizontal component of such a movement to equal the width of a voxel.
            % Similarly,we store in tDeltaY the amount of movement along the ray which has a vertical component equal to the height of a voxel.
            tDeltaX = 1/abs(localDir(1));
            tDeltaY = 1/abs(localDir(2));
            tDeltaZ = 1/abs(localDir(3));

            t = 0;
            while t<tMax
                if tMaxX < tMaxY
                    if tMaxX < tMaxZ
                        X= X + stepX;
                        tMaxX= tMaxX + tDeltaX;
                    else
                        Z= Z + stepZ;
                        tMaxZ= tMaxZ + tDeltaZ;
                    end
                else
                    if tMaxY < tMaxZ
                        Y= Y + stepY;
                        tMaxY= tMaxY + tDeltaY;
                    else
                        Z= Z + stepZ;
                        tMaxZ= tMaxZ + tDeltaZ;
                    end
                end
                prevT = t;
                t = min(min(tMaxX,tMaxY),tMaxZ);

                if tsdf_value(X,Y,Z) * lookforSign > 0

                    if (tsdf_value(X,Y,Z)==-1 && valCurrentVoxel==+1) || (tsdf_value(X,Y,Z)==+1 && valCurrentVoxel==-1)
                        tOptimal = NaN;
                    else

                        % found the zero crossing points!

                        % simplest
                        % tOptimal = t;

                        % simple average
                        tOptimal = (prevT + t)/2;

                        %{
                        % buggy: Ftdt == Ft

                        if lookforSign>0
                            prevTswap = prevT;
                            prevT = t;
                            t= prevTswap;
                        end
                        XYZt   = startPoint + localDir*t;
                        XYZtdt = startPoint + localDir*prevT;

                        Ft = interpolateTrilineary(XYZt(1),XYZt(2),XYZt(3));
                        Ftdt = interpolateTrilineary(XYZtdt(1),XYZtdt(2),XYZtdt(3));

                        tOptimal = t - abs(t-prevT) * Ft / (Ftdt - Ft);
                        if tOptimal>1000
                            error('tOptimal>1000');
                        end
                        %}

                        %fprintf('i=%d: t=%f tOptimal=%f Ft=%f Ftdt=%f\n',i,t,t - abs(t-prevT) * Ft / (Ftdt - Ft), Ft, Ftdt);
                        tOptimal = tStart - tOptimal * lookforSign;

                        prevT = tOptimal;

                    end
                    break;
                end

                valCurrentVoxel = tsdf_value(X,Y,Z);
            end

        end
        if ~isnan(tOptimal)
            tMap(i) = tOptimal;

            % compute normal map

            XYZgrid = camCenterWgrid + rayDir*tOptimal;

            NMap(1,i) = interpolateTrilineary(XYZgrid(1)+1,XYZgrid(2),XYZgrid(3))-interpolateTrilineary(XYZgrid(1)-1,XYZgrid(2),XYZgrid(3));
            NMap(2,i) = interpolateTrilineary(XYZgrid(1),XYZgrid(2)+1,XYZgrid(3))-interpolateTrilineary(XYZgrid(1),XYZgrid(2)-1,XYZgrid(3));
            NMap(3,i) = interpolateTrilineary(XYZgrid(1),XYZgrid(2),XYZgrid(3)+1)-interpolateTrilineary(XYZgrid(1),XYZgrid(2),XYZgrid(3)-1);

            if ~isempty(tsdf_color)
                CMap(:,i) = interpolateTrilinearyColor(XYZgrid(1),XYZgrid(2),XYZgrid(3));
            end
        end
    end
end

% normalize normal map
NMap = NMap ./ repmat(sqrt(sum(NMap.^2,1)),3,1);

% computer vertex map
VMap = repmat(camCenterW,1,640*480) + raycastingDirectionW .* (repmat(tMap*voxel.unit,3,1));

%imagesc(reshape(tMap,480,640)); axis equal; axis tight


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

nsteps = 10;
Vfrac_final = 0.15;
Perimeter_target=3.5;
optimality_final =1e-4;
constr_final =1e-4;

BCscale_factor = 0.3;
HJiter0 = 1;
e2 = 100;
N_holes = [7 3 3];
R_holes = 0.9;
phase_holes = [pi/2 0 0];

Vfrac_initial = 0.3;
optimality_initial = 5e-2;
constr_initial = 5e-2;

TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;

% maxiter = 1;
TOL.nu_plus = 0.3;
TOL.nu_minus = 0.3;

plotting = 1;
printing = 0;
monitoring = 1;
monitoring_interval = 1;


% function [gamma,x,w,sigu,like]=champagne(y,f,sigu,nem,nd,vcs,nupd,gamma0,retx);
% Output:
% gamma(nd,nd,nv) = voxel covariance matrices
% x(nv*nd,nt) = voxel current density
% w(nv*nd,nk) = reconstruction filter
% sigu(nk,nk) = noise covariance matrix
% like(nem,1) = likelihood
%
% Input:
% #s:
% nk = # of sensors
% nv = # of voxels
% nt = # of data time points

% y(nk,nt) = sensor data % preferable to be scaled to pT units
% f(nk,nv*nd) = lead field matrix % preferable to be normalized.
% sigu(nk,nk) = noise covariance matrix,  [] for automatic initialization
% nem = maximum # of iterations
% nd = # of orientations
% vcs = voxel covariance structure: 0 = scalar, 1 = diagonal, 2 = general
% nupd = 2: learn diagonal noise covariance, = 1: learn scalar, = 0: use input noise cov
% gamma0(nd,nd,nv) = initial voxel covariances, [] for automatic initialization
% retx = 1: return voxel current density, = 0: don't

% Two parameters in code that can be adjusted are:
% eps1 = Default is 1e-8 For numerical stability while inverting model covariance matrix.
% eps1z = Default is 1e-8 For numerical stability while calculating
%         voxel variance and auxilliary variable z

function [gamma,x,w,sigu,like]=champagne(y,f,sigu,nem,nd,vcs,nupd,gamma0,retx, fig);

if vcs==2 && nd>1
    [gamma x w sigu like]=champ_mat(y,f,sigu,nem,nd,nupd,gamma0,retx, fig);
else
    [gamma x w sigu like]=champ_vec(y,f,sigu,nem,nd,vcs,nupd,gamma0,retx, fig);
end

return

function [gamma,x,w,sigu,like]=champ_vec(y,f,sigu,nem,nd,vcs,nupd,gamma0,retx, fig);

eps1=1e-8;
eps1z=1e-8;
[nk nvd]=size(f);
nv=nvd/nd;
nt=size(y,2);
nb=1000;

cyy=y*y'/nt;

% Initialize voxel variances

if isempty(gamma0)
    f2=sum(f.^2,1);
    invf2=zeros(1,nvd);
    ff=find(f2>0);
    invf2(ff)=1./f2(ff);
    w=spdiags(invf2',0,nvd,nvd)*f';

    if nt>nb
        inu0=zeros(nvd,1);
        for it=1:nb:nt-nb+1
            inu0=inu0+sum((w*y(:,it:it+nb-1)).^2,2);
        end
        inu0=(inu0+sum((w*y(:,it+nb:nt)).^2,2))/nt;
    else
        inu0=mean((w*y).^2,2);
    end
%    disp(max(inu0-mean((w*y).^2,2)));
    inu0v=mean(reshape(inu0,nd,nv),1);
    inu1=reshape(ones(nd,1)*inu0v,nvd,1);
    vvec=inu1;
else
    vvec=zeros(nvd,1);
    for iv=1:nv
        jv=(iv-1)*nd+1:iv*nd;
        if vcs==0
            vvec(jv)=mean(diag(gamma0(:,:,iv)));
        else
            vvec(jv)=diag(gamma0(:,:,iv));
        end
    end
end

% initialize noise covariance

if nupd>0
    if isempty(sigu)
        ndr=.1;
        sigu=ndr*mean(mean(y.^2))*eye(nk);
    else
        sigu=mean(diag(sigu))*eye(nk);
    end
end

% Learn voxel variances

v=zeros(nv,1);
figure(fig);

like=zeros(nem,1);
for iem=1:nem
    vmat=spdiags(vvec,0,nvd,nvd);
    c=f*vmat*f'+sigu;
    [p d]=svd(c);
    d=max(real(diag(d)),0);
    invd=zeros(nk,1);
    ff=find(d>=eps1);
    invd(ff)=1./d(ff);
%    invd=1./d;
    invc=p*spdiags(invd,0,nk,nk)*p';

%    like(iem)=-.5*(sum(log(d))+nk*log(2*pi))-.5*sum(sum(y.*(invc*y)))/nt;
    like(iem)=-.5*(sum(log(max(d,eps1)))+nk*log(2*pi))-.5*sum(sum(invc.*cyy));
    subplot(2,2,1);plot((1:iem),like(1:iem));
    title(['Likelihood: ' int2str(iem) ' / ' int2str(nem)]);
    xlabel('iteration');
    set(gca(),'XLim',[0 iem]);

    fc=f'*invc;
    w=vmat*fc;
    x2=sum((w*cyy).*w,2);
    z=sum(fc.*f',2);

    if vcs==0
        x20=sum(reshape(x2,nd,nv),1);
        z0=sum(reshape(z,nd,nv),1);
        v0=(sqrt(z0)./max(z0,eps1z)).*sqrt(x20);
        vvec=reshape(ones(nd,1)*v0,nvd,1);
    else
        vvec=(sqrt(z)./max(z,eps1z)).*sqrt(x2);
    end

    if nupd>0
        fw=eye(nk)-f*w;
        sigy1=sum((fw*cyy).*fw,2);
        fgf=sum((fw*f*vmat).*f,2);
        ilam=sigy1+fgf;
        if nupd==1
            sigu=mean(ilam)*eye(nk);
        else
            sigu=diag(ilam);
        end
    end

    v=sum(reshape(vvec,nd,nv),1);
    subplot(2,2,2);plot((1:nv),v);
    title(['Voxel power: ' num2str(nv) ' / ' num2str(nv)]);
    xlabel('voxel index');
    set(gca(),'XLim',[1 nv]);
    drawnow
end

if retx==1
    x=w*y;
else
    x=[];
end

if nd==1
    gamma=reshape(vvec,1,1,nv);
else
    gamma=zeros(nd,nd,nv);
    for iv=1:nv
        gamma(:,:,iv)=diag(vvec((iv-1)*nd+1:iv*nd));
    end
end

return

function [gamma,x,w,sigu,like]=champ_mat(y,f,sigu,nem,nd,nupd,gamma0,retx, fig);

eps1=1e-8;
eps1z=1e-8;
[nk nvd]=size(f);
nv=nvd/nd;
nt=size(y,2);
nb=1000;

cyy=y*y'/nt;

% Initialize voxel covariances

if isempty(gamma0)
    f2=sum(f.^2,1);
    invf2=zeros(1,nvd);
    ff=find(f2>0);
    invf2(ff)=1./f2(ff);
    w=spdiags(invf2',0,nvd,nvd)*f';

    if nt>nb
        inu0=zeros(nvd,1);
        for it=1:nb:nt-nb+1
            inu0=inu0+sum((w*y(:,it:it+nb-1)).^2,2);
        end
        inu0=(inu0+sum((w*y(:,it+nb:nt)).^2,2))/nt;
    else
        inu0=mean((w*y).^2,2);
    end
%    disp(max(inu0-mean((w*y).^2,2)));
    inu0v=mean(reshape(inu0,nd,nv),1);
    inu1=reshape(ones(nd,1)*inu0v,nvd,1);
    vmat=spdiags(inu1,0,nvd,nvd);
else
    vmat=sparse(nvd,nvd);
    for iv=1:nv
        jv=(iv-1)*nd+1:iv*nd;
        vmat(jv,jv)=gamma0(:,:,iv);
    end
end

% initialize noise covariance

if nupd>0
    if isempty(sigu)
        ndr=.1;
        sigu=ndr*mean(mean(y.^2))*eye(nk);
    else
        sigu=mean(diag(sigu))*eye(nk);
    end
end

% Learn voxel covariances

v=zeros(nv,1);
figure;

like=zeros(nem,1);
for iem=1:nem
    c=f*vmat*f'+sigu;
    [p d]=svd(c);
    d=max(real(diag(d)),0);
    invd=zeros(nk,1);
    ff=find(d>=eps1);
    invd(ff)=1./d(ff);
%    invd=1./d;
    invc=p*spdiags(invd,0,nk,nk)*p';

%    like(iem)=-.5*(sum(log(d))+nk*log(2*pi))-.5*sum(sum(y.*(invc*y)))/nt;
    like(iem)=-.5*(sum(log(max(d,eps1)))+nk*log(2*pi))-.5*sum(sum(invc.*cyy));
     figure(fig)
    subplot(2,2,1);plot((1:iem),like(1:iem));
    title(['Likelihood: ' int2str(iem) ' / ' int2str(nem)]);
    xlabel('iteration');
    set(gca(),'XLim',[0 iem]);

    fc=f'*invc;
    w=vmat*fc;
%    x=w*y;
%    x2=mean(x.^2,2);
%    z=sum(fc.*f',2);

    for iv=1:nv
        jv=((iv-1)*nd+1:iv*nd);
        x2=w(jv,:)*cyy*w(jv,:)';
        z=fc(jv,:)*f(:,jv);

        [pz dz]=eig(z);
        dzd=max(real(diag(dz)),0);
        dz5=sqrt(dzd);
        z5=pz*diag(dz5)*pz';
        invdz5=1./sqrt(max(dzd,eps1z));
        invz5=pz*diag(invdz5)*pz';

        [px dx]=eig(z5*x2*z5);
        dx5=sqrt(max(real(diag(dx)),0));
        cx5=px*diag(dx5)*px';
        vmat(jv,jv)=invz5*cx5*invz5;
        v(iv)=sum(diag(vmat(jv,jv)));
    end

    if nupd>0
        fw=eye(nk)-f*w;
        sigy1=sum((fw*cyy).*fw,2);
        fgf=sum((fw*f*vmat).*f,2);
        ilam=sigy1+fgf;
        if nupd==1
            sigu=mean(ilam)*eye(nk);
        else
            sigu=diag(ilam);
        end
    end

    subplot(2,2,2);plot((1:nv),v);
    title(['Voxel power: ' num2str(nv) ' / ' num2str(nv)]);
    xlabel('voxel index');
    set(gca(),'XLim',[1 nv]);
    drawnow
end

if retx==1
    x=w*y;
else
    x=[];
end

gamma=zeros(nd,nd,nv);
for iv=1:nv
    jv=(iv-1)*nd+1:iv*nd;
    gamma(:,:,iv)=vmat(jv,jv);
end

return


function [g1, g2] = lfmaXlfmaKernGradient(lfmKern1, lfmKern2, t1, t2, covGrad, meanVector)

% LFMAXLFMAKERNGRADIENT Compute a cross gradient between a LFMA and a LFMA.
% FORMAT
% DESC computes cross gradient of parameters of a cross kernel
% between two lfm kernels, both lfm kernels correspond to the accelerations.
% with the multiple output kernel.
% ARG lfmKern1 : the kernel structure associated with the first LFM
% kernel (acceleration).
% ARG lfmKern2 : the kernel structure associated with the second LFM
% kernel (acceleration).
% ARG t1 : inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% RETURN g1 : gradient of the parameters of the first kernel, for
% ordering see lfmKernExtractParam.
% RETURN g2 : gradient of the parameters of the second kernel, for
% ordering see lfmKernExtractParam.
%
% FORMAT
% DESC computes cross gradient of parameters of a cross kernel
% between two lfm kernels, both lfm kernels correspond to the accelerations.
% with the multiple output kernel.
% ARG lfmKern1 : the kernel structure associated with the first LFM
% kernel (acceleration).
% ARG lfmKern2 : the kernel structure associated with the second LFM
% kernel (acceleration).
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% RETURN g1 : gradient of the parameters of the first kernel, for
% ordering see lfmKernExtractParam.
% RETURN g2 : gradient of the parameters of the second kernel, for
% ordering see lfmKernExtractParam.
%
% FORMAT
% DESC computes cross gradient of parameters of a cross kernel
% between two lfm kernels. Both kernels correspond to accelerations.
% It is supposed to be used together with the multiple output kernel.
% ARG lfmKern1 : the kernel structure associated with the first LFM
% kernel (acceleration).
% ARG lfmKern2 : the kernel structure associated with the second LFM
% kernel (acceleration).
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% ARG meanVec : precomputed factor that is used for the switching dynamical
% latent force model.
% RETURN g1 : gradient of the parameters of the first kernel, for
% ordering see lfmKernExtractParam.
% RETURN g2 : gradient of the parameters of the second kernel, for
% ordering see lfmKernExtractParam.
%
% SEEALSO : multiKernParamInit, multiKernCompute, lfmvKernParamInit
%
% COPYRIGHT : Mauricio Alvarez, 2010

% KERN

subComponent = false; % This is just a flag that indicates if this kernel is part of a bigger kernel (SDLFM)

if nargin == 4
    covGrad = t2;
    t2 = t1;
elseif nargin == 6
    subComponent = true;
    if numel(meanVector)>1
        if size(meanVector,1) == 1,
            if size(meanVector, 2)~=size(covGrad, 2)
                error('The dimensions of meanVector don''t correspond to the dimensions of covGrad')
            end
        else
            if size((meanVector'), 2)~=size(covGrad,2)
                error('The dimensions of meanVector don''t correspond to the dimensions of covGrad')
            end
        end
    else
        if numel(t1)==1 && numel(t2)>1
            % matGrad will be row vector and so should be covGrad
            dimcovGrad = length(covGrad);
            covGrad = reshape(covGrad, [1 dimcovGrad]);
        elseif numel(t1)>1 && numel(t2)==1
            % matGrad will be column vector and sp should be covGrad
            dimcovGrad = length(covGrad);
            covGrad = reshape(covGrad, [dimcovGrad 1]);
        end
    end
end

if size(t1, 2) > 1 || size(t2, 2) > 1
    error('Input can only have one column');
end
if lfmKern1.inverseWidth ~= lfmKern2.inverseWidth
    error('Kernels cannot be cross combined if they have different inverse widths.')
end

% Parameters of the simulation (in the order providen by kernExtractParam)

m = [lfmKern1.mass lfmKern2.mass];                  % Par. 1
D = [lfmKern1.spring lfmKern2.spring];              % Par. 2
C = [lfmKern1.damper lfmKern2.damper];              % Par. 3
sigma2 = 2/lfmKern1.inverseWidth;                   % Par. 4
sigma = sqrt(sigma2);
S = [lfmKern1.sensitivity lfmKern2.sensitivity];    % Par. 5

alpha = C./(2*m);
omega = sqrt(D./m-alpha.^2);

% Initialization of vectors and matrices

g1 = zeros(1,5);
g2 = zeros(1,5);

% Precomputations
computeH  = cell(4,1);
computeUpsilonMatrix = cell(2,1);
computeUpsilonVector = cell(2,1);
gradientUpsilonMatrix = cell(4,1);
gradientUpsilonVector = cell(4,1);
gamma1_p = alpha(1) + j*omega(1);
gamma1_m = alpha(1) - j*omega(1);
gamma2_p = alpha(2) + j*omega(2);
gamma2_m = alpha(2) - j*omega(2);
preExp1 = zeros(length(t1),2);
preExp2 = zeros(length(t2),2);
preExpg1 = zeros(length(t1),2);
preExpgg1 = zeros(length(t1),2);
preExpg2 = zeros(length(t2),2);
preExpgg2 = zeros(length(t2),2);
preExpt1 = zeros(length(t1),2);
preExpt2 = zeros(length(t2),2);
preGamma(1) = gamma1_p + gamma2_p;
preGamma(2) = gamma1_p + gamma2_m;
preGamma(3) = gamma1_m + gamma2_p;
preGamma(4) = gamma1_m + gamma2_m;
preGamma2 = preGamma.^2;
preConst = 1./preGamma;
preConst2 = 1./(preGamma2);
preFactors(1) = preConst(2) - preConst(1);
preFactors(2) = preConst(3) - preConst(4);
preFactors(3) = preConst(3) - preConst(1);
preFactors(4) = preConst(2) - preConst(4);
preFactors2(1) = -preConst2(2) + preConst2(1);
preFactors2(2) = -preConst2(3) + preConst2(4);
preFactors2(3) = -preConst2(3) + preConst2(1);
preFactors2(4) = -preConst2(2) + preConst2(4);
preExp1(:,1) = exp(-gamma1_p*t1);
preExp1(:,2) = exp(-gamma1_m*t1);
preExp2(:,1) = exp(-gamma2_p*t2);
preExp2(:,2) = exp(-gamma2_m*t2);
preExpg1(:,1) = (2*gamma1_p)*preExp1(:,1);
preExpg1(:,2) = (2*gamma1_m)*preExp1(:,2);
preExpg2(:,1) = (2*gamma2_p)*preExp2(:,1);
preExpg2(:,2) = (2*gamma2_m)*preExp2(:,2);
preExpgg1(:,1) = (gamma1_p^2)*preExp1(:,1);
preExpgg1(:,2) = (gamma1_m^2)*preExp1(:,2);
preExpgg2(:,1) = (gamma2_p^2)*preExp2(:,1);
preExpgg2(:,2) = (gamma2_m^2)*preExp2(:,2);
preExpt1(:,1) = t1.*preExpgg1(:,1);
preExpt1(:,2) = t1.*preExpgg1(:,2);
preExpt2(:,1) = t2.*preExpgg2(:,1);
preExpt2(:,2) = t2.*preExpgg2(:,2);
[computeH{1}, computeUpsilonMatrix{1}, computeUpsilonMatrixLocal{1}] =  lfmComputeH3AA(gamma1_p, gamma1_m, sigma2, t1,t2,preFactors([1 2]), 0);
[computeH{2}, computeUpsilonMatrix{2}, computeUpsilonMatrixLocal{2}] =  lfmComputeH3AA(gamma2_p, gamma2_m, sigma2, t2,t1,preFactors([3 4]), 1);
[computeH{3}, computeUpsilonVector{1}, computeUpsilonVectorLocal{1}] =  lfmComputeH4AA(gamma1_p, gamma1_m, sigma2, t1, preGamma([1 2 4 3]), preExpgg2, 0 );
[computeH{4}, computeUpsilonVector{2}, computeUpsilonVectorLocal{2}] =  lfmComputeH4AA(gamma2_p, gamma2_m, sigma2, t2, preGamma([1 3 4 2]), preExpgg1, 1 );
preKernel = ( computeH{1} + computeH{2}.' + computeH{3} + computeH{4}.');

% Precompute derivatives
gradientUpsilonMatrix{1} = lfmaaGradientUpsilonMatrix(gamma1_p,sigma2,t1, t2, 0, computeUpsilonMatrixLocal{1}{1});
gradientUpsilonMatrix{2} = lfmaaGradientUpsilonMatrix(gamma1_m,sigma2,t1, t2, 0, computeUpsilonMatrixLocal{1}{2});
gradientUpsilonMatrix{3} = lfmaaGradientUpsilonMatrix(gamma2_p,sigma2,t2, t1, 1, computeUpsilonMatrixLocal{2}{1});
gradientUpsilonMatrix{4} = lfmaaGradientUpsilonMatrix(gamma2_m,sigma2,t2, t1, 1, computeUpsilonMatrixLocal{2}{2});
gradientUpsilonVector{1} = lfmapGradientUpsilonVector(gamma1_p,sigma2,t1, computeUpsilonVectorLocal{1}{1});
gradientUpsilonVector{2} = lfmapGradientUpsilonVector(gamma1_m,sigma2,t1, computeUpsilonVectorLocal{1}{2});
gradientUpsilonVector{3} = lfmapGradientUpsilonVector(gamma2_p,sigma2,t2, computeUpsilonVectorLocal{2}{1});
gradientUpsilonVector{4} = lfmapGradientUpsilonVector(gamma2_m,sigma2,t2, computeUpsilonVectorLocal{2}{2});

if lfmKern1.isNormalised
    K0 =  lfmKern1.sensitivity*lfmKern2.sensitivity/(8*sqrt(2)*lfmKern1.mass*lfmKern2.mass*prod(omega));
    K02 = 1/(8*sqrt(2)*prod(m)*prod(omega));
else
    K0 =  sigma*sqrt(pi)*lfmKern1.sensitivity*lfmKern2.sensitivity/(8*lfmKern1.mass*lfmKern2.mass*prod(omega));
    K02 = sigma*sqrt(pi)/(8*prod(m)*prod(omega));
end

% Gradient with respect to m, D and C
for ind_theta = 1:3 % Parameter (m, D or C)
    for ind_par = 0:1 % System (1 or 2)
        % Choosing the right gradients for m, omega, gamma1 and gamma2
        switch ind_theta
            case 1  % Gradient wrt m
                gradThetaM = [1-ind_par ind_par];
                gradThetaAlpha = -C./(2*(m.^2));
                gradThetaOmega = (C.^2-2*m.*D)./(2*(m.^2).*sqrt(4*m.*D-C.^2));
            case 2  % Gradient wrt D
                gradThetaM = zeros(1,2);
                gradThetaAlpha = zeros(1,2);
                gradThetaOmega = 1./sqrt(4*m.*D-C.^2);
            case 3  % Gradient wrt C
                gradThetaM = zeros(1,2);
                gradThetaAlpha = 1./(2*m);
                gradThetaOmega = -C./(2*m.*sqrt(4*m.*D-C.^2));
        end
        gradThetaGamma1 = gradThetaAlpha + j*gradThetaOmega;
        gradThetaGamma2 = gradThetaAlpha - j*gradThetaOmega;
        % Gradient evaluation
        gradThetaGamma11 = [gradThetaGamma1(1) gradThetaGamma2(1)];
        gradThetaGamma2 = [gradThetaGamma1(2) gradThetaGamma2(2)];
        gradThetaGamma1 = gradThetaGamma11;

        if ~ind_par % ind_par = k or d
            matGrad = K0 ...
                * ( lfmGradientH31( preFactors([1 2]), preFactors2([1 2]), gradThetaGamma1, ...
                gradientUpsilonMatrix{1}, gradientUpsilonMatrix{2}, computeUpsilonMatrix{1}{1}, ...
                computeUpsilonMatrix{1}{2}, 1) + ...
                lfmGradientH32( preGamma2,  gradThetaGamma1, computeUpsilonMatrix{2}{1}, ...
                computeUpsilonMatrix{2}{2}, 1).' + ...
                lfmGradientH41( preGamma, preGamma2, gradThetaGamma1, preExpgg2, ...
                gradientUpsilonVector{1}, gradientUpsilonVector{2}, computeUpsilonVector{1}{1},...
                computeUpsilonVector{1}{2}, 1) + ...
                lfmGradientH42AP(preGamma, preGamma2, gradThetaGamma1, preExpg1, preExpgg1, preExpt1, ...
                computeUpsilonVector{2}{1}, computeUpsilonVector{2}{2}).'...
                - (gradThetaM(1+ind_par)/m(1+ind_par) ...
                + gradThetaOmega(1+ind_par)/omega(1+ind_par)) ...
                *preKernel);
        else % ind_par = r or d'
            matGrad = K0 ...
                * ( lfmGradientH31( preFactors([3 4]), preFactors2([3 4]), gradThetaGamma2, ...
                gradientUpsilonMatrix{3}, gradientUpsilonMatrix{4}, computeUpsilonMatrix{2}{1}, ...
                computeUpsilonMatrix{2}{2}, 1).' + ...
                lfmGradientH32( preGamma2([1 3 2 4]),  gradThetaGamma2, computeUpsilonMatrix{1}{1}, ...
                computeUpsilonMatrix{1}{2}, 1) + ...
                lfmGradientH41( preGamma([1 3 2 4]), preGamma2([1 3 2 4]), gradThetaGamma2, preExpgg1, ...
                    gradientUpsilonVector{3}, gradientUpsilonVector{4}, computeUpsilonVector{2}{1},...
                    computeUpsilonVector{2}{2}, 1).' + ...
                lfmGradientH42AP(preGamma([1 3 2 4]), preGamma2([1 3 2 4]), gradThetaGamma2, preExpg2, preExpgg2, preExpt2, ...
                computeUpsilonVector{1}{1}, computeUpsilonVector{1}{2})...
                - (gradThetaM(1+ind_par)/m(1+ind_par) ...
                + gradThetaOmega(1+ind_par)/omega(1+ind_par)) ...
                *preKernel);
        end

        if subComponent
            if size(meanVector,1) ==1,
                matGrad = matGrad*meanVector;
            else
                matGrad = (meanVector*matGrad).';
            end
        end

        % Check the parameter to assign the derivative
        if ind_par == 0
            g1(ind_theta) = sum(sum(matGrad.*covGrad));
        else
            g2(ind_theta) = sum(sum(matGrad.*covGrad));
        end
    end
end

% Gradients with respect to sigma
if lfmKern1.isNormalised
    matGrad = K0*(lfmGradientSigmaH3AA(gamma1_p, gamma1_m, sigma2, t1, t2, preFactors([1 2]), 0)...
        +  lfmGradientSigmaH3AA(gamma2_p, gamma2_m, sigma2, t2, t1, preFactors([3 4]), 1).'...
        +  lfmGradientSigmaH4AA(gamma1_p, gamma1_m, sigma2, t1, preGamma([1 2 4 3]), preExpgg2)...
        +  lfmGradientSigmaH4AA(gamma2_p, gamma2_m, sigma2, t2, preGamma([1 3 4 2]), preExpgg1).');
else
    matGrad = (prod(S)*sqrt(pi)/(8*prod(m)*prod(omega))) ...
        * (preKernel ...
        + sigma*(lfmGradientSigmaH3AA(gamma1_p, gamma1_m, sigma2, t1, t2, preFactors([1 2]), 0)...
        +  lfmGradientSigmaH3AA(gamma2_p, gamma2_m, sigma2, t2, t1, preFactors([3 4]), 1).'...
        +  lfmGradientSigmaH4AA(gamma1_p, gamma1_m, sigma2, t1, preGamma([1 2 4 3]), preExpgg2)...
        +  lfmGradientSigmaH4AA(gamma2_p, gamma2_m, sigma2, t2, preGamma([1 3 4 2]), preExpgg1).' ));
end

if subComponent
    if size(meanVector,1) ==1,
        matGrad = matGrad*meanVector;
    else
        matGrad = (meanVector*matGrad).';
    end
end

g1(4) = sum(sum(matGrad.*covGrad))*(-(sigma^3)/4);
g2(4) = g1(4);

% Gradients with respect to S

matGrad = K02*preKernel;

if subComponent
    if size(meanVector,1) ==1,
        matGrad = matGrad*meanVector;
    else
        matGrad = (meanVector*matGrad).';
    end
end

g1(5) = sum(sum(S(2)*matGrad.*covGrad));
g2(5) = sum(sum(S(1)*matGrad.*covGrad));

g2(4) = 0; % Otherwise is counted twice, temporarly changed by Mauricio Alvarez

g1 = real(g1);
g2 = real(g2);


%% RRT*FN Toolbox
% *RRT*FN Toolbox* for solving path/motion planning problems.
% Toolbox provides *RRT*, *RRT** and *RRT*FN* for solving various problems
% (e.g. 2D mobile robot, planar redundant manipulator)
%
% Sampling-based motion planning algorithms got a lot of research attention
% in past decade or so. The main reason of the popularity is an ability
% to solve relatively easy motion planning problems of 2D mobile robot
% as well as hard high dimensional problems both.
%
% *Rapidly-Exporing Random Tree (RRT)* is sampling-based algorithm, solves
% the problem of motion and path planning providing feasible solutions
% however it does not take into account optimality of the solution.
%
% *RRT** is probabilistically optimal variant of RRT which converges to the
% optimal solution asymtotically. Due to the fact that RRT* takes cost of
% the path into consideration, the computational diffuculty increases with
% each node added to tree, a good example of this problem is finding
% neighbors.
%
% *RRT*FN (Fixed Nodes)* is a memory efficient variant of RRT*. It
% inherents all the optimization features of RRT*, but in contrast using
% limited number of nodes i.e. memory is limited.
%
%%
%

% Please do not change anything in rrt.m,
% rrt_star.m and rrt_star_fn.m files unless it is a bug.
Everything
% you want to add (e.g. new function or modifications of a model)
% please do it creating/editing classes for models.
% e.g. FNSimple2D.m or
% FNRedundantManipulator.m
%

%


function [N] = kN2N(kN)
% Convert force from kilonewtons to newtons.
% Chad A. Greene 2012
N = kN*1000;


function [elist,nextfront]=advancefront(edges,loop,elen)
%
% [elist,nextfront]=advancefront(edges,loop,elen)
%
% advance an edge-front on an oriented surface to the next separated by
% one-element width
%
% author: Qianqian Fang, <q.fang at neu.edu>
% date: 2012/02/09
%
% input:
%      edges: edge list of an oriented surface mesh, must be in CCW order
%      loop: a 2-column array, specifying a closed loop in CCW order
%      elen: node number inside each element, if ignored, elen is set to 3
%
% output:
%      elist: list of triangles that is enclosed between the two
%             edge-fronts
%      nextfront: a new edge loop list representing the next edge-front
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

elist=[];
nextfront=[];

if(nargin<3) elen=3; end

[hasedge, loc]=ismember(loop,edges,'rows');

if(~all(hasedge))
   error('loop edge is not defined in the mesh');
end

nodenum=size(edges,1)/elen;

elist=unique(mod(loc-1,nodenum))+1;
nextfront=edges(elist,:);
for i=1:elen-1
    nextfront=[nextfront;edges(elist+nodenum*i,:)];
end
nextfront=setdiff(nextfront,loop,'rows');

% remove reversed edge pairs
[flag,loc]=ismember(nextfront,nextfront(:,[2 1]),'rows');
id=find(flag);
if(~isempty(id))
    delmark=flag;
    delmark(loc(find(loc>0)))=1;
    nextfront(find(delmark),:)=[];
end
nextfront=nextfront(:,[2 1]); % reverse this loop, as it is reversed to the input loop


function [surf, labels] = imSurfaceAreaEstimate(img, varargin)
% Estimate surface area of a binary 3D structure.
%
%   Usage
%   S = imSurfaceAreaEstimate(IMG)
%   Estimate the surface area of the structure within the image, without
%   measuring surface area of borders.
%   The aim of this function is to be called by the "imSurfaceAreaDensity"
%   function, for providing an estimate of surface area density within a
%   representative volume of interest.
%
%
%   Example
%   imSurfaceAreaEstimate
%
%   See also
%   imSurfaceArea, imSurfaceAreaDensity
%

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2010-07-26,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRAE - Cepia Software Platform.

%% Process input arguments

% check image dimension
if ndims(img) ~= 3
    error('first argument should be a 3D image');
end

% in case of a label image, return a vector with a set of results
if ~islogical(img)
    labels = unique(img);
    labels(labels==0) = [];
    surf = zeros(length(labels), 1);
    for i = 1:length(labels)
        surf(i) = imSurfaceAreaEstimate(img==labels(i), varargin{:});
    end
    return;
end

%% Process input arguments

% in case of binary image, compute only one label...
labels = 1;

% default number of directions
nDirs = 13;

% default image resolution
delta = [1 1 1];

% Process user input arguments
while ~isempty(varargin)
    var = varargin{1};
    if ~isnumeric(var)
        error('option should be numeric');
    end

    % option is either connectivity or resolution
    if isscalar(var)
        nDirs = var;
    else
        delta = var;
    end
    varargin(1) = [];
end

%% Use the LUT to estimate surface area

lut = imSurfaceAreaLut(delta, nDirs);
bch = imBinaryConfigHisto(img);
surf = sum(bch .* lut);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Max manipulability index ALONG A LINE.
% Use stomp like to optimize along a surface/line
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function experiment1B
close all;
global robot
global parameters
global hfigures

%STOMP PARAMETERS
%conversion from cost to Prob factor
parameters.lambda = .4;
parameters.lambda_obstacles = .2;
%height of the obstacle
parameters.yo = 2.5;
%cost function starts at this distance
%must be below 0.3 for the 4 DOF robot
parameters.epsilon = 0.2;
%multiply noise by this facto
%parameters.noise_k = 5;
%parameters.noise_sigma_null_space = 0.01;
parameters.alpha=0.02;
parameters.time_step=0.01;

%number of waypoints
parameters.N = 12;
%number of particles
parameters.K = 1;
%repeat experiment number of times
parameters.n_repeat = 30;
parameters.experiment_name = 'experiment1B_K1_N30.mat';
parameters.animate = 0;

parameters.obstacles = [];
%LINE 1
x1 = -1.5;
y1 = .5; %m
x2 = 0;
y2 = 2; %m
phi = 3*pi/4;
p0 = [x1 y1 0]';
pf = [x2 y2 0]';
T0 = build_T_4dof(p0, phi);
parameters.obstacles{1}.line = [p0 pf];
parameters.obstacles{1}.T0 = T0;

%repeat the experiment E times
random_manips=[];
Gout = [];
for i=1:parameters.n_repeat
    close all
    [pk, final_manip] = stomp_null_space(robot);
    Gout{i}=pk;
    random_manips = [random_manips; final_manip];
    save(parameters.experiment_name)
end

function T = build_T_4dof(p, phi)
T = [cos(phi) -sin(phi) 0 p(1);
     sin(phi) cos(phi) 0 p(2);
     0            0     1  p(3);
     0             0    0   1];

 function T = build_T_sawyer(p, phi)
T = [1  -sin(phi) 0 p(1);
     0  cos(phi) 0 p(2);
     0            0     1  p(3);
     0             0    0   1];


function [data] = bz_shuffleCircular(data)
% circularly shifts each row

for i=1:size(data,1)
    data(i,:) = circshift(data(i,:),randi(size(data,2)));
end


%IBVS   Implement classical IBVS for point features
%
%  results = ibvs(T)
%  results = ibvs(T, params)
%
%  Simulate IBVS with for a square target comprising 4 points is placed
%  in the world XY plane. The camera/robot is initially at pose T and is
%  driven to the orgin.
%
%  Two windows are shown and animated:
%   1. The camera view, showing the desired view (*) and the
%      current view (o)
%   2. The external view, showing the target points and the camera
%
% The results structure contains time-history information about the image
% plane, camera pose, error, Jacobian condition number, error norm, image
% plane size and desired feature locations.
%
% The params structure can be used to override simulation defaults by
% providing elements, defaults in parentheses:
%
%   target_size    - the side length of the target in world units (0.5)
%   target_center  - center of the target in world coords (0,0,3)
%   niter          - the number of iterations to run the simulation (500)
%   eterm          - a stopping criteria on feature error norm (0)
%   lambda         - gain, can be scalar or diagonal 6x6 matrix (0.01)
%   ci             - camera intrinsic structure (camparam)
%   depth          - depth of points to use for Jacobian, scalar for
%                    all points, of 4-vector.  If null take actual value
%                    from simulation      ([])
%
% SEE ALSO: ibvsplot

% IMPLEMENTATION NOTE
%
% 1.  As per task function notation (Chaumette papers) the error is
%     defined as actual-demand, the reverse of normal control system
%     notation.
% 2.  The gain, lambda, is always positive
% 3.  The negative sign is written into the control law

classdef IBVS_sph < VisualServo

    properties
        pt          % phi, theta
        pt_star

        lambda          % IBVS gain
        eterm

        depth

        proj_lines
    end

    methods

        function ibvs = IBVS_sph(cam, varargin)

            % invoke superclass constructor
            ibvs = ibvs@VisualServo(cam, varargin{:});

            % handle arguments
            opt.eterm = 0.01;
            opt.lambda = 0.04;         % control gain
            opt.depth = [];
            opt.example = false;
            opt.T0 = SE3(0.3, 0.3, -2)*SE3.Rz(0.2);
            opt.Tf = SE3(0, 0, -1.5)*SE3.Rz(1);

            opt = tb_optparse(opt, ibvs.arglist);

            % copy options to IBVS object
            if opt.example
                % run a canned example
                fprintf('---------------------------------------------------\n');
                fprintf('canned example, spherical IBVS with 4 points\n');
                fprintf('---------------------------------------------------\n');
                ibvs.P = mkgrid(2, 1.5, 'pose', SE3(0,0,0.5));
                ibvs.Tf = SE3(0.3, 0.3, -2)*SE3.Rz(0.2);
                ibvs.T0 = SE3.convert(opt.T0);
                %ibvs.T0 = transl(-1,-0.1,-3);%*trotx(0.2);
            else
                ibvs.Tf = SE3(0, 0, -1.5)*SE3.Rz(1);
                ibvs.T0 = SE3.convert(opt.T0);
            end

            ibvs.lambda = opt.lambda;
            ibvs.eterm = opt.eterm;
            ibvs.depth = opt.depth;

            clf
            subplot(121);
            ibvs.camera.plot_create(gca)
            ibvs.camera.plot(ibvs.P, 'ro')

            % this is the 'external' view of the points and the camera
            subplot(122)
            plot_sphere(ibvs.P, 0.08, 'r');
            ibvs.camera.plot_camera('pose', ibvs.T0);
            plotvol([-2 2 -2 2 -3 1])
            view(16, 28);
            grid on
            set(gcf, 'Color', 'w')

            % draw lines from points to centre of sphere
            centre = ibvs.camera.T.t;
            ibvs.proj_lines = gobjects;
            for i=1:numcols(ibvs.P)
                ibvs.proj_lines(i) = plot3([centre(1) ibvs.P(1,i)], [centre(2) ibvs.P(2,i)], [centre(3) ibvs.P(3,i)], 'k')
            end

            set(gcf, 'HandleVisibility', 'Off');

            ibvs.type = 'spherical';
        end

        function init(vs)

            if isempty(vs.Tf)
                vs.Tf = transl(0, 0, -1);
                warning('setting Tf to default');
            end
            %% initialize the vservo variables
            vs.camera.T = vs.T0;    % set camera back to its initial pose
            vs.Tcam = vs.T0;                % initial camera/robot pose

            % final pose is specified in terms of a camera-target pose
            %   convert to image coords
            vs.pt_star = vs.camera.project(vs.P, 'pose', vs.Tf);

            vs.history = [];
        end

        function status = step(vs)
            status = 0;
            Zest = [];

                        % plot the points on mage plane
            vs.camera.clf();
            vs.camera.hold(true);
            vs.camera.plot(vs.P, 'pose', vs.Tcam, 'ro');
            vs.camera.plot(vs.pt_star, 'r*');

            % compute the view
            pt = vs.camera.project(vs.P, 'pose', vs.Tcam);

            % compute image plane error as a column
            e = pt - vs.pt_star;   % feature error
            e(2,:) = angdiff(e(2,:));
            e = e(:);

            % compute the Jacobian
            if isempty(vs.depth)
                % exact depth from simulation (not possible in practice)
                P_C = homtrans(inv(vs.Tcam), vs.P);
                J = vs.camera.visjac_p(pt, P_C(3,:) );
            else
                J = vs.camera.visjac_p(pt, vs.depth );
            end

            % compute the velocity of camera in camera frame
            try
                v = -vs.lambda * pinv(J) * e;
            catch
                status = -1;
                return
            end

            if vs.verbose
                fprintf('v: %.3f %.3f %.3f %.3f %.3f %.3f\n', v);
            end

            % update the camera pose
            Td = trnorm(delta2tr(v));    % differential motion

            vs.Tcam = vs.Tcam .* SE3(Td);       % apply it to current pose

            % update the camera pose
            vs.camera.T = vs.Tcam;

            % draw lines from points to centre of sphere
            centre = vs.Tcam.t;
            for i=1:numcols(vs.P)
                vs.proj_lines(i).XData = [centre(1) vs.P(1,i)];
                vs.proj_lines(i).YData = [centre(2) vs.P(2,i)];
                vs.proj_lines(i).ZData = [centre(3) vs.P(3,i)];
            end

            % update the history variables
            hist.pt = pt(:);
            vel = tr2delta(Td);
            hist.vel = vel;
            hist.e = e;
            hist.en = norm(e);
            hist.jcond = cond(J);
            hist.Tcam = vs.Tcam;

            vs.history = [vs.history hist];

            if norm(e) < vs.eterm
                status = 1;
                return
            end
        end

    end % methods
end % class


function y=fitpop(thetabit,bits,loss)
% J. C. Spall, August 1999
% This function returns a vector of N fitness values.
global p N thetamin thetamax
theta=zeros(p,1);
for i=1:N
   pointer=1;
   for j=1:p
      theta(j)=bit2num(thetabit(i,pointer:pointer+bits(j)-1),...
         bits(j),thetamin(j),thetamax(j));
      pointer=pointer+bits(j);
   end
   fitness(i)=-feval(loss,theta);
end
y=fitness;


function ch = i4_to_hex_digit ( i )

%*****************************************************************************80
%
%% I4_TO_HEX_DIGIT converts a (small) I4 to a hexadecimal digit.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I, the integer, between 0 and 15.
%
%    Output, character CH, the hexadecimal representation of the integer.
%
  if ( 0 <= i && i <= 9 )
    ch = '0' + i;
  elseif ( 10 <= i && i <= 15 )
    ch = 'A' + i - 10;
  else
    ch = '?';
  end

  return
end


function this = t_filter(this, cutoffSeconds)
% high-pass filters temporally (4th dimension of the image) as SPM
%
%   MrImage = t_filter(MrImage)
%
% This is a method of class MrImage.
%
% IN
%   cutoffSeconds   slower drifts than this will be filtered out
%
% OUT
%
% EXAMPLE
%   t_filter
%
%   See also MrImage spm_filter

% Author:   Saskia Klein & Lars Kasper
% Created:  2014-07-02
% Copyright (C) 2014 Institute for Biomedical Engineering
%                    University of Zurich and ETH Zurich
%
% This file is part of the TAPAS UniQC Toolbox, which is released
% under the terms of the GNU General Public Licence (GPL), version 3.
% You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version).
% For further details, see the file COPYING or
%  <http://www.gnu.org/licenses/>.

% convert to 2D
nVoxel = this.geometry.nVoxels;
Y = reshape(this.data, [], nVoxel(4))'; % Y = [nVolumes, nVoxel]
nVoxel3D = prod(nVoxel(1:3));

% create K for spm_filter and do it
K.RT = this.geometry.TR_s;
K.HParam = cutoffSeconds;
K.row = 1:nVoxel(4);

% spm_filter assumes Y = [nVolumes, nVoxel] dimensions
% K.row is specified to enable different filtering for different time
% frames e.g. sessions, to not filter drifts between session time gaps
Y = spm_filter(K, Y);

% back-conversion to 4D image
this.data = reshape(Y', nVoxel);


%% Generate best fit function
target_vy = 0.0;%[-0.8, -0.6, -0.4, -0.2, 0, 0.2, 0.4, 0.6, 0.8];
% target_vy = [0];
target_vx = [-0.4, -0.3, -0.2, -0.1, 0, 0.1, 0.2, 0.3, 0.4];
% target_vx = [0];
T = 0.4;
subfolder_name = 'library4';
if ~exist(fullfile('local', subfolder_name, 'periodic_fitting'), 'dir')
    mkdir(fullfile('local', subfolder_name, 'periodic_fitting'));
end
joint_names = {    'BasePosX'
    'BasePosY'
    'BasePosZ'
    'BaseRotX'
    'BaseRotY'
    'BaseRotZ'
    'qHRight'
    'qARight'
    'qBRight'
    'fourBarARight'
    'fourBarBRight'
    'qHLeft'
    'qALeft'
    'qBLeft'
    'fourBarALeft'
    'fourBarBLeft'};

N = 16;
N_vx = length(target_vx);
N_vy = length(target_vy);
t_all = cell(N_vx, N_vy);
q_all = cell(N_vx, N_vy);
dq_all = cell(N_vx, N_vy);
px_all = cell(N_vx, N_vy);
py_all = cell(N_vx, N_vy);
vx_all = cell(N_vx, N_vy);
vy_all = cell(N_vx, N_vy);
counter = 1;
for i = 1:N_vx
    vx = target_vx(i);
    for j = 1:N_vy
        vy = target_vy(j);
        data_name = fullfile('local', subfolder_name, sprintf('gait_X%0.1f_Y%.1f.mat', vx, vy));
        param = load(data_name);

        t_all{i,j} = [param.gait(1).tspan]./T;%, param.gait(3).tspan];
        q_all{i,j} = [param.gait(1).states.x];%, param. gait(3).states.x];
        dq_all{i,j} = [param.gait(1).states.dx];%, param. gait(3).states.dx];
        px_all{i,j} = [param.gait(1).states.x(1,:)];%ones(size(t_all{i,j}))*vx;
        py_all{i,j} = [param.gait(1).states.x(2,:)];%ones(size(t_all{i,j}))*vy;
        vx_all{i,j} = [param.gait(1).states.dx(1,:)];%ones(size(t_all{i,j}))*vx;
        vy_all{i,j} = [param.gait(1).states.dx(2,:)];%ones(size(t_all{i,j}))*vy;

    end
end

inpt = [horzcat(t_all{:})
%     horzcat(px_all{:})
%     horzcat(py_all{:})
    horzcat(vx_all{:})
%     horzcat(vx_all{:})
    ];
q_outpt = horzcat(q_all{:});
dq_outpt = horzcat(dq_all{:});
%%
for i = 1:N
    q_outpt_specific = q_outpt(i, :);
    dq_outpt_specific = dq_outpt(i, :);
    opt.neuralFitting(q_outpt_specific, inpt, 5, fullfile('local', subfolder_name, 'periodic_fitting', sprintf('sagittal_library_q%i', i)));
    opt.neuralFitting(dq_outpt_specific, inpt, 5, fullfile('local', subfolder_name, 'periodic_fitting', sprintf('sagittal_library_dq%i', i)));
end
addpath(fullfile('local', subfolder_name, 'periodic_fitting'));
%% Plot features
% joint angle
for n = 1:N
    f = figure(1000+n);
    f.Name = joint_names{n};
    set(f, 'WindowStyle', 'docked');

    ax = axes(f);
    hold(ax);

    scatter3(ax, inpt(1,:),inpt(2,:),q_outpt(n,:), 'r');

    [S, V] = meshgrid(0:0.01:1, -2:0.01:2);
    L = zeros(size(S));
    for i = 1:size(S, 1)
        for j = 1:size(S, 2)
            L(i, j) = feval(sprintf('sagittal_library_q%i', n), ([S(i, j); V(i, j)]));
            %L(i, j) = l(n);
        end
    end

    surface(ax, S, V, L);
end
%% joint velocity
for n = 1:N
    f = figure(2000+n);
    f.Name = joint_names{n};
    set(f, 'WindowStyle', 'docked');

    ax = axes(f);
    hold(ax);

    scatter3(ax, inpt(1,:),inpt(2,:),dq_outpt(n,:), 'r');

    [S, V] = meshgrid(0:0.01:1, -2:0.01:2);
    L = zeros(size(S));
    for i = 1:size(S, 1)
        for j = 1:size(S, 2)
            L(i, j) = feval(sprintf('sagittal_library_dq%i', n), ([S(i, j); V(i, j)]));
        end
    end

    surface(ax, S, V, L);
end


%%%
%
% Gather src hidden vectors for attention-based models.
% For each sentence ii, we extract a set of vectors to pay attention to
%   srcVecsAll(:, ii, srcPositions(ii)-posWin:srcPositions(ii)+posWin)
%   and put it into srcVecs(:, ii, :). Boundary cases are handled as well.
%
% Thang Luong @ 2015, <lmthang@stanford.edu>
%
%%%

% TODO move srcMaxLen out
% IMPORTANT: we assume the sentences are reversed here
% srcPositions = srcMaxLen - srcPositions
function [srcVecsSub, h2sInfo] = buildSrcVecs(srcVecsAll, srcPositions, curMask, srcLens, srcMaxLen, params, h2sInfo)
  posWin = params.posWin;
  numPositions = 2*posWin+1;
  [lstmSize, batchSize, numSrcHidVecs] = size(srcVecsAll);

  % masking
  srcPositions(curMask.maskedIds) = [];
  srcLens(curMask.maskedIds) = [];
  unmaskedIds = curMask.unmaskedIds;

  % flatten matrices of size batchSize*numPositions (not exactly batch size but close)
  % init. IMPORTANT: don't swap these two lines
  % assume unmaskedIds = [1, 2, 3, 4, 5], numPositions=3
  indicesSub = reshape(repmat(1:numPositions, length(unmaskedIds), 1), 1, []); % 1 1 1 1 1 2 2 2 2 2 3 3 3 3 3
  unmaskedIds = repmat(unmaskedIds, 1, numPositions); % 1 2 3 4 5 1 2 3 4 5 1 2 3 4 5

  % Note: generate multiple sequences of the same lengths without using for loop, see this post for many elegant solutions
  % http://www.mathworks.com/matlabcentral/answers/217205-fast-ways-to-generate-multiple-sequences-without-using-for-loop
  % The below version is the only solution that is faster than for loop (3 times).
  % If startAttnIds = [ 2 4 6 8 10 ] and numPositions=3
  % then indicesAll = [ 2 4 6 8 10 3 5 7 9 11 4 6 8 10 12 ].
  srcLens = repmat(srcLens, [1, numPositions]);
  startAttnIds = srcPositions-posWin;
  indicesAll = reshape(bsxfun(@plus, startAttnIds(:), 0:(numPositions-1)), 1, []);

  % check those that are out of boundaries
  excludeIds = find(indicesAll>numSrcHidVecs | indicesAll<(srcMaxLen-srcLens+1));
  if ~isempty(excludeIds)
    indicesAll(excludeIds) = []; unmaskedIds(excludeIds) = []; indicesSub(excludeIds) = [];
  end

  h2sInfo.indicesAll = indicesAll;
  h2sInfo.unmaskedIds = unmaskedIds;

  if ~isempty(unmaskedIds)
    srcVecsSub = zeroMatrix([lstmSize, batchSize*numPositions], params.isGPU, params.dataType);

    % create linear indices
    h2sInfo.linearIdSub = sub2ind([batchSize, numPositions], unmaskedIds, indicesSub);
    h2sInfo.linearIdAll = sub2ind([batchSize, numSrcHidVecs], unmaskedIds, indicesAll);

    % create srcVecs
    srcVecsAll = reshape(srcVecsAll, lstmSize, []);
    srcVecsSub(:, h2sInfo.linearIdSub) = srcVecsAll(:, h2sInfo.linearIdAll);
    srcVecsSub = reshape(srcVecsSub, [lstmSize, batchSize, numPositions]);
  else
    srcVecsSub = zeroMatrix([lstmSize, batchSize, numPositions], params.isGPU, params.dataType);
    h2sInfo.linearIdSub = [];
    h2sInfo.linearIdAll = [];
  end

  h2sInfo.alignMask = zeroMatrix([batchSize, numPositions], params.isGPU, params.dataType);
  h2sInfo.alignMask(h2sInfo.linearIdSub) = 1;
  h2sInfo.alignMask = h2sInfo.alignMask'; % numPositions * batchSize
  h2sInfo.srcPositions = srcPositions;
end


function []=plot_dramapb(vResults)
% --------------------------------------------
% Plot db map on topography
%
% Incoming variables:
% vResults: Necessarily includes vResults.mPolygon, vResults.mValueGrid, vResults.fSpacingHorizontal
% tmap    : Topographic map data ETOPO5
% tmapleg : Topographic map data ETOPO5 legend
%
% Author: J. Woessner, woessner@seismo.ifg.ethz.ch
% last update: 27.02.03

% Get tmap from tbase.bin
[tmap,tmapleg] = tbase(3);

% Construct a map graticule mesh for surface object display
[lat,lon] = meshgrat(tmap,tmapleg);

% Get coordinates from calculated Mc values, set up matrices
vX=min(vResults.mPolygon(:,1)):vResults.fSpacingHorizontal:max(vResults.mPolygon(:,1));
vY=min(vResults.mPolygon(:,2)):vResults.fSpacingHorizontal:max(vResults.mPolygon(:,2));
[X , Y]  = meshgrid(vX,vY);

% % Set values of dMc to NaN
vSel = find(vResults.mValueGrid(:,10)> 0.5);
vResults.mValueGrid(vSel,10) = NaN;

% Reshape
mMcValues=reshape(vResults.mValueGrid(:,10),length(vY),length(vX));
% Interpolate Values
mMcGrid = interp2(X,Y,mMcValues,lon,lat);

% Create figure
exfig=figure_exists('db Worldmap',0);
if exfig==0
    ui=figure_w_normalized_uicontrolunits( 'Name','db Worldmap',...
        'NumberTitle','off');
    figure_w_normalized_uicontrolunits(ui)
end

clf
hold on;
axis off;
axesm('MapProjection','robinson')

% Magic coloring
mi = min(min(mMcGrid));
l =  isnan(mMcGrid);
mMcGrid(l) = mi-10;
ll = tmap < 0 & mMcGrid < 0;
mMcGrid(ll) = mMcGrid(ll)*0 + 10;
hMap=meshm(mMcGrid,tmapleg,size(tmap),tmap);

daspectm('m',20);
tightmap
view([0 90])
camlight; lighting phong
set(gca,'projection','perspective');

%j = jet;
j = hot(400000);
j = j(500:350000,:);
j = flipud(j);
j = [ [ 0.9 0.9 0.9 ] ; j; [ 0.5 0.5 0.5] ];

%caxis([ min(min(mMcValues)) max(max(mMcValues))]);
caxis([0 0.5])
colormap(j); brighten(0.1);

axis off; set(gcf,'color','k')

setm(gca,'ffacecolor','k')
setm(gca,'fedgecolor','w','flinewidth',1);

setm(gca,'mlabellocation',60)
setm(gca,'meridianlabel','on')
setm(gca,'plabellocation',25)
setm(gca,'parallellabel','on')
setm(gca,'Fontcolor','w','FontSize',10,'Fontweight','bold','Fontname','times','Labelunits','degrees')
setm(gca,'Labelformat','none')
setm(gca,'Grid','on')
setm(gca,'GLineStyle','-')

h5 = colorbar;
set(h5,'position',[0.82 0.35 0.01 0.3],'TickDir','out','Ycolor','w','Xcolor','w',...
    'Fontweight','bold','FontSize',10,'YTick',[0 0.1 0.2 0.3 0.4 0.5 0.6]);
set(gcf,'Inverthardcopy','off');


function [u, Ps, ue] = spm_uc_clusterFDR(q,df,STAT,R,n,Z,XYZ,V2R,ui,G)
% Cluster False Discovery critical extent threshold
% FORMAT [u, Ps, ue] = spm_uc_clusterFDR(q,df,STAT,R,n,Z,XYZ,ui[,G])
%
% q     - prespecified upper bound on False Discovery Rate
% df    - [df{interest} df{residuals}]
% STAT  - statistical field
%         'Z' - Gaussian field
%         'T' - T - field
%         'X' - Chi squared field
%         'F' - F - field
% R     - RESEL Count {defining search volume}
% n     - conjunction number
% Z     - height {minimum over n values}
%         or mapped statistic image(s)
% XYZ   - locations [x y x]' {in voxels}
%         or vector of indices of elements within mask
%         or mapped mask image
% V2R   - voxel to resel
% ui    - feature-inducing threshold
% G     - patch structure (for surface-based inference)
%
% u     - critical extent threshold
% Ps    - sorted p-values
% ue    - critical extent threshold for FWE
%__________________________________________________________________________
%
% References
%
% J.R. Chumbley and K.J. Friston, "False discovery rate revisited: FDR and
% topological inference using Gaussian random fields". NeuroImage,
% 44(1):62-70, 2009.
%
% J.R. Chumbley, K.J. Worsley, G. Flandin and K.J. Friston, "Topological
% FDR for NeuroImaging". NeuroImage, 49(4):3057-3064, 2010.
%__________________________________________________________________________
% Copyright (C) 2009-2013 Wellcome Trust Centre for Neuroimaging

% Justin Chumbley & Guillaume Flandin
% $Id: spm_uc_clusterFDR.m 5809 2013-12-20 14:30:22Z guillaume $

% Read statistical value from disk if needed
%--------------------------------------------------------------------------
if isstruct(Z)
    Vs         = Z;
    Vm         = XYZ;
    [Z, XYZmm] = spm_read_vols(Vs(1),true);
    for i=2:numel(Vs)
        Z      = min(Z, spm_read_vols(Vs(i),true));
    end
    Z          = Z(:)';
    XYZ        = round(Vs(1).mat \ [XYZmm; ones(1, size(XYZmm, 2))]);
    try
        M      = spm_vol(spm_file(Vs(1).fname,'basename','mask'));
        I      = spm_get_data(M,XYZ,false) > 0;
    catch
        I      = ~isnan(Z) & Z~=0;
    end
    XYZ        = XYZ(1:3,I);
    Z          = Z(I);
    if isstruct(Vm)
        Vm     = spm_get_data(Vm,XYZ,false) > 0;
    end
    XYZ        = XYZ(:,Vm);
    Z          = Z(:,Vm);
end

% Threshold the statistical field
%--------------------------------------------------------------------------
XYZ      = XYZ(:,Z >= ui);

% Extract size of excursion sets
%--------------------------------------------------------------------------
if nargin == 9
    N    = spm_clusters(XYZ);
else
    tmp  = false(size(G.vertices,1),1);
    tmp(XYZ(1,:)) = true;
    N    = spm_mesh_clusters(G,tmp);
    N    = N(XYZ(1,:));
end
if ~isempty(N)
    N    = histc(N,(0:max(N))+0.5);
end
N        = N(1:end-1);
N        = N .* V2R;

% Compute uncorrected p-values based on N using Random Field Theory
%--------------------------------------------------------------------------
Ps       = zeros(1,numel(N));
Pk       = zeros(1,numel(N));
ws       = warning('off','SPM:outOfRangePoisson');
for i = 1:length(N)
    [Pk(i), Ps(i)] = spm_P_RF(1,N(i),ui,df,STAT,R,n);
end
warning(ws);
[Ps, J]  = sort(Ps, 'ascend');

S        = length(Ps);

% Calculate FDR inequality RHS
%--------------------------------------------------------------------------
cV       = 1;    % Benjamini & Yeuketeli cV for independence/PosRegDep case
Fi       = (1:S)/S*q/cV;

% Find threshold
%--------------------------------------------------------------------------
I        = find(Ps <= Fi, 1, 'last');
if isempty(I)
    u    = Inf;
else
    u    = N(J(I)) / V2R;
end

% As we're there, also determine the FWE critical extent threshold
%--------------------------------------------------------------------------
if nargout == 3
    [Pk, J] = sort(Pk, 'ascend');
    I       = find(Pk <= q);
    if isempty(I)
        ue  = Inf;
    else
        ue  = min(N(J(I))) / V2R;
    end
end


load -ascii soybeanD
load -ascii soybeanTD

% soybeanD(2:36,:) = soybeanD(2:36,:)+1;
% soybeanTD(2:36,:) = soybeanTD(2:36,:)+1;
% soybeanD(find(soybeanD==-9998))=-9999;
% soybeanTD(find(soybeanTD==-9998))=-9999;

[N, Napp] = size(soybeanD);
[N, Ntest] = size(soybeanTD);

N, m=Napp+Ntest, Napp, Ntest,
class = 1

%rand('state',0); randn('state',0);
%abalone = abalone(:,randperm(m));

app  = soybeanD;
test = soybeanTD;

unique(app(class,:))
unique(test(class,:))

ns = max(soybeanD')
clear soybeanD soybeanTD

% N, ns(class), Napp, Ntest, mean(ns),


function test0076 ( )

%*****************************************************************************80
%
%% TEST0076 tests JED_TO_RD and RD_TO_JED.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 February 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST0076\n' );
  fprintf ( 1, '  For the RD:\n' );
  fprintf ( 1, '  JED_TO_RD: JED -> RD.\n' );
  fprintf ( 1, '  RD_TO_JED: RD -> JED.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  JED (in)    RD                JED (out)\n' );
  fprintf ( 1, '\n' );

  i = 0;

  while ( 1 )

    i = i + 1;
    jed1 = jed_test ( i );

    if ( jed1 < 0.0 )
      break
    end

    rd2 = jed_to_rd ( jed1 );

    jed3 = rd_to_jed ( rd2 );

    fprintf ( 1, '  %11.2f     %11.2f     %11.2f\n', jed1, rd2, jed3 );

  end

  return
end


function y = R3RSR(x,batch)
% R3RSR will R3R a vector x follow by a repeated splitting
%    (repeated SR) of the results from R3R.
%
% Usage: y = R3RSR(x)
%
%   where  x is the input vector with length at least 3
%          y is the return vector of the same length as x
%

% See John Wilder Tukey, "EXPLORATORY DATA ANALYSIS".
% Addison-Wesley Publishing Co. 1977. Chpt 7A, 7D

if min(size(x)) ~= 1, error('Input must be a vector'); end
x = x(:); xlen = length(x); cont = 1; y = x;
if nargin<2, batch = 0; end
if xlen < 3,
   if ~batch, warning('Input vector must be at least 3 elements long'); end
   cont = 0;
end
xtemp1 = R3R(x,batch);
xtemp2 = SR(xtemp1,batch);
while any(xtemp2-xtemp1),
   xtemp1 = xtemp2;
   xtemp2 = SR(xtemp1,batch);
end

return


function image_rgb(M)
% Show a matrix of integers as a color image.
% This is like imagesc, except we know what the mapping is from integer to color.
% If entries of M contain integers in {1,2,3}, we map
% this to red/green/blue

cmap = [1 0 0; % red
	0 1 0; % green
	0 0 1; % blue
	127/255 1 212/255]; % aquamarine
image(M)
set(gcf,'colormap', cmap);

if 1
  % make dummy handles, one per object type, for the legend
  str = {};
  for i=1:size(cmap,1)
    dummy_handle(i) = line([0 0.1], [0 0.1]);
    set(dummy_handle(i), 'color', cmap(i,:));
    set(dummy_handle(i), 'linewidth', 2);
    str{i} = num2str(i);
  end
  legend(dummy_handle, str, -1);
end

if 0
[nrows ncols] = size(M);
img = zeros(nrows, ncols, 3);
for r=1:nrows
  for c=1:ncols
    q = M(r,c);
    img(r,c,q) = 1;
  end
end
image(img)
end


function metabModel = extractMetModel(model, metabNames, nLayers, allCompFlag, nRxnsMetThr)
% Creates a subnetwork model around one or more metabolites
%
% USAGE:
%
%    metabModel = extractMetModel(model, metabNames, nLayers, allCompFlag, nRxnsMetThr)
%
% INPUTS:
%    model:          COBRA model structure
%    metabNames:     Metabolites to build subnetwork model around
%    nLayers:        Number of layers
%    allCompFlag:    Use all metabolites regardless of compartment
%
% OPTIONAL INPUT:
%    nRxnsMetThr:    Ignore metabolites which appear in more than `nRxnMetThr` (Default = 100)
%
% OUTPUT:
%    metabModel:     COBRA model around one or more metabolites
%
% .. Author: - Markus Herrgard 3/1/06
%              Uri David Akavia 26-Mar-2017

if (nargin < 5)
    nRxnsMetThr = 100;
end

% Filter out high degree metabolites
nRxnsMet = full(sum(model.S~=0,2));
baseMetNames = parseMetNames(model.mets);
for i = 1:length(baseMetNames)
    nRxnsMetComp(i) = sum(nRxnsMet(strcmp(baseMetNames,baseMetNames{i})));
end
nRxnsMetComp = nRxnsMetComp';
selLowDegMet = nRxnsMetComp < nRxnsMetThr;
model.S(~selLowDegMet,:) = 0;
%model.mets = model.mets(selLowDegMet); Commented out, because other fields
%aren't shrunk, which can lead to mismatch later

if (~iscell(metabNames))
    tmpMetName = metabNames;
    clear metabNames;
    metabNames{1} = tmpMetName;
end

if (allCompFlag)
    allMetNames = parseMetNames(model.mets);
    metabNames = parseMetNames(metabNames);
else
    allMetNames = model.mets;
end

metabNames = intersect(metabNames, allMetNames(selLowDegMet));
selMets = find(ismember(allMetNames,metabNames));

metS = model.S(selMets,:);
[nMet,tmp] = size(metS);
if (nMet > 1)
    selRxns = any(full(metS) ~= 0)';
elseif (~nMet)
	selRxns = false(size(model.rxns));
else
    selRxns = (full(metS) ~= 0)';
end
for i = 1:nLayers+1
    metS = model.S(selMets,:);
    [nMet,tmp] = size(metS);
    if (nMet > 1)
        selRxns = any(full(metS) ~= 0)';
    elseif (~nMet)
        selRxns = false(size(model.rxns));
	else
        selRxns = (full(metS) ~= 0)';
    end

    if (isfield(model,'c'))
        selRxns = selRxns & ~ (model.c == 1);
    end
    nextLayerMets = find(any(model.S(:,selRxns) ~= 0,2));
    selMets = union(selMets,nextLayerMets);
end

rxnNames = model.rxns(selRxns);
metNames = model.mets(selMets);

metabModel = extractSubNetwork(model,rxnNames,metNames);


function [positions, time] = tracker_affine(video_path, img_files, pos, target_sz, ...
	padding, kernel, lambda, output_sigma_factor, interp_factor, cell_size, ...
	features, show_visualization)
%TRACKER Kernelized/Dual Correlation Filter (KCF/DCF) tracking.
%   This function implements the pipeline for tracking with the KCF (by
%   choosing a non-linear kernel) and DCF (by choosing a linear kernel).
%
%   It is meant to be called by the interface function RUN_TRACKER, which
%   sets up the parameters and loads the video information.
%
%   Parameters:
%     VIDEO_PATH is the location of the image files (must end with a slash
%      '/' or '\').
%     IMG_FILES is a cell array of image file names.
%     POS and TARGET_SZ are the initial position and size of the target
%      (both in format [rows, columns]).
%     PADDING is the additional tracked region, for context, relative to
%      the target size.
%     KERNEL is a struct describing the kernel. The field TYPE must be one
%      of 'gaussian', 'polynomial' or 'linear'. The optional fields SIGMA,
%      POLY_A and POLY_B are the parameters for the Gaussian and Polynomial
%      kernels.
%     OUTPUT_SIGMA_FACTOR is the spatial bandwidth of the regression
%      target, relative to the target size.
%     INTERP_FACTOR is the adaptation rate of the tracker.
%     CELL_SIZE is the number of pixels per cell (must be 1 if using raw
%      pixels).
%     FEATURES is a struct describing the used features (see GET_FEATURES).
%     SHOW_VISUALIZATION will show an interactive video if set to true.
%
%   Outputs:
%    POSITIONS is an Nx2 matrix of target positions over time (in the
%     format [rows, columns]).
%    TIME is the tracker execution time, without video loading/rendering.
%
%   Joao F. Henriques, 2014

	%if the target is large, lower the resolution, we don't need that much
	%detail
	resize_image = (sqrt(prod(target_sz)) >= 100);  %diagonal size >= threshold
	if resize_image,
		pos = floor(pos / 2);
		target_sz = floor(target_sz / 2);
	end

	%window size, taking padding into account
	window_sz = floor(target_sz * (1 + padding));

% 	%we could choose a size that is a power of two, for better FFT
% 	%performance. in practice it is slower, due to the larger window size.
% 	window_sz = 2 .^ nextpow2(window_sz);

	%create regression labels, gaussian shaped, with a bandwidth
	%proportional to target size
	output_sigma = sqrt(prod(target_sz)) * output_sigma_factor / cell_size;
	yf = fft2(gaussian_shaped_labels(output_sigma, floor(window_sz / cell_size)));

	%store pre-computed cosine window
	cos_window = hann(size(yf,1)) * hann(size(yf,2))';

	if show_visualization,  %create video interface
		update_visualization = show_video(img_files, video_path, resize_image);
	end

	%note: variables ending with 'f' are in the Fourier domain.

	time = 0;  %to calculate FPS
	positions = zeros(numel(img_files), 2);  %to calculate precision

	for frame = 1:numel(img_files),
		%load image
		im = imread([video_path img_files{frame}]);
		if size(im,3) > 1,
			im = rgb2gray(im);
		end
		if resize_image,
			im = imresize(im, 0.5);
		end

		tic()

		if frame == 1
			camera_feat.detector = cv.FeatureDetector('SURF');
			camera_feat.extractor = cv.DescriptorExtractor('SURF');
			camera_feat.matcher = cv.DescriptorMatcher('FlannBased');
			camera_feat.last_keypoints = camera_feat.detector.detect(im);
			camera_feat.last_descriptors = camera_feat.extractor.compute(im, camera_feat.last_keypoints);
		end

		if frame > 1,
			[camera_feat, camera_H] = find_affine(camera_feat, im, pos, target_sz);
			pos = project_t(pos([2, 1]), camera_H);
            pos = pos([2, 1]);
            pos = min([size(im,1),size(im,2)], pos);
            pos = max([0,0], pos);
			%obtain a subwindow for detection at the position from last
			%frame, and convert to Fourier domain (its size is unchanged)
			patch = get_subwindow(im, pos, window_sz);
			zf = fft2(get_features(patch, features, cell_size, cos_window));

			%calculate response of the classifier at all shifts
			switch kernel.type
			case 'gaussian',
				kzf = gaussian_correlation(zf, model_xf, kernel.sigma);
			case 'polynomial',
				kzf = polynomial_correlation(zf, model_xf, kernel.poly_a, kernel.poly_b);
			case 'linear',
				kzf = linear_correlation(zf, model_xf);
			end
			response = real(ifft2(model_alphaf .* kzf));  %equation for fast detection

			%target location is at the maximum response. we must take into
			%account the fact that, if the target doesn't move, the peak
			%will appear at the top-left corner, not at the center (this is
			%discussed in the paper). the responses wrap around cyclically.
			[vert_delta, horiz_delta] = find(response == max(response(:)), 1);
			if vert_delta > size(zf,1) / 2,  %wrap around to negative half-space of vertical axis
				vert_delta = vert_delta - size(zf,1);
			end
			if horiz_delta > size(zf,2) / 2,  %same for horizontal axis
				horiz_delta = horiz_delta - size(zf,2);
			end
			pos = pos + cell_size * [vert_delta - 1, horiz_delta - 1];
		end

		%obtain a subwindow for training at newly estimated target position
		patch = get_subwindow(im, pos, window_sz);
		xf = fft2(get_features(patch, features, cell_size, cos_window));

		%Kernel Ridge Regression, calculate alphas (in Fourier domain)
		switch kernel.type
		case 'gaussian',
			kf = gaussian_correlation(xf, xf, kernel.sigma);
		case 'polynomial',
			kf = polynomial_correlation(xf, xf, kernel.poly_a, kernel.poly_b);
		case 'linear',
			kf = linear_correlation(xf, xf);
		end
		alphaf = yf ./ (kf + lambda);   %equation for fast training

		if frame == 1,  %first frame, train with a single image
			model_alphaf = alphaf;
			model_xf = xf;
		else
			%subsequent frames, interpolate model
			model_alphaf = (1 - interp_factor) * model_alphaf + interp_factor * alphaf;
			model_xf = (1 - interp_factor) * model_xf + interp_factor * xf;
		end

		%save position and timing
		positions(frame,:) = pos;
		time = time + toc();

		%visualization
		if show_visualization,
			box = [pos([2,1]) - target_sz([2,1])/2, target_sz([2,1])];
			stop = update_visualization(frame, box);
			if stop, break, end  %user pressed Esc, stop early

			drawnow
% 			pause(0.05)  %uncomment to run slower
		end

	end

	if resize_image,
		positions = positions * 2;
	end
end


function [ dat ] = visual_spectrum( data , varargin )
% Description:
%   It is the feature plots of data. This function can be useful to see an
%   overview of data according to time, frequency, and channel.
%
% Example Code:
%    visuspect = visual_spectrum(SMT , {'Xaxis' , 'Frequency'; 'Yaxis' , 'Channel'});
%    visuspect = visual_spectrum(SMT , {'Xaxis' , 'Time'; 'Yaxis' , 'Channel'; 'Band' ,[8 10]});
%    visuspect = visual_spectrum(SMT , {'Xaxis' , 'Time'; 'Yaxis' , 'Frequency'; 'Channel' ,{'C4'}});
%
% Input:
%   [dat] = visual_spectrum(data , <OPT>)
%   data: Data structrue (ex) Epoched data structure
%
% Option:
%      .Xaxis - selecting the domain what you interested in x axis
%                 (e.g. {'Xaxis' , 'Frequency'},{'Xaxis' , 'time'})
%      .Yaxis - selecting the domain what you interested in y axis
%                 (e.g. {'Yaxis' , 'Channel'},{'Yaxis' , 'Frequency'})
%      .Band -  Selecting the interested frequency band in Time-Channel
%               domian
%                 (e.g. {'Band', [8 10]})
%      .Channel - Selecting the interested channel in Time-Frequency domain
%                 (e.g. {'Channel', {'C4'}})
%
% Return:
%    data:  Epoched data structure
%
% See also:
%    opt_cellToStruct
%
% Reference:
%           G. Schalk, D.J. McFarland, T. Hinterberger, N. Birbaumer, and
%           J. R. Wolpaw,"BCI2000: A General-Purpose Brain-Computer
%           Interface (BCI) System, IEEE Transactions on Biomedical
%           Engineering, Vol. 51, No. 6, 2004, pp.1034-1043.
%
% Ji Hoon, Jeong
% jh_jeong@korea.ac.kr
%

%% Load the data
dat = data;
opt = opt_cellToStruct(varargin{:});
sampleFrequency=dat.fs;

%% Setting the parameter
% We refer to visualization matlab code in BCI 2000 open source toolbox
%
% modelOrder = 18+round(sampleFrequency/100);
modelOrder = 20;
opt.lowPassCutoff=(sampleFrequency/2);
opt.freqBinWidth= 2;
opt.highPassCutoff=-1;
opt.trend=1;
% Setting the parameter about spectral moving (using autoregression)
opt.spectralSize= 500;
opt.spectralStep = 100;
opt.spectralSize = round(opt.spectralSize/1000 * sampleFrequency);
opt.spectralStep = round(opt.spectralStep/1000 * sampleFrequency);
params = [modelOrder, opt.highPassCutoff+opt.freqBinWidth/2, opt.lowPassCutoff, opt.freqBinWidth,round(opt.freqBinWidth/.2), ...
    opt.trend, sampleFrequency];
params(8) = opt.spectralStep;
params(9) = opt.spectralSize/opt.spectralStep;
%%
C1_idx=find(dat.y_dec==1);
C2_idx = find(dat.y_dec==2);
C1=prep_selectTrials(dat,{'Index', C1_idx});
C2=prep_selectTrials(dat,{'Index',C2_idx});
for trial=1:size(C1.x,2)
    plotData1=C1.x(:,trial,:);
    plotData1=squeeze(plotData1);
    %trialspectrum = [frequency by channel by time by trial]
    [trialspectrum(:,:,:,trial), C1_freqBins] = mem( plotData1, params );
    plotData2=C2.x(:,trial,:);
    plotData2=squeeze(plotData2);
    [trialspectrum2(:,:,:,trial), C2_freqBins] = mem( plotData2, params );
end
%%
if strcmp(opt.Xaxis, 'Frequency') == 1 && strcmp(opt.Yaxis, 'Channel') == 1
    for tri = 1: size(C1_idx,2)
        tmp=mean(trialspectrum(:,:,:,tri),3);
        C1Data(:,:,tri)=tmp;
        tmp2=mean(trialspectrum2(:,:,:,tri),3);
        C2Data(:,:,tri)=tmp2;
    end
    C1Data = double(C1Data); C2Data = double(C2Data);
    for ch=1:size(C1Data, 2)
        for samp=1:size(C1Data, 1)
            ressq(samp, ch)=rsqu(C1Data(samp, ch, :), C2Data(samp, ch, :));
        end
    end
    plotData = {mean(C1Data,3),mean(C2Data,3), ressq};
    freqBins = C1_freqBins - opt.freqBinWidth/2;
    freqBins(end+1) = freqBins(end) + diff(freqBins(end-1:end));
    for i = 1:size(plotData,2)
        plotData{i}=cat(2, plotData{i}, zeros(size(plotData{i}, 1), 1));
        plotData{i}=cat(1, plotData{i}, zeros(1, size(plotData{i}, 2)));
        figure(i);
        surf(freqBins, [1:size(plotData{i}, 2)], plotData{i}(1:end,:)'); hold on;
        axis tight; set(gcf,'Renderer','Zbuffer');
        xlim([freqBins(2) freqBins(end)]);
        set(gca,'XTick',[freqBins(2):2: freqBins(end)]);
        set(gca,'YTick',[1:1: size(C1Data,2)]);
        set(gca,'Yticklabel',char(dat.chan));
        view(2);colormap jet;colorbar;
        xlabel('Frequency [Hz]'); ylabel('Channels');
        switch(i)
            case 1
                caxis([0 300]);
                title('Frequency-channel power spectrum per channel in Class 1');
            case 2
                caxis([0 300]);
                title('Frequency-channel power spectrum per channel in Class 2');
            case 3
                title('Frequency-channel power spectrum per channels between classes using r^2 values');
        end
    end
%%
elseif strcmp(opt.Xaxis, 'Time') == 1 && strcmp(opt.Yaxis, 'Channel') == 1
    fre_inx = find(C1_freqBins == opt.Band(2));
    for tri = 1: size(C1_idx,2)
        tmp = trialspectrum(fre_inx,:,:,tri);
        tmp = squeeze(tmp);
        C1Data(:,:,tri)=tmp;
        tmp2 = trialspectrum2(fre_inx,:,:,tri);
        tmp2 = squeeze(tmp2);
        C2Data(:,:,tri)=tmp2;
    end
    res1Data = double(permute(C1Data,[2 1 3]));
    res2Data = double(permute(C2Data,[2 1 3]));
    for ch=1:size(res1Data, 2)
        for samp=1:size(res1Data, 1)
            ressq(samp, ch)=rsqu(res1Data(samp, ch, :), res2Data(samp, ch, :));
        end
    end
    plotData = {mean(C1Data,3),mean(C2Data,3) , ressq'};
    a = dat.ival(1)/100;b = size(tmp,2)-abs(dat.ival(1)/100)-1;
    a = a*100;b = b*100;
    for i = 1:size(plotData,2)
        figure(i);
        surf([a:100:b], [1:size(plotData{i}, 1)], plotData{i}(1:end,:)); hold on;
        axis tight; set(gcf,'Renderer','Zbuffer');
        set(gca,'XTick',[a:500:b]);
        set(gca,'YTick',[1:1: size(C1Data,2)]);
        set(gca,'Yticklabel',char(dat.chan));
        view(2);colormap jet;colorbar;
        xlabel('Time'); ylabel('Channels');
        switch(i)
            case 1
                caxis([0 1000]);
                title('Time-channel power spectrum per channel in Class 1');
            case 2
                caxis([0 1000]);
                title('Time-channel power spectrum per channel in Class 2');
            case 3
                title('Time-channel power spectrum per channels between classes using r^2 values');
        end
    end
%%
elseif strcmp(opt.Xaxis, 'Time') == 1 && strcmp(opt.Yaxis, 'Frequency') == 1
    Chanidx = find(strcmp(dat.chan, opt.Channel)== 1 );
    for tri = 1: size(C1_idx,2)
        tmp = trialspectrum(:,Chanidx,:,tri);
        tmp = squeeze(tmp);
        C1Data(:,:,tri)=tmp;
        tmp2 = trialspectrum2(:,Chanidx,:,tri);
        tmp2 = squeeze(tmp2);
        C2Data(:,:,tri)=tmp2;
    end
    plotData = {mean(C1Data,3),mean(C2Data,3)};
    a = dat.ival(1)/100;b = size(tmp,2)-abs(dat.ival(1)/100)-1;
    a = a*100;b = b*100;
    freqBins = C1_freqBins - opt.freqBinWidth/2;
    freqBins(end+1) = freqBins(end) + diff(freqBins(end-1:end));
    for i = 1:size(plotData,2)
        plotData{i}=cat(1, plotData{i}, zeros(1, size(plotData{i}, 2)));
        figure(i);
        surf([a:100:b]', freqBins', plotData{i}(1:end,:)); hold on;
        axis tight; set(gcf,'Renderer','Zbuffer');
        set(gca,'XTick',[a:500:b]);
        ylim([freqBins(2) freqBins(end)]);
        set(gca,'YTick',[freqBins(2):2: freqBins(end)]);
        view(2);colormap jet;colorbar;caxis([0 1000]);
        xlabel('Time'); ylabel('Frequency');
        switch(i)
            case 1
                title(['Time-frequency power spectrum in channel ', char(opt.Channel),  ' and class 1']);
            case 2
                title(['Time-frequency power spectrum in channel ', char(opt.Channel),  ' and class 2']);
        end
    end
end
end


function [Jul1,Jul2]=TT2TCB(Jul1,Jul2,deltaT,clockLoc)
%%TT2TCB Convert from terrestrial time (TT) to barycentric coordinate
%        time (TCB) to an accuracy of nanoseconds (if deltaT is accurate)
%        using the routines from the International Astronomical Union's
%        library that do not require external ephemeris data.
%
%INPUTS: Jul1,Jul2 Two parts of a Julian date given in TT. The units of
%                  the date are days. The full date is the sum of both
%                  terms. The date is broken into two parts to provide
%                  more bits of precision. It does not matter how the date
%                  is split.
%           deltaT An optional parameter specifying the offset between TT
%                  and UT1 in seconds. If this parameter is omitted or an
%                  empty matrix is passed, then the value of the function
%                  getEOP will be used.
%         clockLoc An optional 3X1 vector specifying the location of the
%                  clock in WGS-84 ECEF Cartesian [x;y;z] coordinates with
%                  units of meters. Due to relativistic effects, clocks
%                  that are synchronized with respect to TT are not
%                  synchronized with respect to TCB. If this parameter is
%                  omitted, then a clock at the center of the Earth is
%                  used and the precision declines to microseconds.
%
%OUTPUTS:Jul1,Jul2 Two parts of a Julian date given in TCB.
%
%This function relies on a number of functions in the International
%Astronomical Union's Standards of Fundamental Astronomy library. The time
%is first converted to barycentric dynamical time (TDB) and then to
%barycentric coordinated time (TCB).
%
%The algorithm can be compiled for use in Matlab  using the
%CompileCLibraries function.
%
%The algorithm is run in Matlab using the command format
%[Jul1,Jul2]=TT2TCB(Jul1,Jul2);
%or
%[Jul1,Jul2]=TT2TCB(Jul1,Jul2,deltaT);
%or
%[Jul1,Jul2]=TT2TCB(Jul1,Jul2,deltaT,clockLoc);
%
%Many temporal coordinate systems standards are compared in [1].
%
%REFERENCES:
%[1] D. F. Crouse, "An Overview of Major Terrestrial, Celestial, and
%    Temporal Coordinate Systems for Target Tracking," Formal Report,
%    Naval Research Laboratory, no. NRL/FR/5344--16-10,279, 10 Aug. 2016,
%    173 pages.
%
%March 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

error('This function is only implemented as a mexed C or C++ function. Please run CompileCLibraries.m to compile the function for use.')

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [ know, x ] = p06_sol ( n )

%*****************************************************************************80
%
%% P06_SOL returns the solution for problem 6.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the problem.  This value
%    is only needed for those problems with variable N.
%
%    Output, integer KNOW.
%    If KNOW is 0, then the solution is not known.
%    If KNOW is positive, then the solution is known, and is returned in X.
%
%    Output, real X(N), the solution, if known.
%
  know = 1;

  x(1:n) = 1.0;

  return
end


function [neighbors nns] = sameclass_nn(data, nn, W)

if (exist('W', 'var'))
  if (~iscell(W))
    B1 = W * data.Xtraining;
  else
    resp1 = compute_NN_output3(W, data.Xtraining);
    B1 = resp1{end};
  end

  B1 = logical(single(B1 > 0));
  B1 = compactbit(B1);
  [nw1 ~] = size(B1);
  nb = nw1*8;
end

nl = zeros(numel(data.labels(:)), 1);
for l = data.labels(:)'
  nl(l+1) = sum(data.Ltraining == l);
end

nns = zeros(data.Ntraining,1);
neighbors = zeros(min(nn, min(nl)-1), data.Ntraining, 'single');

for l = data.labels(:)'
  which_l = data.Ltraining == l;
  id_l = find(which_l);
  nn_l = min(nn, nl(l+1)-1);
  nns(which_l) = nn_l;
  if (~exist('W', 'var'))
    [nnTraining_l ~] = yael_nn(single(data.Xtraining(:,which_l)), ...
			       single(data.Xtraining(:,which_l)), nn_l+1 );
  else
    [btmp nnTraining_l ctmp dtmp] = linscan_sorted_multi_mex(B1(:,which_l), B1(:,which_l), nl(l+1), nb, ceil(nb/2), nn_l+1, 4);
  end

  for i=1:numel(id_l)
    neighbors(1:nn_l,id_l(i)) = id_l(nnTraining_l(2:end, i));
  end
end


function  A1 = UpdataArchive(A1,New,V,mu,NI)
% Update archive

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Cheng He

    %% Delete duplicated solutions
    All       = [A1.decs;New.decs];
    [~,index] = unique(All,'rows');
    ALL       = [A1,New];
    Total     = ALL(index);

    %% Select NI solutions for updating the models
	if length(Total)>NI
        [~,active] = NoActive(New.objs,V);
        Vi         = V(setdiff(1:size(V,1),active),:);
        % Select the undeplicated solutions without re-evaluated solutions
        index = ismember(Total.decs,New.decs,'rows');
        Total = Total(~index);
        % Since the number of inactive reference vectors is smaller than
        % NI-mu, we cluster the solutions instead of reference vectors
        PopObj = Total.objs;
        PopObj = PopObj - repmat(min(PopObj,[],1),length(Total),1);
        Angle  = acos(1-pdist2(PopObj,Vi,'cosine'));
        [~,associate] = min(Angle,[],2);
        Via    = Vi(unique(associate)',:);
        Next   = zeros(1,NI-mu);
        if size(Via,1) > NI-mu
            [IDX,~] = kmeans(Via,NI-mu);
            for i = unique(IDX)'
                current = find(IDX==i);
                if length(current)>1
                    best = randi(length(current),1);
                else
                    best = 1;
                end
                Next(i)  = current(best);
            end
        else
            % Cluster solutions based on objective vectors when the number
            % of active reference vectors is smaller than NI-mu
            [IDX,~] = kmeans(Total.objs,NI-mu);
            for i   = unique(IDX)'
                current = find(IDX==i);
                if length(current)>1
                    best = randi(length(current),1);
                else
                    best = 1;
                end
                Next(i)  = current(best);
            end
        end
        A1 = [Total(Next),New];
    else
        A1 = Total;
    end
end


function [Iphi,SigmaPhi,deltaMuPhi,suffStat] = VBA_Iphi_binomial(phi,y,posterior,suffStat,dim,u,options)
% Gauss-Newton update of the observation parameters, for binomial data

if options.DisplayWin % Display progress
    if isequal(options.g_fname,@VBA_odeLim)
        STR = 'VB Gauss-Newton on observation/evolution parameters... ';
    else
        STR = 'VB Gauss-Newton on observation parameters... ';
    end
    set(options.display.hm(1),'string',STR);
    set(options.display.hm(2),'string','0%');
    drawnow
end

%  Look-up which evolution parameter to update
indIn = options.params2update.phi;

% Preallocate intermediate variables
Q = options.priors.SigmaPhi(indIn,indIn);
iQ = VBA_inv(Q);
muPhi0 = options.priors.muPhi;
Phi = muPhi0;
Phi(indIn) = phi;
dphi0 = muPhi0-Phi;
ddydphi = 0;
dy = zeros(dim.p,dim.n_t);
vy = zeros(dim.p,dim.n_t);
gx = zeros(dim.p,dim.n_t);
d2gdx2 = zeros(dim.n_phi,dim.n_phi);
logL = 0;
if isequal(options.g_fname,@VBA_odeLim)
    clear VBA_odeLim
    muX = zeros(options.inG.old.dim.n,dim.n_t);
    SigmaX = cell(dim.n_t,1);
end
div = 0;

%--- Loop over time series ---%
for t=1:dim.n_t

    % evaluate observation function at current mode
    [gx(:,t),dG_dX,dG_dPhi] = VBA_evalFun('g',posterior.muX(:,t),Phi,u(:,t),options,dim,t);

    % fix numerical instabilities
    gx(:,t) = VBA_finiteBinomial (gx(:,t));

    % store states dynamics if ODE mode
    if isequal(options.g_fname,@VBA_odeLim)
        % get sufficient statistics of the hidden states from unused i/o in
        % VBA_evalFun.
        muX(:,t) = dG_dX.xt;
        SigmaX{t} = dG_dX.dx'*posterior.SigmaPhi*dG_dX.dx;
    end

    % predicted variance over binomial data
    vy(:,t) = gx(:,t).*(1-gx(:,t));

    % remove irregular trials
    yin = find(~options.isYout(:,t));

    % accumulate log-likelihood
    logL = logL + y(yin,t)'*log(gx(yin,t)) + (1-y(yin,t))'*log(1-gx(yin,t));

    % prediction error
    dy(yin,t) = y(yin,t) - gx(yin,t);

    % gradient and Hessian
    ddydphi = ddydphi + dG_dPhi(:,yin)*(dy(yin,t)./vy(yin,t));
    tmp = y(yin,t)./gx(yin,t).^2 - (y(yin,t)-1)./(1-gx(yin,t)).^2;
    d2gdx2 = d2gdx2 + dG_dPhi(:,yin)*diag(tmp)*dG_dPhi(:,yin)';

    % Display progress
    if mod(t,dim.n_t./10) < 1
        if options.DisplayWin
            set(options.display.hm(2),'string',[num2str(floor(100*t/dim.n_t)),'%']);
            drawnow
        end
    end

    % Accelerate divergent update
    if VBA_isWeird ({dy, dG_dPhi, dG_dX})
        div = 1;
        break
    end

end

% Display progress
if options.DisplayWin
    set(options.display.hm(2),'string','OK');
    drawnow
end

% posterior covariance matrix
iSigmaPhi = iQ + d2gdx2(indIn,indIn);
SigmaPhi = VBA_inv(iSigmaPhi);

% mode
tmp = iQ*dphi0(indIn) + ddydphi(indIn);
deltaMuPhi = SigmaPhi*tmp;

% variational energy
Iphi = -0.5.*dphi0(indIn)'*iQ*dphi0(indIn) + logL;
if VBA_isWeird ({Iphi, SigmaPhi}) || div
    Iphi = -Inf;
end

% update sufficient statistics
suffStat.gx = gx;
suffStat.dy = dy;
suffStat.logL = logL;
suffStat.vy = vy;
suffStat.dphi = dphi0;
if isequal(options.g_fname,@VBA_odeLim)
    suffStat.muX = muX;
    suffStat.SigmaX = SigmaX;
end
suffStat.div = div;


function image = RGBCircle(image, x, y, rad, col , pointnum, phase)
%RGBCircle  Draws a circle or ellipse on a RGB image.
%   image = RGBCircle(image, x, y, rad, rgbcol)
%   Returns the image with a circle centerd at x ,y drawn.
%
%   image = RGBCircle(image, x, y, [radx rady], rgbcol) draws a ellipse.
%   image = RGBCircle(image, x, y, rad, [r g b]) draws a circle colored by
%   r g b values.
%   image = RGBCircle(image, x, y, [radx rady]) draws a white ellipse.
%
%   If the ellipse is outside the image it will be drawn on the edge

%   Made by Kobi Nistel
%   $Revision: 1 $  $Date: 2010/21/12 17:05:47 $

% Setting default values
if nargin < 5
    col = 255;
end

if nargin < 6
    delta = 1/max([rad,0.1]);
else
    delta = 2*pi/pointnum;
end;

if nargin < 7
  phaseX = 0;
  phaseY = 0;
else
  phaseX = phase(1);
  phaseY = phase(min(length(phase),2));
end

radx = rad(1);
rady = rad(min(length(rad),2));
r = col(1);
g = col(min(length(col),2));
b = col(min(length(col),3));

% Drawing the ellipse
deg = 0:delta:2*pi;
[sy sx ~] = size(image);
vy = min(max(round(y + cos(deg + phaseY)*rady),1),sy);
vx = min(max(round(x + sin(deg + phaseX)*radx),1),sx);
index = (vy-1) + (vx-1)*sy + 1;
image(index) = r;
image(index+sx*sy) = g;
image(index+2*sx*sy) = b;
end


function newLabel=GetLabel(trnData,tstData,trnLabel)

% Shared by algorithms.
% copyright @ guan naiyang

dist=EuDist2(tstData',trnData',0);
[junk, sortedIdx]=sort(dist,2);
newLabel=trnLabel(sortedIdx);
clear dist;

return;


% This file is part of the project NILM-Eval (https://github.com/beckel/nilm-eval).
% Licence: GPL 2.0 (http://www.gnu.org/licenses/gpl-2.0.html)
% Copyright: ETH Zurich, 2014
% Author: Romano Cicchetti

function [best_clustersOfFsm] = find_best_variation(events_FSMs_matrix, clusterOfEvents, valid_cluster_permutations, fsm_idx, events, maxSequenceLength)

    best_deviation_from_median = intmax;
    best_clustersOfFsm = valid_cluster_permutations(1,:);

    for variation = 1:size(valid_cluster_permutations)
        clustersOfFsm = valid_cluster_permutations(variation, :);
        for i=1:length(clustersOfFsm)
            eventsOfFsm = clusterOfEvents == clustersOfFsm(1,i);
            events_FSMs_matrix(eventsOfFsm, fsm_idx) = i;
        end

        % build event sequences of a finte state machine
        sequencesOfFsm = buildSequences(events_FSMs_matrix, events, fsm_idx, maxSequenceLength);
        events_FSMs_matrix(eventsOfFsm, fsm_idx) = 0;
        if isempty(sequencesOfFsm)
            continue;
        end

        % compute median duration of all event sequences that have a high
        % probability of belonging to the shortest path
        duration_of_sequences = events(sequencesOfFsm(:,2),4) - events(sequencesOfFsm(:,1),4);
        if nnz(sequencesOfFsm(:,3) == 1) > 1
            median_duration_of_sequences = median(duration_of_sequences(sequencesOfFsm(:,3) == 1,1));
        else
            median_duration_of_sequences = median(duration_of_sequences(:,1));
        end

        % build directed graph with event sequences as nodes and
        % compute the shortest path
        [directed_graph] = buildGraph(sequencesOfFsm, duration_of_sequences, median_duration_of_sequences);
        num_of_sequences = size(sequencesOfFsm,1);
        [~, path, ~] = graphshortestpath(directed_graph, 1, num_of_sequences, 'Method', 'Bellman-Ford');
        median_duration_of_sequences_on_path = median(duration_of_sequences(path, 1));
        deviation_from_median = abs((duration_of_sequences(path, 1) - median_duration_of_sequences_on_path)/median_duration_of_sequences_on_path);
        quality = deviation_from_median .* log(deviation_from_median);
        quality(isnan(quality)) = -10;

        if sum(quality) < best_deviation_from_median
            best_clustersOfFsm = valid_cluster_permutations(variation, :);
            best_deviation_from_median = deviation_from_median;
        end
    end

end