clc

clear

tic

%Desired sampling of wavelength in nm to test over:
wave=400:1:700;
%With the pig data you can probably go up to 800nm but not more because
%then it gets noisy.

snrs=20:5:40;     %Reasonable values are 20-40.  This is sweeped out below.


%Specify sensor
%size(sensor)=number of channels x length(wave)
%The absolute scale of the sensor matrix is irrelevant.
%Units should be quanta so all values should be between 0 and 1.
tmp=load('NikonD100');
sensor=interp1(tmp.wavelength,tmp.data,wave)';

figure
plot(wave,sensor)
title('NikonD100 SPD')
ylabel('Transmission')
xlabel('Wavelength (nm)')


values = load('pig_reflectances');

figure
original_spectra = interp1(values.wavelength,values.data,wave);
plot(wave,original_spectra)
title('Original Spectra')
xlabel('Wavelength (nm)')
ylabel('Reflectance')


% Specify illuminant
%You can choose other illuminant if desired.  Tungsten might be good since
%it has a lot of energy in the red region of the spectrum, just like tissue.
illuminant_name='Tungsten';


%Load illuminant:
tmp=load(illuminant_name);
illuminant=interp1(tmp.wavelength,tmp.data,wave); 
figure
plot(wave,illuminant)
title('Tungsten Illuminant')
ylabel('Strength')
xlabel('Wavelength (nm)')


% Specify SNR
snr=snrs(1)


[xhat,rmse,deltaE]=rectest(sensor,illuminant_name,snr,wave);
mean_RMSE = mean(rmse)
mean_deltaE = mean(deltaE)




figure
plot(wave,xhat)
title('Estimated Spectra')
ylabel('Reflectance')
xlabel('Wavelength (nm)')


figure

plot(deltaE)
title('\DeltaE for each Surface')
xlabel('Surface Number')
ylabel('\DeltaE')


min_deltaE_surface = find(deltaE == min(deltaE));

figure

hold on
plot(wave,original_spectra(:,min_deltaE_surface))
plot(wave,xhat(:,min_deltaE_surface),'--')
title('Surface with Minimum \DeltaE')
xlabel('Wavelength (nm)')
ylabel('Reflectance')


hold off


max_deltaE_surface = find(deltaE == max(deltaE));

figure

hold on
plot(wave,original_spectra(:,max_deltaE_surface))
plot(wave,xhat(:,max_deltaE_surface),'--')
title('Surface with Maximum \DeltaE')
xlabel('Wavelength (nm)')
ylabel('Reflectance')

hold off



figure

plot(rmse)
title('RMSE for each Surface')
xlabel('Surface Number')
ylabel('RMSE')



min_rmse_surface = find(rmse == min(rmse));

figure

hold on
plot(wave,original_spectra(:,min_rmse_surface))
plot(wave,xhat(:,min_rmse_surface),'--')
title('Surface with Minimum RMSE')
xlabel('Wavelength (nm)')
ylabel('RMSE')

hold off



max_rmse_surface = find(rmse == max(rmse));

figure

hold on
plot(wave,original_spectra(:,max_rmse_surface))
plot(wave,xhat(:,max_rmse_surface),'--')
title('Surface with Maximum RMSE')
xlabel('Wavelength (nm)')
ylabel('RMSE')

hold off



% going through each wavelength for all surfaces
for counter = 1:size(original_spectra,1)
    
    RMSE_lambda(counter) = sqrt(mean((original_spectra(counter,:) - xhat(counter,:)).^2));
    
end

figure

hold on

[AX H H1] = plotyy(wave,RMSE_lambda,wave,sensor)
[AX H H2] = plotyy(wave,RMSE_lambda,wave,sensor)


title('RMSE & Transmission vs. Wavelength')
xlabel('Wavelength (nm)')


set(get(AX(1),'Ylabel'),'String','RMSE','color','r')
set(AX(1),'Ycolor','r')
set(get(AX(2),'Ylabel'),'String','Transmission','color','b')
set(AX(2),'Ycolor','b')




set(H,'LineStyle','-','color','r')
set(H1,'LineStyle','-','color','b')
set(H2,'LineStyle','--','color','b','LineWidth',1.01)


