%% Target Localization;
%% 2005-11-17;MaDengYong;

clc;
clear all;
close all;

Vxy=[0 200;200 0;100 100;100 200;100 0;200 200;0 0];
x190=0;y190=200;
x193=200;y193=0;
x195=100;y195=200;
x196=100;y196=0;
x197=200;y197=200;
XY_r=Vxy;
IP_set=[0 3 4 5 6 7 8];
IP_id=[1 2 4 5 6];
ID=[IP_set(IP_id(1));IP_set(IP_id(2));IP_set(IP_id(3));IP_set(IP_id(4));IP_set(IP_id(5))];
%%   197-6      195-4     190-1


%%              194-3   


%%   193-2      196-5      198-7

cp='sixth data\';

sr=['F:\2005_11_15\2005_11_15\afternoon_second_group\' cp];


path1=[sr 'AoA_tracking_values_paper\'];


load([path1 '19' num2str(0) '_thita_long']); 
thite190=thit_r;
% thite11=smooth(thite11);
clear thit_r;
% 
% % load([path1 '19' num2str(3) '_thita_long']); 
% thite193=thita;
% % thite11=smooth(thite11);
% clear thita;

load([path1 '19' num2str(5) '_thita_long']); 
thite195=thit_r;
% thite11=smooth(thite11);
clear thit_r;
% 
% load([path1 '19' num2str(6) '_thita_long']);
% thite196=thita;
% % thite22=smooth(thite22);
% clear thita;

load([path1 '19' num2str(7) '_thita_long']);
thite197=thit_r;
% thite22=smooth(thite22);
clear thit_r;

LP=[53:148];
lp1_n=length(LP);

xt=[];yt=[];

for lp1=1:lp1_n

    lp1
    
    bite190=90-thite190(LP(lp1));
    
    K190=tan(bite190*pi/180);
    fai190=0;
%     
%     bite193=90-thite193(LP(lp1));
%     
%     K193=tan(bite193*pi/180);
%     fai193=0;
    
    bite195=90-thite195(LP(lp1));
    K195=tan(bite195*pi/180);
    fai195=0;
    
    
%     bite196=90-thite196(LP(lp1));
%     K196=tan(bite196*pi/180);
%     fai196=0;
%     
    bite197=90-thite197(LP(lp1));
    K197=tan(bite197*pi/180);
    fai197=0;
    
    g190=sin(fai190)+K190*cos(fai190);
    h190=-cos(fai190)+K190*sin(fai190);
    
%     g193=sin(fai193)+K193*cos(fai193);
%     h193=-cos(fai193)+K193*sin(fai193);
%     
    g195=sin(fai195)+K195*cos(fai195);
    h195=-cos(fai195)+K195*sin(fai195);
    
%     g196=sin(fai196)+K196*cos(fai196);
%     h196=-cos(fai196)+K196*sin(fai196);
%     
    g197=sin(fai197)+K197*cos(fai197);
    h197=-cos(fai197)+K197*sin(fai197);
%     ;g197 h197;
% ;g197*x197+h197*y197
    H=[g190 h190;g195 h195;g197 h197];
    B=[g190*x190+h190*y190;g195*x195+h195*y195;g197*x197+h197*y197];
    
    XY=inv(H'*H)*H'*B
    
    xt=[xt;XY(1,:)];
    yt=[yt;XY(2,:)];
    
end

figure(1);
hold on;
plot_node6(XY_r,ID);

plot(xt,yt,'r- .','linewidth',2,'markersize',15);
plot(xt(1,1),yt(1,1),'ko','markersize',10);
plot(xt(lp1_n,1),yt(lp1_n,1),'ro','markersize',10);

hold off;
grid;
% set(gca,'xlim',[-50,250],'ylim',[-50,250],'linewidth',2,'fontsize',12,'fontweight','bold');
set(gca,'linewidth',2,'fontsize',12,'fontweight','bold');
xlabel('X Axis (meter) ','FontSize',12,'FontWeight','bold');
ylabel('Y Axis (meter) ','FontSize',12,'FontWeight','bold');
title1='Airplane Location Tracking  ';
% title([title1 '19' num2str(ID(1))  '-19' num2str(ID(2)) ' (' num2str(lp_l) '~' num2str(lp_r) ')'],'FontSize',12,'FontWeight','bold');
% saveas(gcf,[path2 'airplane_location_tracking_rotational_smooth' '_19' num2str(ID(1))   '_19' num2str(ID(2))]);
% saveas(gcf,[path2 'airplane_location_tracking_convention' '_19' num2str(ID(1))   '_19' num2str(ID(2))]);

% 
% figure(2);
% hold on;
% plot_node4(XY_i,ID);
% 
% plot(APi_xy(:,1),APi_xy(:,2),'r- .','linewidth',2,'markersize',15);
% plot(APi_xy(1,1),APi_xy(1,2),'ko','markersize',10);
% plot(APi_xy(size(APi_xy,1),1),APi_xy(size(APi_xy,1),2),'ro','markersize',10);
% 
% hold off;
% grid;
% % set(gca,'xlim',[-20,220],'ylim',[-20,220],'linewidth',2,'fontsize',12,'fontweight','bold');
% set(gca,'linewidth',2,'fontsize',12,'fontweight','bold');
% xlabel('X Axis (meter) ','FontSize',12,'FontWeight','bold');
% ylabel('Y Axis (meter) ','FontSize',12,'FontWeight','bold');
% title1='Airplane Location Tracking  ';
% title([title1 '19' num2str(ID(1))  '-19' num2str(ID(2)) ' (' num2str(lp_l) '~' num2str(lp_r) ')'],'FontSize',12,'FontWeight','bold');
% % saveas(gcf,[path2 'airplane_location_tracking_conventional_smooth' '_19' num2str(ID(1))   '_19' num2str(ID(2))]);
% saveas(gcf,[path2 'airplane_location_tracking_rotation' '_19' num2str(ID(1))   '_19' num2str(ID(2))]);