function  [xf_LLA_calculada, error_cometido]= Solve_problem_MC_Weighted(Rmax, t_w, tolerancia, database, lat_index_dat_file)
%% ---------------------------------------------------------------------------------
% Version:      24/09/2015    
% (c):          Vicente Calabuig Simón
%               vicasim@etsid.upv.es
%       
% 1.- Utilidad:
%
%       Resuelve la posición de la aeronave: latitud y longitud (NO ALTURA, va aparte)
%       por el método de mínimos cuadrados PONDERADOS.                                                      
%
% 2.- Parámetros de entrada:
%
%       Rmax       : Radio de búsqueda máximo    (Km)
%
%       t_w    : Intervalo de tiempo que quiere asignársele al proceso de escritura. 
%
%       tolerancia : Diferencia máxima que queremos admitir entre la posición real de la aeronave
%                    leída directamente desde X-plane, y la posición cálculada por el método de 
%                    mínimos cuadrados.
%
%       NOTA : Los errores intencionados y la tolerancia sirven para comprobar la convergencia del método, 
%              nº de iteraciones necesarias... etc
%
% 3.- Parámetros de salida:
%
%       xf_LLA_calculada = [lat_calc, lon_calc];
%
%       lat_calc : Latitud  calculada (º)
%       lon_calc : Longitud calculada (º)
%
% 4.- Ejemplo de uso:
%
%
%    Probado en : Madrid

%
%   1) Desde Xplane: Situar la aeronave en el lugar deseado
%   
%   2) [xf_LLA_calculada, error_cometido] = Solve_problem_MC_Weighted(200, 1, 0.01, '3_12_13_Database_Lat_Sorted.dat', 'LatIndex_3_12_13.dat')
%      [xf_LLA_calculada, error_cometido] = Solve_problem_MC_Weighted(200, 0.5, 0.01, '3_12_13_Lat_Sorted_WO_R_ID_COOR.dat', 'LatIndex_3_12_13_Lat_Sorted_WO_R_ID_COOR.dat')          Menor tiempo de escritura
%
%  NOTA : No suele converger con tolerancias iguales o inferiores a 0.001 
%
%  NOTA2 : X-plane debe estar ejecutándose, y debe tener configurado:  IP salida de datos     : 127.0.0.1
%                                                                     Port that we recieve on: 6500
%         
%% -------------------------------------------------------------------------------------------
tic
% Asumiendo la simplificacion de 'Tierra Plana' : 1 grado latitud  ~ 110 Km | 1 grado longitud ~ 111 Km     
latDeg2Km = 110;   lonDeg2Km = 111;          Km2LatDeg = 1/latDeg2Km;    Km2LonDeg = 1/lonDeg2Km; 

%Configuraciones de lectura - escritura referentes a X-Plane :
port_lectura    = 49000;
port_escritura  = 6500;

% 1) Lectura de la posición de la aeronave ¿Sentido..? 

[position] = readXPLANE(port_lectura);

ft2m = 0.3048;
lat_0 = position.latitude;          %Grados centesimales
lon_0 = position.longitude;         %Grados centesimales
alt_0 = position.altitude * ft2m;   %m

x0_LLA_original = [lat_0, lon_0, alt_0]; % Posición de la aeronave en sistema LLA

% Introducción de errores intencionados para provar la convergencia del método:
% Los errores deben seguir una distribución normal de media 0:
lat_error = random('norm', 0, 1);   lon_error = random('norm', 0, 1);   Errores_intencionados = [lat_error, lon_error, 0];
%Posicion inicial 'desviada' intencionadamente
x0_LLA = x0_LLA_original + Errores_intencionados; 

% 2) Obtención del conjunto de radioayudas de interés

% Radioayudas situadas a una distancia inferior a Rmax
[NavAid_Valid] = Search_NavAid(x0_LLA_original, Rmax, database, lat_index_dat_file) 

% Subconjunto filtrado de radioayudas anteriores (Cumplimiento de otros requisitos)
[NavAid_Valid_Filtered] = Filter_NavAid(NavAid_Valid, x0_LLA)

