classdef MapClass < handle
    %MapClass Summary of this class goes here
    %   Detailed explanation goes here
    
    properties
        N_abs %Absolute Northings
        N  %Local Northings
        E_abs %Absolute Eastings
        E %Local Eastings
        D %Down matrix
        D_Nan %Dow matrix with NaNs
        
        
        varD
        varTot
        
        n_E %number of East Points
        n_N %number of North Points
        
        n_I %total number of DEM squares
        
        d_N %North Gradient
        d_N_smooth %North Gradient Smoothed
        d_E %East Gradient
        d_E_smooth %East Gradient Smoothed
        d_mag %Gradient Magnitude
        
        
        HtH % H = [d_N d_E]
        HtH_smoothed %H = [d_N_smoothed d_E_smoothed]
        
        
        infoN
        infoN_smooth
        infoN_thresh
        infoE
        infoE_smooth
        infoE_thresh
        
        threshBoth
        
        map_name
        badMap
        
        res;
        
        varModMap = 0.02;
        varBeam = 3;
        alpha;
        
    end
    
    methods
        %Constructor
        function obj = MapClass(name,bounds,show)
            
            if(nargin < 3)
                show = 0; %Don't plot map
            end
            
            %Add on .grd if the name is missing it
            if(strcmp(name((end-3):end),'.grd'))
                obj.map_name = name;
            else
                obj.map_name = strcat(name,'.grd');
            end
            
            
            [x,y,z] = read_grd(obj.map_name); %read full map
            obj.res = round(abs(x(2)-x(1)));
            
            if(isempty(bounds))
                bounds = [2 length(y)-1 2 length(x)-1];
            end
            
            obj.N_abs = y(bounds(1):1:bounds(2));
            obj.N = obj.N_abs-min(obj.N_abs)+1;
            
            obj.E_abs = x(bounds(3):1:bounds(4));
            obj.E = obj.E_abs-min(obj.E_abs)+1;
%             if(min(min(z))<0)
%                 obj.D = -z(bounds(1):bounds(2),bounds(3):bounds(4));
%             else
%                 obj.D = z(bounds(1):bounds(2),bounds(3):bounds(4));
%             end
            obj.D = z(bounds(1):bounds(2),bounds(3):bounds(4));
            obj.D_Nan = obj.D;
            obj.D(isnan(obj.D)) = 0;
            
            obj.n_N = length(obj.N_abs);
            obj.n_E = length(obj.E_abs);
            obj.n_I = length(obj.D(:));
            
            obj.d_N = (-obj.D-z((bounds(1)+1):1:(bounds(2)+1),bounds(3):1:bounds(4)))/obj.res;
            obj.d_N(isnan(obj.d_N)) = 0;
            obj.d_E = (-obj.D-z(bounds(1):1:bounds(2),(bounds(3)+1):1:(bounds(4)+1)))/obj.res;
            obj.d_E(isnan(obj.d_E)) = 0;
            obj.d_mag = sqrt(obj.d_N.^2+obj.d_E.^2);
            obj.d_mag(isnan(obj.d_mag)) = 0;
            
            obj.HtH(1,1,:,:) = obj.d_N.*obj.d_N;
            obj.HtH(1,2,:,:) = obj.d_N.*obj.d_E;
            obj.HtH(2,1,:,:) = obj.HtH(1,2,:,:);
            obj.HtH(2,2,:,:) = obj.d_E.*obj.d_E;
            
            obj.badMap = isnan(obj.D_Nan);
            
            if(show)
                obj.plot
            end
            
        end
        
        function reduceMap(obj,bounds)
            coordLow = obj.getClosestPointAbs(bounds(1),bounds(3));
            coordHigh = obj.getClosestPointAbs(bounds(2),bounds(4));
            Nlow = coordLow(1);
            Nhigh = coordHigh(1);
            Elow = coordLow(2);
            Ehigh = coordHigh(2);
            
            obj.N_abs = obj.N_abs(Nlow:Nhigh);
            obj.N = obj.N_abs-min(obj.N_abs)+1;
            
            obj.E_abs = obj.E_abs(Elow:Ehigh);
            obj.E = obj.E_abs-min(obj.E_abs)+1;
