function X = FangAlgorithm(MSP, R)
%  本程序實現無線定位中的FANG算法
%  FANGALGORITHM
%    參數說明：
%       MSP：移動臺相對位置；
%       R：  小區半徑；
%       X：  輸出移動臺的估計位置。
%  Also see: FangAlgorithm.

%  Designed by 李金倫, SWJTU, 2004.12.15 

%  輸入參數檢測:
    if  nargout>1,
        error('Too many output arguments.');
    end
    if nargin~=2,
        error('Wrong number of input arguments.');
    end
        flag = size(MSP);
    if flag(1)~=1 | flag(2)~=2,
        error('Wrong position vector!');
    end

%  輸入：
%     R = 5000; %單位：km
%     BS = [0, sqrt(3)*R, 0.5*sqrt(3)*R;
%           0, 0,         1.5*R];
%     MSP = [0.5, 0.8];
    
%  初始參數：
    MS  = R*MSP;
    BS  = [0, sqrt(3)*R, 0.5*sqrt(3)*R;
          0, 0,         1.5*R];
    c   = 3*10^8; %單位: m/s
    DEV = 3/(c*c);
    %
    R1 = sqrt(MS(1)*MS(1) + MS(2)*MS(2));
    R2 = sqrt((BS(1,2) - MS(1))*(BS(1,2) - MS(1)) + (BS(2,2) - MS(2))*(BS(2,2) - MS(2)));
    R3 = sqrt((BS(1,3) - MS(1))*(BS(1,3) - MS(1)) + (BS(2,3) - MS(2))*(BS(2,3) - MS(2)));
    % noise
    n1 = c*sqrt(DEV)*randn(1);
    n2 = c*sqrt(DEV)*randn(1);
    %
    R21 = R2 - R1 + n1;
    R31 = R3 - R1 + n2;
    %
    g = ((R31*BS(1,2))/R21 - BS(1,3))/BS(2,3);
    h = (BS(1,3)*BS(1,3) + BS(2,3)*BS(2,3) - R31*R31 + R31*R21*(1 - (BS(1,2)/R21)*(BS(1,2)/R21)))/(2*BS(2,3));
    d = -((1 - (BS(1,2)/R21)*(BS(1,2)/R21)) + g*g);
    e = BS(1,2)*(1 - (BS(1,2)/R21)*(BS(1,2)/R21)) - 2*g*h;
    f = (R21*R21/4)*(1-(BS(1,2)/R21)*(BS(1,2)/R21))*(1-(BS(1,2)/R21)*(BS(1,2)/R21)) - h*h;
    
%  輸出：
    EMSX = (-e - sqrt(e*e - 4*d*f))/(2*d);
    EMSY = g*EMSX + h;
    EMS = [EMSX, EMSY];
    if nargout == 1,
        X = EMS;
    else
        disp(EMS);
    end