% 3) Separación en distintos arrays de todos los datos referentes al subconjunto de radioayudas filtradas:

[NAtype, NAlat, NAlon, NAalt,  NAfreq, NArange] = Get_NavAid_Data(NavAid_Valid_Filtered);
NavAid_Total_filtered = length(NAtype);


% 4) Obtención de las lecturas de las radioayudas (DME_distance de cada una de ellas)

% Proceso : Sintonización --> Lectura de 2 en 2 (Nav_1, Nav_2)

% Comprobamos si el numero de radioayudas filtradas es par o impar
if      (-1)^(NavAid_Total_filtered) == 1
            Par_impar = 'par';
elseif (-1)^(NavAid_Total_filtered)  == -1
            Par_impar = 'impar';
end

DME_distance = [];      NM2m  = 1852;
% Si es impar, prescindimos de la última radioayuda:  NECESITAMOS estrictamente un numero par de radioayudas
if  (strcmp(Par_impar,'impar'))
    NavAid_Valid_Filtered = NavAid_Valid_Filtered(1:(end-1));
    NavAid_Total_filtered = NavAid_Total_filtered - 1;      
    [NAtype, NAlat, NAlon, NAalt,  NAfreq, NArange] = Get_NavAid_Data(NavAid_Valid_Filtered);
    disp('Se ha eliminado la última radioayuda para que el número total sea par: ')
   NavAid_Valid_Filtered
end
% Una vez que el numero de radioayudas filtradas es par: 

if (NavAid_Total_filtered < 4)
    error('myApp:argChk', 'No se han detectado al menos 4 radioayudas con las que operar');
end

j = 1;    
for i = 1 : (NavAid_Total_filtered/2)
    %realizamos las lecturas por parejas, de 2 en 2
    WriteFrequencyNav1Nav2(NAfreq(j), NAfreq(j + 1), t_w);          % Sintonización
    r1 = Read_DME_distance(1);        r2 = Read_DME_distance(2);    % Lectura
    [DME_distance]= [DME_distance; r1*NM2m; r2*NM2m]; %m
    % Siguiente pareja:
    j = j + 2;
end

% Precision de cada radioayuda
alphas = []; sigmas = [];          s1_sis = 0.05*NM2m;      s2_sis = 0.05*NM2m;
for i = 1: (NavAid_Total_filtered/2)
    k = NavAid_Total_filtered -(i-1);
    % Calculamos alpha, sigma, in_out validez de la pareja: (Sin PLOT) --> Matrid de pesos
    [alpha,sigma,in_out, validez] = DME_DME_Upgrade_Area (lat_0, lon_0,  NAlat(i), NAlon(i), NArange(i),  NAlat(k), NAlon(k), NArange(k), 0, 0);
    alphas(i) = alpha;        alphas(k) = alpha;      
    sigma = sigma *NM2m;    
    sigmas(i) = sigma;        sigmas(k) = sigma; 
    
    % MATRIZ DE PESOS asociada
    W (i, i) = 1/((sigma)^2);    
    W (k, k) = 1/((sigma)^2);
end
    

% DME_distance es la distancia absoluta: necesitamos la proyección horizontal:
Horizontal_distance = [];
for i = 1 : NavAid_Total_filtered
    Nav_alt_i               = NAalt(i) * ft2m;
    Diferencia_alturas(i)   = Nav_alt_i - alt_0;
    h_distance              = sqrt( (DME_distance(i) )^2 - ( Diferencia_alturas(i) )^2 );
    Horizontal_distance     = [Horizontal_distance; h_distance];
end
    
% 5) RESOLUCIÓN POR MÍNIMOS CUADRADOS PONDERADOS :    
    fprintf('Resolviendo por mínimos cuadrados ponderados...\n'); disp(' ');
% Elegimos una radiobaliza de referencia ¿CRITERIO? -> Ninguno de momento -> La primera de la lista
    
 % Conversión de LLA a ECEF
 
 x0_ECEF = lla2ecef(x0_LLA); 
 
