%tgt_data = readtable('gmc_table_4d2258.txt');
%tgt_truth = readtable('air_data');
%tgt_lalonidl = readtable('latlonidl.dat');


[file,location] = uigetfile('C:\Progetti_MATLAB\Search&Track\Data\*.dat');


tgt_gmtt = readtable(fullfile(location,file));

size_tgt = size(tgt_gmtt);

lattgtpath= [];
lontgtpath= [];

%tgt_data.az= rad2deg(tgt_data.az);
%tgt_data.el= rad2deg(tgt_data.el);


heading_own = 205;
wander_angle = -0.8;

wgs84 = wgs84Ellipsoid('meter');

%%%%%%%%%PLOT SCAN%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        OwnPosHeading = wrapTo360(rad2deg(tgt_gmtt.Heading(1)));

        %wrapTo180(heading_own-tgt_data.az(k))

        OwnPosLatitude = rad2deg(tgt_gmtt.Latitude(1));
        OwnPosLongitude = rad2deg(tgt_gmtt.Longitude(1));
        
        currentaz = heading_own;
        maxrng= max(tgt_gmtt.Range);
        beamw= 4;

        scanregion   = [-60, 60, 0, 30]; 
        azscanspan   = diff(scanregion(1:2));
        numazscan    = ceil(azscanspan/beamw);
        azscanangles = linspace(scanregion(1),scanregion(2),numazscan);

        angmin = azscanangles(1)-beamw/2;
        angmax = azscanangles(end)+beamw/2;

        sareax = [0 maxrng*sind(-[angmax -azscanangles angmin]+OwnPosHeading) 0];
        sareay = [0 maxrng*cosd(-[angmax -azscanangles angmin]+OwnPosHeading) 0];
         
        sizeaz = size(sareax);
         
        [x1,y1,z1] = enu2ecef(sareax,zeros(1,sizeaz(2)),zeros(1,sizeaz(2)),OwnPosLatitude,OwnPosLongitude,0.0,wgs84);
        [~,lonP1,~] = ecef2gno(x1,y1,z1);
         
        [x2,y2,z2] = enu2ecef(zeros(1,sizeaz(2)),sareay,zeros(1,sizeaz(2)),OwnPosLatitude,OwnPosLongitude,0.0,wgs84);
        [latP2,~,~] = ecef2gno(x2,y2,z2);

        figure;
        ploth.own = geoplot(latP2,lonP1,'blue-',LineWidth=2);
        
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Set the basemap
geobasemap('topographic'); % Choose your preferred basemap
title('MAP Tracking');

OwnPosHeading = wrapTo360(rad2deg(tgt_gmtt.Heading(1)));

tgt_east= - tgt_gmtt.Range(1) * sind(wrapTo180(rad2deg(tgt_gmtt.Azimuth(1))+wander_angle));
tgt_north= tgt_gmtt.Range(1) * cosd(wrapTo180(rad2deg(tgt_gmtt.Azimuth(1))+wander_angle));


 OwnPosLatitude = rad2deg(tgt_gmtt.Latitude(1));
 OwnPosLongitude = rad2deg(tgt_gmtt.Longitude(1));

 [x1,y1,z1] = enu2ecef(tgt_east,tgt_north,200,OwnPosLatitude,OwnPosLongitude,2000,wgs84);
 [lattgt,lontgt,~] = ecef2gno(x1,y1,z1);


 hold on;

 ploth.marker{1} = geoplot(lattgt(1),lontgt(1),"ow","MarkerSize",10,"MarkerFaceColor","r");

step=10;
for k=1:step:size_tgt(1)

        k
        tic
        %heading_tgt= rad2deg(atan2(-tgt_data.vy(k),tgt_data.vx(k)));%tgt_heading(XR,XAZ,XEL);
        OwnPosHeading = wrapTo360(rad2deg(tgt_gmtt.Heading(k)));

        tgt_east= - tgt_gmtt.Range(k) * sind(wrapTo180(rad2deg(tgt_gmtt.Azimuth(k))+wander_angle));
        tgt_north= tgt_gmtt.Range(k) * cosd(wrapTo180(rad2deg(tgt_gmtt.Azimuth(k))+wander_angle));

        %wrapTo180(heading_own-tgt_data.az(k))

        OwnPosLatitude = rad2deg(tgt_gmtt.Latitude(k));
        OwnPosLongitude = rad2deg(tgt_gmtt.Longitude(k));

        [x1,y1,z1] = enu2ecef(tgt_east,tgt_north,200,OwnPosLatitude,OwnPosLongitude,800,wgs84);
        [lattgt,lontgt,~] = ecef2gno(x1,y1,z1);
        
        
        set(ploth.marker{1},"LatitudeData",lattgt,"LongitudeData",lontgt);

        lattgtpath = cat(1,lattgtpath,lattgt);
        lontgtpath = cat(1,lontgtpath,lontgt);

        geoplot(lattgtpath, lontgtpath, 'magenta', LineWidth=2)

        %Aggiornamento Ownship ------------------------

        sareax = [0 maxrng*sind(-[angmax -azscanangles angmin]+OwnPosHeading) 0];
        sareay = [0 maxrng*cosd(-[angmax -azscanangles angmin]+OwnPosHeading) 0];
         
        sizeaz = size(sareax);
         
        [x1,y1,z1] = enu2ecef(sareax,zeros(1,sizeaz(2)),zeros(1,sizeaz(2)),OwnPosLatitude,OwnPosLongitude,0.0,wgs84);
        [~,lonP1,~] = ecef2gno(x1,y1,z1);
         
        [x2,y2,z2] = enu2ecef(zeros(1,sizeaz(2)),sareay,zeros(1,sizeaz(2)),OwnPosLatitude,OwnPosLongitude,0.0,wgs84);
        [latP2,~,~] = ecef2gno(x2,y2,z2);
         
        %sareax = lonP1;
        %sareay = latP2;

        set(ploth.own,"LatitudeData",latP2,"LongitudeData",lonP1);

        %-----------------------------------------------
        
        
        dt.DataIndex = k;
    
        drawnow
        TimeSearch = 0.1;
        TimeElab=toc;
        PauseVal=max(TimeSearch-TimeElab,0);
        pause(PauseVal)

end



aa='fine'