close all;
clear;
clc;

%% Experiment Data
filenames = "combined_test_case_3.xlsx";
raw_data = xlsread(filenames);
raw_data(:,1:5)=[];


%% Raw data
raw_temperatures(:,1) = ((raw_data(:,1)-1.25)./0.005)-23;
raw_temperatures(:,2) = ((raw_data(:,2)-1.25)./0.005)-23;
raw_temperatures(:,3) = ((raw_data(:,3)-1.25)./0.005)-23;
raw_temperatures(:,4) = ((raw_data(:,4)-1.25)./0.005)-23;
raw_temp_sum=(raw_temperatures(:,1)+raw_temperatures(:,2)+raw_temperatures(:,3)+raw_temperatures(:,4));
raw_source_location(:,1)=.5+(((raw_temperatures(:,1)+raw_temperatures(:,3))./raw_temp_sum(:)).*9);
raw_source_location(:,2)=.5+(9-((raw_temperatures(:,1)+raw_temperatures(:,2))./raw_temp_sum(:)).*9);

%% Kalman filtering data
R=[0.00003,0.00003,0.00003,0.00003]; %Based off test case 3
Q=0.001*ones(1,4); %Assume low noise
F=[1,1,1,1]; %Assume little change
Pc=[0,0,0,0]; %
G=[0,0,0,0]; %
P=[0,0,0,0]; %
Xp=[0,0,0,0]; %
Zp=[0,0,0,0]; %
Xe=[0,0,0,0]; %
filt_temperatures=zeros(length(raw_data(:,1)),4);
for i = 1:length(raw_data(:,1))
    Pc=P+Q;
    G= Pc./(Pc+R); %Kalman gain
    P=(1-G).*Pc;
    Xp=Xe;
    Zp=Xp;
    Xe=G.*(raw_temperatures(i,:)-Zp)+Xp; %Kalman estimate
    filt_temperatures(i,:)=Xe;
end
%filt_temperatures(:,1) = ((filt_data(:,1)-1.25)./0.005);
%filt_temperatures(:,2) = ((filt_data(:,2)-1.25)./0.005);
%filt_temperatures(:,3) = ((filt_data(:,3)-1.25)./0.005);
%filt_temperatures(:,4) = ((filt_data(:,4)-1.25)./0.005);

%Zero temperatures to get differential
filt_temp_sum=(filt_temperatures(:,1)+filt_temperatures(:,2)+filt_temperatures(:,3)+filt_temperatures(:,4));
filt_source_location(:,1)=.5+((filt_temperatures(:,1)+filt_temperatures(:,3))./filt_temp_sum(:)).*9;
filt_source_location(:,2)=.5+(9-((filt_temperatures(:,1)+filt_temperatures(:,2))./filt_temp_sum(:)).*9);

%%
figure()
hold on
plot(raw_temperatures(:,1));
plot(filt_temperatures(:,1));
title('Plot of raw and filtered T1 thermocouple for test case 1')
xlabel('Time Step (0.001s)')
ylabel('Temperature Differential(C)')
legend('Raw measurement','Filtered measurement')
%% Bayesian Fusion

% User inputs
size_x = 0.1; % length of plate [m]
size_y = 0.1; % width of plate [m]
grid_x = 10; %number of cells in x-directions
grid_y = grid_x; %number of cells in y-directions
num_ir = 4;
target_rad = 0.015;
sensor_pos = [1  3; 1 8 ; 2 3 ; 2 8]; % ([x(1),y(2)], position) line of sight

% Occupancy Grid
map = robotics.OccupancyGrid(0.1,0.1,grid_x*10);
[x,y] = meshgrid(1:grid_x);
setOccupancy(map,[x(:) y(:)],0.5, "grid");

% Initialize grid probabilities
p_occ = 0.5*ones(grid_x,grid_y); % overall grid
l_odds = log10(p_occ./(1-p_occ));

lmin = -2.0;
lmax = 3.5;
measured = 0;

dx = size_x/grid_x;
dy = size_y/grid_y;

delta_t = 0.001;
tau = 0.1;
%% Simulation data
Temp_Var=0.75; %Temperature variance