for i = 1 : NavAid_Total_filtered
    Nav_Aid_LLA  (i, 1:3) = [NAlat(i), NAlon(i), NAalt(i)*ft2m];
    Nav_Aid_ECEF (i, 1:3) = lla2ecef_2( Nav_Aid_LLA(i, 1:3) );
end

% Conversión de ECEF a NEU: 

orgece = Nav_Aid_ECEF(1, 1:3);            % Radiobaliza de referencia en ECEF (1ª de la lista de radioayudas)
orgllh = Nav_Aid_LLA (1, 1:3) * (pi/180); % Radiobaliza de referencia en LLA ? 
    
for i = 1 : NavAid_Total_filtered
    
Nav_Aid_NEU (i, 1:3) = ECEF2NEU (Nav_Aid_ECEF(i,1:3)', orgece', orgllh');

end  
x0_NEU = ECEF2NEU (x0_ECEF', orgece', orgllh');
    
% Proceso ITERATIVO de resolución en el marco NEU de la radiobaliza de referencia:   
    
convergencia            = 0; 
numero_de_iteraciones   = 0;     
    
while (1 - convergencia) 
    numero_de_iteraciones = numero_de_iteraciones + 1 ; 
% 1) Inicialización:
    x = x0_NEU(1:2)';      %Posicion actual como la ultima solucion obtenida (x0_NEU va a actualizarse al final del bucle)
    
% 2) Previsión de la distancia a las radioayudas:
    
    for j = 1 : NavAid_Total_filtered
    
        rho_teo(j) = sqrt( (x(1)-Nav_Aid_NEU(j, 1) )^2 + ( x(2)-Nav_Aid_NEU (j, 2) )^2); 

       % 3 Cálculo del error en la medida:  
    
        delta_rho(j) = Horizontal_distance(j) - rho_teo(j); 

    % 4) Formmulación del problema de Mínimos Cuadrados:

        for k = 1:2
             G (j, k) = ( x(k) - Nav_Aid_NEU (j, k) ) / rho_teo(j);
        end  
    end 

%delta_x = (inv((G'*G))*G')*delta_rho';
delta_x  =  inv(G'*W*G)*G'*W*delta_rho';

% 5) Actualización de la estimación de la posición inicial:

x0_NEU(1:2) = (x0_NEU(1:2) + delta_x); 

% 6) Comprobacion de la convergencia de la solución:

        if sqrt( delta_x(1)^2 + delta_x(2)^2 ) < tolerancia 
           convergencia = 1;
        end
        
        if numero_de_iteraciones > 100000
            fprintf('No se ha convergido después de %g iteraciones\n', numero_de_iteraciones);
            convergencia = 1;
        end
end
% 7) Conversión  NEU -> ECEF -> LLA

%NEU -> ECEF

xf_ECEF = NEU2ECEF (x0_NEU, orgece', orgllh'); 

%ECEF -> LLA

xf_LLA = ECEF2LLA (xf_ECEF); 

% Lat y lon a grados centesimales:
xf_LLA(1:2) = xf_LLA(1:2) * (180/pi); 
xf_LLA_calculada = xf_LLA(1:2)'; 
% Error cometido:
x0 = x0_LLA_original(1); y0 = x0_LLA_original(2); xf = xf_LLA_calculada(1); yf = xf_LLA_calculada(2);
error_cometido = sqrt( (latDeg2Km *(xf-x0))^2 + (lonDeg2Km*(yf-y0))^2); %Km
error_cometido = error_cometido * 1/1.852;    % NM (<5 ?)

%% AVISO SI EL ERROR ES MAYOR QUE 5 NM

if error_cometido < 5
    fprintf('El sistema de navegación funciona correctamente: SNE = %g NM\n', error_cometido)
else
    fprintf('El sistema de navegación NO funciona correctamente: SNE = %g NM\n ', error_cometido)
end

%% INFO POR PANTALLA..
format short g
x0_LLA_original    
x0_LLA    

xf_LLA_calculada = xf_LLA(1:2)';    
numero_de_iteraciones   

toc