hold off






toc



















%{


% possibilities
blue_mean_range = 400:5:700;
green_mean_range = 400:5:700;
red_mean_range = 400:5:700;


blue_FWHM_range = 10:5:300;
green_FWHM_range = 10:5:300;
red_FWHM_range = 10:5:300;


% % previously visited numbers
% prev_visit_blue_mean = zeros(1,length(blue_mean_range));
% prev_visit_green_mean = zeros(1,length(green_mean_range));
% prev_visit_red_mean = zeros(1,length(red_mean_range));
%
% prev_visit_blue_FWHM = zeros(1,length(blue_FWHM_range));
% prev_visit_green_FWHM = zeros(1,length(green_FWHM_range));
% prev_visit_red_FWHM = zeros(1,length(red_FWHM_range));



% Specify illuminant
%You can choose other illuminant if desired.  Tungsten might be good since
%it has a lot of energy in the red region of the spectrum, just like tissue.
illuminant_name='Tungsten';


% Specify SNR
snr=snrs(5)



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

% initial guess


figure
hold


curr_blue_mean = 450;
index_curr_blue_mean = find(blue_mean_range == curr_blue_mean);
curr_blue_FWHM = 20;
index_curr_blue_FWHM = find(blue_FWHM_range == curr_blue_FWHM);
curr_blue_std = curr_blue_FWHM/(2*sqrt(2*log(2)));
blue_pdfNormal = normpdf(wave, curr_blue_mean, curr_blue_std);
blue_pdfNormal = blue_pdfNormal/max(blue_pdfNormal);
plot(wave, blue_pdfNormal,'b');


curr_green_mean = 550;
index_curr_green_mean = find(green_mean_range == curr_green_mean);
curr_green_FWHM = 50;
index_curr_green_FWHM = find(green_FWHM_range == curr_green_FWHM);
curr_green_std = curr_green_FWHM/(2*sqrt(2*log(2)));
green_pdfNormal = normpdf(wave, curr_green_mean, curr_green_std);
green_pdfNormal = green_pdfNormal/max(green_pdfNormal);
plot(wave, green_pdfNormal,'g');


curr_red_mean = 600;
index_curr_red_mean = find(red_mean_range == curr_red_mean);
curr_red_FWHM = 20;
index_curr_red_FWHM = find(red_FWHM_range == curr_red_FWHM);
curr_red_std = curr_red_FWHM/(2*sqrt(2*log(2)));
red_pdfNormal = normpdf(wave, curr_red_mean, curr_red_std);
%red_pdfNormal = 0.7*red_pdfNormal/max(red_pdfNormal);
red_pdfNormal = red_pdfNormal/max(red_pdfNormal);
plot(wave, red_pdfNormal,'r');

hold off


sensor = vertcat(blue_pdfNormal,green_pdfNormal,red_pdfNormal);


[xhat,rmse,deltaE]=rectest(sensor,illuminant_name,snr,wave);
curr_cost = mean(deltaE);



prev_visit_blue_mean = curr_blue_mean;
prev_visit_green_mean = curr_green_mean;
prev_visit_red_mean = curr_red_mean;

prev_visit_blue_FWHM = curr_blue_FWHM;
prev_visit_green_FWHM = curr_green_FWHM;
prev_visit_red_FWHM = curr_red_FWHM;





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

num_iteration = 1;
control_param = 1;

T_small = 10^-31;



while control_param > T_small
    
    for counter = 1:1:log(num_iteration +1)
        
        
        
        
        % not_done = true;
        
        %while not_done
        
        % create new state
        
        % randomly select indices
        index_new_blue_mean = round(1 + (length(blue_mean_range)-1)*rand(1));
        index_new_green_mean = round(1 + (length(green_mean_range)-1)*rand(1));
        index_new_red_mean = round(1 + (length(red_mean_range)-1)*rand(1));
        
        index_new_blue_FWHM  = round(1 + (length(blue_FWHM_range)-1)*rand(1));
        index_new_green_FWHM  = round(1 + (length(blue_FWHM_range)-1)*rand(1));
        index_new_red_FWHM  = round(1 + (length(blue_FWHM_range)-1)*rand(1));
        
        
        
        
        % create blue filter
        new_blue_mean = blue_mean_range(index_new_blue_mean);
        new_blue_FWHM = blue_FWHM_range(index_new_blue_FWHM);
        new_blue_std = new_blue_FWHM/(2*sqrt(2*log(2)));
        blue_pdfNormal = normpdf(wave, new_blue_mean, new_blue_std);
        blue_pdfNormal = blue_pdfNormal/max(blue_pdfNormal);
        
        % create green filter
        new_green_mean = green_mean_range(index_new_green_mean);
        new_green_FWHM = green_FWHM_range(index_new_green_FWHM);
        new_green_std = new_green_FWHM/(2*sqrt(2*log(2)));
        green_pdfNormal = normpdf(wave, new_green_mean, new_green_std);
        green_pdfNormal = green_pdfNormal/max(green_pdfNormal);
        
        
        % create red filter
        new_red_mean = red_mean_range(index_new_red_mean);
        new_red_FWHM = red_FWHM_range(index_new_red_FWHM);
        new_red_std = new_red_FWHM/(2*sqrt(2*log(2)));
        red_pdfNormal = normpdf(wave, new_red_mean, new_red_std);
        red_pdfNormal = red_pdfNormal/max(red_pdfNormal);
        
        
        %         % do not consider previously visited numbers
        %         condition1 = isequal(prev_visit_blue_mean(find(prev_visit_blue_mean == new_blue_mean)),new_blue_mean);
        %         condition2 = isequal(prev_visit_green_mean(find(prev_visit_green_mean == new_green_mean)),new_green_mean);
        %         condition3 = isequal(prev_visit_red_mean(find(prev_visit_red_mean == new_red_mean)),new_red_mean);
        %         condition4 = isequal(prev_visit_blue_FWHM(find(prev_visit_blue_FWHM == new_blue_FWHM)),new_blue_FWHM);
        %         condition5 = isequal(prev_visit_green_FWHM(find(prev_visit_green_FWHM == new_green_FWHM)),new_green_FWHM);
        %         condition6 = isequal(prev_visit_red_FWHM(find(prev_visit_red_FWHM == new_red_FWHM)),new_red_FWHM);
        
        %         if condition1 & condition2 & condition3 & condition4 & condition5 & condition6
        %
        %
        %             not_done = true;
        %
        %
        %         else
        %
        %             not_done = false;
        %
        %         end
        %
        % end
        
        
        
        
        
        
        % create sensor
        sensor = vertcat(blue_pdfNormal,green_pdfNormal,red_pdfNormal);
        
        
        % calculate cost function
        [xhat,rmse,deltaE]=rectest(sensor,illuminant_name,snr,wave);
        new_cost = mean(deltaE);
        
        
        
        if (new_cost - curr_cost <= 0)
            
            % assign new state to current state
            
            curr_blue_mean = new_blue_mean;
            curr_green_mean = new_green_mean;
            curr_red_mean = new_red_mean;
            
            curr_blue_FWHM = new_blue_FWHM;
            curr_green_FWHM = new_green_FWHM;
            curr_red_FWHM = new_red_FWHM;
            
            curr_cost = new_cost;
            
            best_deltaE = deltaE;
            best_xhat = xhat;
            
            
            
            % slightly perturb parameters
            
            blue_mean_range = curr_blue_mean-50:5:curr_blue_mean+50;
            green_mean_range = curr_green_mean-50:5:curr_green_mean+50;
            red_mean_range = curr_red_mean-50:5:curr_red_mean+50;
            
            
            blue_FWHM_range = 10:5:300;
            green_FWHM_range = 10:5:300;
            red_FWHM_range = 10:5:300;
            
            
             control_param = control_param/log(100);
            
            
            %         % update previous visits
            %         if ~isequal(prev_visit_blue_mean(find(prev_visit_blue_mean == new_blue_mean)),new_blue_mean)
            %
            %             prev_visit_blue_mean = [prev_visit_blue_mean(1:length(prev_visit_blue_mean)) new_blue_mean prev_visit_blue_mean(length(prev_visit_blue_mean)+1:end)];
            %         end
            %
            %         if ~isequal(prev_visit_green_mean(find(prev_visit_green_mean == new_green_mean)),new_green_mean)
            %             prev_visit_green_mean = [prev_visit_green_mean(1:length(prev_visit_green_mean)) new_green_mean prev_visit_green_mean(length(prev_visit_green_mean)+1:end)];
            %
            %         end
            %
            %         if ~isequal(prev_visit_red_mean(find(prev_visit_red_mean == new_red_mean)),new_red_mean)
            %             prev_visit_red_mean = [prev_visit_red_mean(1:length(prev_visit_red_mean)) new_red_mean prev_visit_red_mean(length(prev_visit_red_mean)+1:end)];
            %         end
            %
            %
            %         if ~isequal(prev_visit_blue_FWHM(find(prev_visit_blue_FWHM == new_blue_FWHM)),new_blue_FWHM)
            %             prev_visit_blue_FWHM = [prev_visit_blue_FWHM(1:length(prev_visit_blue_FWHM)) new_blue_FWHM prev_visit_blue_FWHM(length(prev_visit_blue_FWHM)+1:end)];
            %
            %         end
            %
            %
            %         if ~isequal(prev_visit_green_FWHM(find(prev_visit_green_FWHM == new_green_FWHM)),new_green_FWHM)
            %             prev_visit_green_FWHM = [prev_visit_green_FWHM(1:length(prev_visit_green_FWHM)) new_green_FWHM prev_visit_green_FWHM(length(prev_visit_green_FWHM)+1:end)];
            %         end
            %
            %         if ~isequal(prev_visit_red_FWHM(find(prev_visit_red_FWHM == new_red_FWHM)),new_red_FWHM)
            %
            %             prev_visit_red_FWHM = [prev_visit_red_FWHM(1:length(prev_visit_red_FWHM)) new_red_FWHM prev_visit_red_FWHM(length(prev_visit_red_FWHM)+1:end)];
            %         end
            %
            
            
            
            %         % remove current state from the arrays
            %
            %         blue_mean_range(index_curr_blue_mean) = [];
            %         green_mean_range(index_curr_green_mean) = [];
            %         red_mean_range(index_curr_red_mean) = [];
            %
            %
            %         blue_FWHM_range(index_curr_blue_FWHM) = [];
            %         green_FWHM_range(index_curr_green_FWHM) = [];
            %         red_FWHM_range(index_curr_red_FWHM) = [];
            %
            %
            %
            %         % values in arrays get shifted after removal of elements
            %         % need to keep track of the indices of the new values
            %
            %         index_curr_blue_mean = find(blue_mean_range == curr_blue_mean);
            %         index_curr_green_mean = find(green_mean_range == curr_green_mean);
            %         index_curr_red_mean = find(red_mean_range == curr_red_mean);
            %
            %
            %
            %         index_curr_blue_FWHM = find(blue_FWHM_range == curr_blue_FWHM);
            %         index_curr_green_FWHM = find(green_FWHM_range == curr_green_FWHM);
            %         index_curr_red_FWHM = find(red_FWHM_range == curr_red_FWHM);
            %
            
            
            
            
        elseif (curr_cost - new_cost)/control_param > rand(1)
                
                
                % assign new state to current state
                
                curr_blue_mean = new_blue_mean;
                curr_green_mean = new_green_mean;
                curr_red_mean = new_red_mean;
                
                curr_blue_FWHM = new_blue_FWHM;
                curr_green_FWHM = new_green_FWHM;
                curr_red_FWHM = new_red_FWHM;
                
                curr_cost = new_cost;
                
                best_deltaE = deltaE;
                best_xhat = xhat;
                
                
                blue_mean_range = 400:5:700;
                green_mean_range = 400:5:700;
                red_mean_range = 400:5:700;
                
                
                blue_FWHM_range = 10:5:300;
                green_FWHM_range = 10:5:300;
                red_FWHM_range = 10:5:300;
                
                
                 control_param = 10
                 
                %         % need to keep track of the indices of the new values
                %
                %         index_curr_blue_mean = find(blue_mean == curr_blue_mean);
                %         index_curr_green_mean = find(blue_mean == curr_blue_mean);
                %         index_curr_red_mean = find(blue_mean == curr_blue_mean);
                %
                %
                %
                %         index_curr_blue_FWHM = find(blue_mean == curr_blue_FWHM);
                %         index_curr_green_FWHM = find(blue_mean == curr_blue_FWHM);
                %         index_curr_red_FWHM = find(blue_mean == curr_blue_FWHM);
                %
                
                
            %end
            
            
        else
                
            
             blue_mean_range = 400:5:700;
                green_mean_range = 400:5:700;
                red_mean_range = 400:5:700;
                
                
                blue_FWHM_range = 10:5:300;
                green_FWHM_range = 10:5:300;
                red_FWHM_range = 10:5:300;
            
            
            
            
            
            
            %
            %         % remove new state from the arrays
            %
            %         blue_mean_range(index_new_blue_mean) = [];
            %         green_mean_range(index_new_green_mean) = [];
            %         red_mean_range(index_new_red_mean) = [];
            %
            %
            %         blue_FWHM_range(index_new_blue_FWHM) = [];
            %         green_FWHM_range(index_new_green_FWHM) = [];
            %         red_FWHM_range(index_new_red_FWHM) = [];
            %
            
            
            
            %         % update previous visits
            %         if ~isequal(prev_visit_blue_mean(find(prev_visit_blue_mean == new_blue_mean)),new_blue_mean)
            %
            %             prev_visit_blue_mean = [prev_visit_blue_mean(1:length(prev_visit_blue_mean)) new_blue_mean prev_visit_blue_mean(length(prev_visit_blue_mean)+1:end)];
            %         end
            %
            %         if ~isequal(prev_visit_green_mean(find(prev_visit_green_mean == new_green_mean)),new_green_mean)
            %             prev_visit_green_mean = [prev_visit_green_mean(1:length(prev_visit_green_mean)) new_green_mean prev_visit_green_mean(length(prev_visit_green_mean)+1:end)];
            %
            %         end
            %
            %         if ~isequal(prev_visit_red_mean(find(prev_visit_red_mean == new_red_mean)),new_red_mean)
            %             prev_visit_red_mean = [prev_visit_red_mean(1:length(prev_visit_red_mean)) new_red_mean prev_visit_red_mean(length(prev_visit_red_mean)+1:end)];
            %         end
            %
            %
            %         if ~isequal(prev_visit_blue_FWHM(find(prev_visit_blue_FWHM == new_blue_FWHM)),new_blue_FWHM)
            %             prev_visit_blue_FWHM = [prev_visit_blue_FWHM(1:length(prev_visit_blue_FWHM)) new_blue_FWHM prev_visit_blue_FWHM(length(prev_visit_blue_FWHM)+1:end)];
            %
            %         end
            %
            %
            %         if ~isequal(prev_visit_green_FWHM(find(prev_visit_green_FWHM == new_green_FWHM)),new_green_FWHM)
            %             prev_visit_green_FWHM = [prev_visit_green_FWHM(1:length(prev_visit_green_FWHM)) new_green_FWHM prev_visit_green_FWHM(length(prev_visit_green_FWHM)+1:end)];
            %         end
            %
            %         if ~isequal(prev_visit_red_FWHM(find(prev_visit_red_FWHM == new_red_FWHM)),new_red_FWHM)
            %
            %             prev_visit_red_FWHM = [prev_visit_red_FWHM(1:length(prev_visit_red_FWHM)) new_red_FWHM prev_visit_red_FWHM(length(prev_visit_red_FWHM)+1:end)];
            %         end
            %
        end
        
        
        %     % hold control_param fixed for every 50 iterations
        %     if mod(counter,100) == 0
        %
        %         control_param = control_param/log(num_iteration);
        %
        %     end
        
%         curr_cost
%         
%         counter
%         
%         control_param
%         
    end
    
    
   
    
    num_iteration = num_iteration + 1
    
    curr_cost
    
end

figure
hold on

plot(wave, blue_pdfNormal,'b');
plot(wave, green_pdfNormal,'g');
plot(wave, red_pdfNormal,'r');


hold off



figure
plot(wave,best_xhat(:,:))
title('x-hat')


figure
plot(best_deltaE)
title('x-hat')

%
%
% figure
% plot(deltaE)
% title('RMSE')

toc

%}