Sim_Temp_Data_1=[(randn(60000,1)*Temp_Var)+linspace(0,6,60000)'...
    (randn(60000,1)*Temp_Var)+linspace(0,2,60000)' (randn(60000,1)*Temp_Var)+linspace(0,2,60000)' (randn(60000,1)*Temp_Var)+linspace(0,2,60000)'];

Sim_Temp_Data_2=[(randn(60000,1)*Temp_Var)+linspace(0,2,60000)'...
    (randn(60000,1)*Temp_Var)+linspace(0,2,60000)' (randn(60000,1)*Temp_Var)+linspace(0,2,60000)' (randn(60000,1)*Temp_Var)+linspace(0,6,60000)'];

Sim_Temp_Data_3=[(randn(60000,1)*Temp_Var)+linspace(0,2,60000)'...
    (randn(60000,1)*Temp_Var)+linspace(0,6,60000)' (randn(60000,1)*Temp_Var)+linspace(0,2,60000)' (randn(60000,1)*Temp_Var)+linspace(0,2,60000)'];

% Sim_Temp_Data_4=[(randn(60000,1)*Temp_Var)+linspace(0,2,60000)'...
%     (randn(60000,1)*Temp_Var)+linspace(0,2,60000)' (randn(60000,1)*Temp_Var)+linspace(0,6,60000)' (randn(60000,1)*Temp_Var)+linspace(0,2,60000)']
%
% Sim_Temp_Data_5=[(randn(60000,1)*Temp_Var)+linspace(0,2,60000)'...
%     (randn(60000,1)*Temp_Var)+linspace(0,6,60000)' (randn(60000,1)*Temp_Var)+linspace(0,6,60000)' (randn(60000,1)*Temp_Var)+linspace(0,2,60000)']
%
% Sim_Temp_Data_6=[(randn(60000,1)*Temp_Var)+linspace(0,6,60000)'...
%     (randn(60000,1)*Temp_Var)+linspace(0,6,60000)' (randn(60000,1)*Temp_Var)+linspace(0,2,60000)' (randn(60000,1)*Temp_Var)+linspace(0,2,60000)']

%Get source location from sim data
% Sum_Sim_Temp_Data=(Sim_Temp_Data_3(:,1)+Sim_Temp_Data_3(:,2)+Sim_Temp_Data_3(:,3)+Sim_Temp_Data_3(:,4));
% filt_source_location(:,1)=.5+9-(((Sim_Temp_Data_3(:,1)+Sim_Temp_Data_3(:,2))./Sum_Sim_Temp_Data(:)).*9);
% filt_source_location(:,2)=.5+((Sim_Temp_Data_3(:,1)+Sim_Temp_Data_3(:,3))./Sum_Sim_Temp_Data(:)).*9;


%%
%for t=1:size(filt_source_location,1)
for t=1:1000
    t;
    % Bivariate Gaussian distribution
    % mu = current estimate position of heat source
    mu = filt_source_location(i,:)/100;
    sigma = [0.0025,0.0025];
    for j=1:grid_x
        for k=1:grid_y
            %apply forgetting factor
            p_occ(j,k) = (p_occ(j,k)-0.5)*exp(-delta_t/tau)+0.5;
            l_odds(j,k) = log10(p_occ(j,k)/(1-p_occ(j,k)));

            xy = grid2world(map,[j k]);
            prob_inv = 0.8*(mvnpdf(xy,mu,sigma)/mvnpdf(mu,mu,sigma))+0.1;
            l = l_odds(j,k) + log10(prob_inv/(1-prob_inv));

            % log odds update (temporal)
            if l > lmax
                l = lmax;
            elseif l < lmin
                l = lmin;
            end
            l_odds(j,k) = l;

            prob = 1-(1/(1+exp(l_odds(j,k))));
            p_occ(j,k) = prob;

        end
    end

end

[y,x] = meshgrid(1:grid_x);
setOccupancy(map,[x(:) y(:)],p_occ(:), "grid");
show(map);
hold on
% Get xy values of grid with probability of 0.8 or higher
x = [];
y = [];
for i=1:size(p_occ,1)
    for j=1:size(p_occ,2)
        if p_occ(i,j) >= 0.7
            xy = grid2world(map,[i j]);
            x = vertcat(x,xy(1));
            y = vertcat(y,xy(2));
        end
    end
end
x_loc = mean(x);
y_loc = mean(y);
plot(x_loc,y_loc,'xr','MarkerSize',10)
grid on;
colorbar;
%%
figure()
hold on
c = linspace(1,60000,length(filt_temp_sum));
scatter(filt_source_location(:,1),filt_source_location(:,2),[5],c)
xlim([0 10])
ylim ([0 10])
hcb=colorbar;
title('Plot of heat source centerpoint')
xlabel('X [meters]')
ylabel('Y [meters]')
%%
figure()
hold on
c = linspace(1,60000,length(filt_temp_sum));
scatter(raw_source_location(:,1),raw_source_location(:,2),[5],c)
xlim([0 10])
ylim ([0 10])
hcb=colorbar;
title('Plot of heat source centerpoint')
xlabel('X [meters]')
ylabel('Y [meters]')
%%
x_moving_Variance=movvar(raw_source_location(:,1),600);
y_moving_Variance=movvar(raw_source_location(:,2),600);

figure()
hold on
plot(x_moving_Variance)
plot(y_moving_Variance)
plot(1:1:60000,0.75*ones(60000,1))
ylim([0 4])
title('Change in variance over time with 600 point window')
xlabel('Time step (0.001s)')
ylabel('Variance [cm]')
%%
x_moving_Variance=movvar(filt_source_location(:,1),600);
y_moving_Variance=movvar(filt_source_location(:,2),600);

figure()
hold on
plot(x_moving_Variance)
plot(y_moving_Variance)
plot(1:1:60000,0.75*ones(60000,1))
ylim([0 4])
title('Change in variance over time with 600 point window')
xlabel('Time step (0.001s)')
ylabel('Variance [cm]')