%             if(min(min(z))<0)
%                 obj.D = -z(bounds(1):bounds(2),bounds(3):bounds(4));
%             else
%                 obj.D = z(bounds(1):bounds(2),bounds(3):bounds(4));
%             end
            obj.D_Nan = obj.D_Nan(Nlow:Nhigh,Elow:Ehigh);
            obj.D = obj.D_Nan;
            obj.D(isnan(obj.D)) = 0;
            
            obj.n_N = length(obj.N_abs);
            obj.n_E = length(obj.E_abs);
            obj.n_I = length(obj.D(:));
            
            obj.d_N = obj.d_N(Nlow:Nhigh,Elow:Ehigh);
            obj.d_E = obj.d_E(Nlow:Nhigh,Elow:Ehigh);
            obj.d_mag = sqrt(obj.d_N.^2+obj.d_E.^2);
            obj.d_mag(isnan(obj.d_mag)) = 0;
            
            obj.HtH = [];
            obj.HtH(1,1,:,:) = obj.d_N.*obj.d_N;
            obj.HtH(1,2,:,:) = obj.d_N.*obj.d_E;
            obj.HtH(2,1,:,:) = obj.HtH(1,2,:,:);
            obj.HtH(2,2,:,:) = obj.d_E.*obj.d_E;
            
            obj.badMap = isnan(obj.D_Nan);
        end
                
        function D = getDepth(obj,I, varargin)
            if(nargin < 3)
                coord = obj.getCoord(I);
            else
                north = I;
                east = varargin{:};
                coord = obj.getClosestPointAbs(north,east);
            end
            
            D = obj.D(coord(1),coord(2));
        end
        
        function coord = getCoord(obj,I)
            coord = [];
            for i = 1:1:length(I)
                N = ceil(I(i)/obj.n_E);
                E = mod((I(i)-1),(obj.n_E))+1;
                coord = [coord [N;E]];
            end
        end
        
        function I = getIndex(obj,N,E)
            I = (N-1)*obj.n_E + E;
        end
        
        function coord = getClosestPoint(obj,north,east)
            [~, Ntemp] = min(abs(north-obj.N));
            [~, Etemp] = min(abs(east-obj.E));
            coord = [Ntemp;Etemp];
        end
        
        function coord = getClosestPointAbs(obj,north,east)
            [~, Ntemp] = min(abs(north-obj.N_abs));
            [~, Etemp] = min(abs(east-obj.E_abs));
            coord = [Ntemp;Etemp];
        end
        
        function [slope, mag] = getSlope(obj,I, varargin)
            if(nargin < 3)
                coord = obj.getCoord(I);
            else
                north = I;
                east = varargin{:};
                coord = obj.getClosestPoint(north,east);
            end
            
            slope = [obj.d_N_smooth(coord(1),coord(2));obj.d_E_smooth(coord(1),coord(2))];
            mag = obj.d_mag(coord(1),coord(2));
        end
        
        function plot(obj,bounds)
            if(nargin<2)
                obj.PlotDEM(obj.N_abs,obj.E_abs,obj.D_Nan');
            else
                N = obj.N_abs(obj.N_abs>=bounds(1));
                E = obj.E_abs(obj.E_abs>=bounds(3));
                D = obj.D_Nan(obj.N_abs>=bounds(1),obj.E_abs>=bounds(3));
                D = D(N<=bounds(2),E<=bounds(4));
                E = E(E<=bounds(4));
                N = N(N<=bounds(2));
                
                obj.PlotDEM(N,E,D');
            end
            
        end
        
        
        
        function contour(obj,bounds,levels)
            if(nargin<3)
                levels = 20;
            end
            if(nargin<2 || isempty(bounds))
                contour(obj.N,obj.E,obj.D_Nan',levels)
            else
                N = obj.N(obj.N>=bounds(1));
                E = obj.E(obj.E>=bounds(3));
                D = obj.D_Nan(obj.N>=bounds(1),obj.E>=bounds(3));
                D = D(N<=bounds(2),E<=bounds(4));
                E = E(E<=bounds(4));
                N = N(N<=bounds(2));
                contour(N,E,D',levels)
            end
            
            view(90,-90);
            colormap(flipud(haxby(256)));
            xlabel('Eastings');
            xlabel('Northings');
            cb = colorbar;
            xlabel(cb,'Depth (m)');
            axis equal;
            set(cb, 'location','southoutside');
        end
            
            
        
        function obj = smoothGrad(obj,radius, type, plot)
            if(type == 1)
                Fil = fspecial('disk',radius);
            elseif(type == 2)
                Fil = fspecial('gaussian',ones(1,1)*ceil(3*radius),(radius));
            end
            D_smooth = imfilter(obj.D,Fil);
            [obj.d_E_smooth, obj.d_N_smooth] = gradient(D_smooth, obj.E, obj.N);

            if(plot)
                figure;
                a(1) = subplot(2,1,1);
                h(1) = pcolor(obj.E,obj.N,obj.d_N);
                title('Unsmooth North Gradient');
                axis equal
                a(2) = subplot(2,1,2);
                h(2) = pcolor(obj.E,obj.N,abs(obj.d_N_smooth));
                title(strcat('Smooth North Gradient, Radius=',num2str(radius)));
                axis equal;

                linkaxes(a);
                set(a,'CLim',[0 0.5]);
                set(h,'edgecolor','none')

                figure;
                b(1) = subplot(2,1,1);
                h2(1) = pcolor(obj.E,obj.N,abs(obj.d_E));
                title('Unsmooth East Gradient');
                axis equal;
                b(2) = subplot(2,1,2);
                h2(2) = pcolor(obj.E,obj.N,abs(obj.d_E_smooth));
                title(strcat('Smooth East Gradient, Radius=',num2str(radius)));
                axis equal;

                linkaxes(b);
                set(b,'CLim',[0 0.5]);
                set(h2,'edgecolor','none')
            end
            
            
        end
        
        function obj = calcVarTot(obj,radius, plot)
            obj.varD = 0*obj.D;
            for iE = 1:1:obj.n_E
                for iN = 1:1:obj.n_N
                    Dtemp = obj.D_Nan(iN,iE);
                    Etemp = obj.E(iE);
                    Ntemp = obj.N(iN);
                    if(~isnan(Dtemp))
                        Dvec = obj.D_Nan(abs(obj.N-Ntemp)<(radius),abs(obj.E-Etemp)<(radius))-Dtemp;
                        obj.varD(iN,iE) = var(Dvec(:));
                    else
                        obj.varD(iN,iE) = nan;
                    end
                end
            end
            

            var0 = 1-obj.varModMap;
            varInfo = (obj.varD-obj.varModMap);
            varInfo = varInfo.*(varInfo>0)+0.0000001*(varInfo<=0);
            obj.varTot = ((obj.varBeam + var0 + obj.varModMap)*obj.varD+(var0+obj.varBeam)*obj.varModMap)./varInfo;
            obj.alpha = ((obj.varBeam + var0 + obj.varModMap)*obj.varD+(var0+obj.varBeam)*obj.varModMap)./(varInfo*(obj.varBeam + var0 + obj.varModMap));
            
            if(plot)
                figure;
                h = pcolor(obj.E,obj.N, obj.varTot);
                set(gca,'CLim',[0 50]);
                set(h,'edgecolor','none');
                xlabel('Eastings (m)');
                ylabel('Northings (m)');
                axis equal;
                title(strcat('Total Measurement Variance, Beam Var=',num2str(obj.varBeam),', radius=',num2str(radius)));
            end
            
            
        end
         
        function obj = calcInfo(obj, plot)
            if(isempty(obj.varTot))
                obj.calcVarTot(12,0);
            else
                obj.infoN = obj.d_N.^2./obj.varTot;
                obj.infoE = obj.d_E.^2./obj.varTot;
                
                if(plot)
                    figure;
                    a(1) = subplot(2,1,1);
                    h(1) = pcolor(obj.E,obj.N,log(obj.infoN)); %Easy to see in logspace
                    xlabel('Eastings (m)');
                    ylabel('Northings (m)');
                    title('North Info');
                    axis equal;
                    a(2) = subplot(2,1,2);
                    h(2) = pcolor(obj.E,obj.N,log(obj.infoE));%Easy to see in logspace
                    xlabel('Eastings (m)');
                    ylabel('Northings (m)');
                    title('East Info');
                    axis equal;
                    set(h,'edgecolor','none');
                    linkaxes(a);
                    %set(a,'CLim',[0 0.01]);
                end
                
                
                if(~(isempty(obj.d_N_smooth)||isempty(obj.d_E_smooth)))
                    obj.infoN_smooth = obj.d_N_smooth.^2./obj.varTot;
                    obj.infoE_smooth = obj.d_E_smooth.^2./obj.varTot;
                    
                    if(plot)
                        figure;
                        a(1) = subplot(2,1,1);
                        h(1) = pcolor(obj.E,obj.N,log(obj.infoN_smooth));%Easy to see in logspace
                        xlabel('Eastings (m)');
                        ylabel('Northings (m)');
                        title('North Info Smooth');
                        axis equal;
                        
                        a(2) = subplot(2,1,2);
                        h(2) = pcolor(obj.E,obj.N,log(obj.infoE_smooth));%Easy to see in logspace
                        xlabel('Eastings (m)');
                        ylabel('Northings (m)');
                        title('East Info Smooth');
                        axis equal;
                        set(h,'edgecolor','none');
                        linkaxes(a);
                        %set(a,'CLim',[0 0.01]);
                    end
                end
            end
        end
        
        function obj = threshInfo(obj,thresh,plot)
           if(~(isempty(obj.infoN_smooth)||isempty(obj.infoE_smooth)))
               obj.infoN_thresh = abs(obj.infoN_smooth<thresh);
               obj.infoE_thresh = abs(obj.infoE_smooth<thresh);
           else
               obj.infoN_thresh = abs(obj.infoN<thresh);
               obj.infoE_thresh = abs(obj.infoE<thresh);
           end
           if(plot)
               figure;
               a(1) = subplot(2,1,1);
               h(1) = pcolor(obj.E,obj.N,obj.infoN_thresh);
               xlabel('Eastings (m)');
               ylabel('Northings (m)');
               title(strcat('North Info < ',num2str(thresh)));
               axis equal;
               
               a(2) = subplot(2,1,2);
               h(2) = pcolor(obj.E,obj.N,obj.infoE_thresh);
               xlabel('Eastings (m)');
               ylabel('Northings (m)');
               title(strcat('East Info < ',num2str(thresh)));
               axis equal;
               
               linkaxes(a);
               set(h,'edgecolor','none');
               
               figure;
               threshBoth = abs((obj.infoN_thresh + obj.infoE_thresh)>1);
               h2 = pcolor(obj.E,obj.N,threshBoth);
               xlabel('Eastings (m)');
               ylabel('Northings (m)');
               title(strcat('North and East Info both <',num2str(thresh)));
               axis equal;
               set(h2,'edgecolor','none');
           end
        end
        
        function obj = threshCov(obj,threshCov, processNoise, nBeams, plot)
            obj = obj.calcVarTot(sqrt(threshCov)*2,0); %Uniform over 2-sig
            obj = obj.smoothGrad(4,1,0); %smooth the gradient
            obj = obj.calcInfo(0); %Calculate the information
            thresh = processNoise/(nBeams*threshCov*(threshCov+processNoise));%threshold based on cov and process noise
            if(~(isempty(obj.infoN_smooth)||isempty(obj.infoE_smooth)))
               obj.infoN_thresh = abs(((obj.infoN_smooth<thresh)+isnan(obj.D_Nan))>0);
               obj.infoE_thresh = abs(((obj.infoE_smooth<thresh)+isnan(obj.D_Nan))>0);
           else
               obj.infoN_thresh = abs(((obj.infoN<thresh)+isnan(obj.D_Nan))>0);
               obj.infoE_thresh = abs(((obj.infoE<thresh)+isnan(obj.D_Nan))>0);
           end
           if(plot)
               figure;
               a(1) = subplot(2,1,1);
               h(1) = pcolor(obj.E,obj.N,obj.infoN_thresh);
               xlabel('Eastings (m)');
               ylabel('Northings (m)');
               title(strcat('North Info < ',num2str(thresh)));
               axis equal;
               
               a(2) = subplot(2,1,2);
               h(2) = pcolor(obj.E,obj.N,obj.infoE_thresh);
               xlabel('Eastings (m)');
               ylabel('Northings (m)');
               title(strcat('East Info < ',num2str(thresh)));
               axis equal;
               
               linkaxes(a);
               set(h,'edgecolor','none');
               
               figure;
               obj.threshBoth = abs((obj.infoN_thresh + obj.infoE_thresh)>1);
               h2 = pcolor(obj.E,obj.N,obj.threshBoth);
               xlabel('Eastings (m)');
               ylabel('Northings (m)');
               title(strcat('North and East Info both <',num2str(thresh)));
               axis equal;
               set(h2,'edgecolor','none');
           end
            
        end
        
        function [high,low] = getHighLowInfoPts(obj)
            allPts = [repmat(obj.N,1,obj.n_E);kron(obj.E,ones(1,obj.n_N));reshape(obj.threshBoth(:),1,obj.n_E*obj.n_N)];
            high = allPts(1:2,allPts(3,:)==0);
            low = allPts(1:2,allPts(3,:)~=0);
            
        end
        
        function Q = getInfoMatrix(obj,I,varargin)
            if(nargin < 3)
                coord = obj.getCoord(I);
            else
                north = I;
                east = varargin{:};
                coord = obj.getClosestPoint(north,east);
            end
            
            slope = [obj.d_N_smooth(coord(1),coord(2));obj.d_E_smooth(coord(1),coord(2))];
            Q = (1/obj.varTot(coord(1),coord(2)))*(slope*slope');
        end
        
        
        function map = outputStruct(obj)
            map = struct(obj);
            map = rmfield(map,'D_Nan');
            map = rmfield(map,'HtH');
            map = rmfield(map,'HtH_smoothed');
            map = rmfield(map,'map_name');
            map = rmfield(map,'d_N');
            map = rmfield(map,'d_E');
            map = rmfield(map,'d_mag');
            map = rmfield(map,'varD');
            map = rmfield(map,'varTot');
            map = rmfield(map,'d_N_smooth');
            map = rmfield(map,'d_E_smooth');
            map = rmfield(map,'infoN');
            map = rmfield(map,'infoN_smooth');
            map = rmfield(map,'infoN_thresh');
            map = rmfield(map,'infoE');
            map = rmfield(map,'infoE_smooth');
            map = rmfield(map,'infoE_thresh');
            
        end
        
    end
    
    methods (Static)
        function figHandle = PlotDEM(Ns,Es,Ds)
            %This plots a DEM in MBARI style - using the NED frame, lit and view from
            %above.
            
            figHandle = surf(Ns,Es,Ds,'LineStyle','none');
            axis equal
%             colormap((haxby(256)))
            cmocean('-deep');
            ylabel('East [m]')
            xlabel('North [m]')
            
            cb = colorbar;
            set(cb, 'location', 'eastoutside')
            xlabel(cb,'Elevation [m]')
            cb.FontSize = 16;
            cb.FontWeight = 'bold';
            % lighting flat
            %     lightangle(-10,20) for plots in ENU
            lightangle(-100,-20)
            set(figHandle, 'AmbientStrength',.5) %.2?
            view(90,-90)
            
            xlim([Ns(1) Ns(end)])
            ylim([Es(1) Es(end)])

            a = gca;
            a.FontSize = 16;
            a.FontWeight = 'bold';
            drawnow
            
        end
        
    end

end