% PURPOSE : main matlab file to execute the whole program, the layout can
%           be generated using both square layout code or circular layout 
%           code. The ground-truth trajectory is square trajectory.
%           Function of each parameters are detalied in the comments. 
% AUTHORS  : Xi Chen and Yunpeng Li
%            @ECE Department, McGill University
%            http://www.tsp.ece.mcgill.ca/Networks/index.html
% DATE     : Jun 29, 2011

clc
clear all;
close all;

%% Basic setting
numNodes = 24;     %Number of nodes, should be common multiple of 4.
linkNum = numNodes * (numNodes-1)/2;  %Number of links
sideLength = 7;   %Length of the square is 21 feet.
deltaP = 0.1;     %Pixel width of image
numRealization = 1;   %Number of identical simulations
stateDimension = 2;    %State dimension for dynamic model
sigma = 0.02; % \sigma_lambda to control the curve

% Set ground-truth parameters' values, generate RSS
phi_true = 5;
sigma_v_true = 0.3;
sigma_w_true = 0.5;

T_interval = 0.24;    %Simulated time interval per sensing loop
blockLength = T_interval * 50; %Data block to process in a run

%%% Generate squre layout
gridSize = sideLength + 1;
lower(1,:) = 1:(gridSize-2);
lower(2,:) = zeros(1,gridSize-2);
right(1,:) = (gridSize - 1)*ones(1,gridSize-2);
right(2,:) = 1:(gridSize-2);
upper(1,:) = (gridSize-2):-1:1;
upper(2,:) = (gridSize - 1)*ones(1,gridSize-2);
left(1,:) = zeros(1,gridSize-2);
left(2,:) = (gridSize-2):(-1):1;
actualLocs = [lower, right, upper, left];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %% Circle layout, link order is node 1 to node 2,node 1 to 3,...%%%%%
% % node 1 to node 24, node 2 to node 3,...
% actualLocs(1,:) = sideLength / 2 *(1 + ...
%                   cos(2*pi/numNodes*[0:numNodes-1]));
% actualLocs(2,:) = sideLength / 2 *(1 + ...
%                 sin(2*pi/numNodes*[0:numNodes-1]));
% temp = actualLocs(:,19:24);
% actualLocs(:,7:24) = actualLocs(:,1:18);
% actualLocs(:,1:6) = temp;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Store the sensor locations correspond to this link
linkLocs      = zeros(4,linkNum);
linksDistances = zeros(linkNum,1);

linkIndex = 1;
for i = 1:numNodes
   for j = i+1:numNodes
        linkLocs(1:2,linkIndex) = actualLocs(:,i);
        linkLocs(3:4,linkIndex) = actualLocs(:,j);
        linksDistances(linkIndex,:) = ...
            L2distance(actualLocs(:,i),actualLocs(:,j));
        linkIndex = linkIndex + 1;
   end
end

% Generate the zigzag trajectory 
targetTruePos = getTrueTrajectory(sideLength);

% Generate the attenuated RSS data
attenuationCausedByMoving = phi_true * simData(...
    targetTruePos, linkLocs, linksDistances, sigma);

% Store particles for later use, store ground-truth locations
estimatedPositions = zeros(2,size(targetTruePos,2),numRealization);
truePositions = zeros(size(targetTruePos,1),...
    size(targetTruePos,2),numRealization);
for i = 1:numRealization
    truePositions(:,:,i) = targetTruePos;
end

%% Main algorithm 
for realizationForOneTrack = 1:numRealization
    
    % Initialize the unknown parameters
    phi = unidrnd(10);   % \phi in measurement model
    sigma_v = unidrnd(1); % noise \sigma_v in dynamic model
    sigma_w = unidrnd(1);% noise \sigma_s in measurement model
    
    deltaYMat(:,:) = attenuationCausedByMoving +...
        sigma_w_true * randn(size(attenuationCausedByMoving));
    
    counter = 0;
    N = 1000;  % Number of particles
    L = 10;    % Length of one online EM run
    
    % Algorithm parameter setting
    psiVec = zeros(1,4);  % Store the unknown parameters 
    deltaYBuffer = zeros(L,size(deltaYMat,1));
    xParticlesInOneEMRun = zeros(stateDimension,N,L);
    xParticlesPreviousRound = zeros(stateDimension,N);
    xParticlesPreviousRound(1:2,:) = sideLength * rand(2,N);
    weightsCurrent = ones(1,N);
    diffVec = [];
    mseResultVec = [];
    pathDistanceFromSide = 1;
    targetPosVec = zeros(2,22);
    phiVec = [phi];
    sigmavVec = [sigma_v];
    sigmawVec = [sigma_w];
    phiLength = floor(size(deltaYMat,2) / L);

    
    while true        
        counter = counter + 1;

        if counter > size(deltaYMat,2)
            break
        end

        % Particle filtering 
        deltaY = deltaYMat(:,counter);
        
        [xParticlesThisRound,weightsCurrent] = ...
            particleFilter(xParticlesPreviousRound,deltaY,...
            phi,sigma_v,N,sideLength,...
            weightsCurrent,linkLocs,linksDistances,sigma);

        % Get the estimated target position
        targetPos = mean(xParticlesThisRound,2);
        estimatedPositions(:,counter,realizationForOneTrack)...
            = targetPos(:);

        %Buffer the number of particle filtering runs
        indexWithinOneEMRun = mod(counter,L);

        %Online EM when buffer L is full 
        if indexWithinOneEMRun
            deltaYBuffer(indexWithinOneEMRun,:) = deltaY;
            xParticlesInOneEMRun(1:2,:,indexWithinOneEMRun)...
                = xParticlesThisRound;
        else
            deltaYBuffer(L,:) = deltaY;
            xParticlesInOneEMRun(1:2,:,L) = xParticlesThisRound;

            %Compute the sufficient statistics, 
            %update the unknown model parameter.
            [phi, sigma_v, sigma_w, psiVec, phiVec,sigmavVec, ...
                sigmawVec] = onlineEM(phi,...
                psiVec, xParticlesInOneEMRun, deltaYBuffer,...
                linkLocs, linksDistances, counter, ...
                phiVec, sigmavVec, sigmawVec, sigma);

        end
        %Update the particles
        xParticlesPreviousRound = xParticlesThisRound;
    end

    phis(:,realizationForOneTrack) = phiVec(:);
    sigmavs(:,realizationForOneTrack) = sigmavVec;
    sigmaws(:,realizationForOneTrack) = sigmawVec;
end

    %% Save the tracked results

    save('estimatedPositions.mat', 'estimatedPositions');
    save('truePositions.mat', 'truePositions');
    save('phis.mat', 'phis');
    save('sigmavs.mat', 'sigmavs');
    save('sigmaws.mat', 'sigmaws');

