function solve
% Function SOLVE is part of program HAVOC, a least squares
% traverse adjustment program.  Function SOLVE forms and solves
% the normal equations for the adjustment.

%============================================================================
% Function:  solve
%
% Author:
%  Rod Deakin, 
%  School of Mathematical & Geospatial Science, 
%  RMIT University,
%  GPO Box 2476V, MELBOURNE VIC 3001
%  AUSTRALIA
%  email: rod.deakin@rmit.edu.au
%
% Date:
%  Version 1.0   3 August   2006
%
% Functions required:
%  join
%
% Remarks:
%  This function forms and solves the normal equations for a least squares 
%  traverse network.
%
% Variables
%  aik,bik   direction coefficients of the line AT(i) to TO(k)
%  cik,dik   distance  coefficients of the line AT(i) to TO(k)
%  ail,bil   direction coefficients of the line AT(i) to LEFT(l)
%  cil,dil   distance  coefficients of the line AT(i) to LEFT(l)
%  air,bir   direction coefficients of the line AT(i) to RIGHT(r)
%  cir,dir   distance  coefficients of the line AT(i) to RIGHT(r)
%  dei,dni   coefficient of station AT(i) in observation equation
%  dek,dnk   coefficient of station TO(k) in observation equation
%  ei,ni     east and north coord of station AT(i)
%  ek,nk     east and north coord of station TO(k)
%  el,nl     east and north coord of station LEFT(l)
%  er,nr     east and north coord of station RIGHT(r)
%  f         numeric terms of observation equation
%  g         numeric term of constraint equation
%  i,k       index for stations AT(i) and TO(k) in vector Line
%  ide,idn   index for coeff's de,dn of station "i" in vector Line/matrix N1
%  index     index for orientation const/coeff in vector Z/Line
%  j,m       loop counters
%  kde,kdn   index for coeff's de,dn of station "k" in vector Line/matrix N1
%  kk        index for coeff's of constraints in matrix N1
%  last      serial number of last station AT
%  l,r       index of LEFT(l) and RIGHT(r) stations in angle constraint
%  lde,ldn   index for coeff's de,dn of station LEFT  in matrix N1
%  phi       bearing of line (degrees)
%  phil,phir bearing of left and right line defining an angle
%  rde,rdn   index for coeff's de,dn of station RIGHT in matrix N1
%  rhodeg    degree to radian conversion factor.  rhodeg = 57.29577951...
%  s         distance
%
%  A         partitioned square matrix of order u+c containing the normal
%            equations (multiplied by -1) and the coefficients of the
%            constraint equations.  See explanation below.
%  Ainv      inverse of A
%  b         u+c,1 vector containing the numeric terms of the normal
%            equations (multiplied by -1) and the numeric terms of the
%            constraint equations.  The solution of the system of equations
%            for the adjustment is gixen by x1 = inv(A)*b
%  Line      vector of coefficients of a direction or a distance observation
%            equation
%  N1        partitioned matrix containing the normal equations N, vector of
%            numeric terms of the normal equations t, coefficients of 
%            constraint equations C and the transpose of C and the vector of
%            numeric terms of the constraint equations g.
%  Qxx       square matrix of order u containing the estimate of the variance 
%            matrix for the adjustment.  In the case of no constraints,
%            Qxx is the inverse of the normal equations.  Qxx is the u,u
%            upper-left portion of Ainv multiplied by -1.
%  Z         vector of approximate orientation constants
%  x1        u+c,1 solution vector of the system of normal equations.
%  X         u,1 vector of corrections to approx coords and orientation
%            constants.  X is equal to the first u terms of x1.
%
%  fixed     no. of fixed stations
%  floating  no. of floating stations
%  dirns     no. of observed directions
%  dists     no. of observed distances
%  cbrgs     no. of constrained bearings
%  cdists    no. of constrained distances
%  cangles   no. of constrained angles
%  orients   no. of orientation constants
%  n         total number of observations (n = dirns + dists)
%  u         no. of unknowns (u = 2*floating + orients)
%  c         no. of constraints
%
%  NOTE:  If there are no fixed stations then it is assumed that the
%  adjustment is a free net adjustment.  Any constraints will be ignored
%  and only inner constraints allowed.  These inner constraints will be
%  determined and added automatically.
%
%============================================================================

global STATION OBS JOB X Z VFACT Qxx               

fixed    = JOB(1).fixed;       % number of fixed stations
floating = JOB(1).floating;    % number of floating stations
dirns    = JOB(1).dirns;       % number of directions
dists    = JOB(1).dists;       % number of distances
cbrgs    = JOB(1).cbrgs;       % number of constrained bearings
cdists   = JOB(1).cdists;      % number of constrained distances
cangles  = JOB(1).cangles;     % number of constrained angles
orients  = JOB(1).orients;     % number of orientation constants
n        = JOB(1).n;           % number of observation equations
u        = JOB(1).u;           % number of unknowns
c        = JOB(1).c;           % number of constraint equations

% determine the number of inner constraints if free net, 
% i.e., no fixed stations
if fixed == 0
    % there will be THREE inner constraints if there are:
    % (i) distances only (trilateration) or
    % (ii) directions and distances (triangulation & trilateration)
    c = 3;
    if dists == 0
        % there will be FOUR inner constraints if there are
        % directions only (triangulation)
        c = 4;
    end    
end    

rhodeg = 180/pi;
%
% form normal equations
while 1
    % clear matrix N1 which holds the normal equations, the constraint equations 
    % and the vector of numeric terms
    N1 = zeros(u+c+1,u+c+1);
    %
    % clear an array to contain the approximate orientation constants
    Z = zeros(orients,1);

    last  = 0;
    index = 0;
    ftf   = 0;
    %
	for m = 1:(dirns+dists)
        %----------------------------------------
        % form a line of the coefficient matrix B
        %----------------------------------------
        %
        % find the east and north coords of station AT (ei,ni)
        % and station TO (ek,nk) and compute the bearing, distance
        % and direction/distance coefficients
        for j = 1:(fixed+floating)
            if (OBS(m).at == STATION(j).serial)
                ei = STATION(j).east;
                ni = STATION(j).north;
                i  = j;
            end  
            if (OBS(m).to == STATION(j).serial)
                ek = STATION(j).east;
                nk = STATION(j).north;
                k  = j; 
            end  
        end  %-- end of j-loop
        % calculate bearing, distance and direction and distance coefficients
        [phi,s,aik,bik,cik,dik]=join(ei,ni,ek,nk);
        %
        % calculate the elements of the coefficient matrix
        if (OBS(m).code == 4)                  % observation = distance
            dek = dik/(OBS(m).sigma*100.0);      % east  coeff for station k  
            dnk = cik/(OBS(m).sigma*100.0);      % north coeff for station k
            dei = -dek;                          % east  coeff for station i
            dni = -dnk;                          % north coeff for station i
            f   = (OBS(m).val-s)/OBS(m).sigma;   % numeric term in metres divided by st.dev.
        end  
        if (OBS(m).code == 3)                   % observation = direction
            if (OBS(m).at ~= last)
                last = OBS(m).at;                   % new station
                index = index+1;                    % increment index for orientations
                Z(index,1) = phi;                   % orientation = bearing
            end
            dek = bik/OBS(m).sigma;               % east  coeff for station k
            dnk = aik/OBS(m).sigma;               % north coeff for station k
            dei = -dek;                           % east  coeff for station i
            dni = -dnk;                           % north coeff for station i
            f   = OBS(m).val + Z(index,1) - phi;  % numeric term in degrees
            if (f > 180.0 || f < -180.0)
                if (f < 0.0)
                    f = f + 360.0;  
                end  
                if (f > 0.0)
                    f = f - 360.0;  
                end  
            end  
            f = f*3600.0/OBS(m).sigma;  % numeric term in seconds of arc divided by st.dev.
        end
        %
        % clear the vector Line which will hold 1 observation equation + numeric
        % term
        Line = zeros(u+c+1,1);
        %
        % set indexes for array locations of floating stations
        % i and k are array locations of stations AT and TO in the
        % list of stations (fixed + floating stations).
        % If stations AT and TO are floating then subtracting the 
        % number of fixed stations will give i and k as locations
        % in the list of floating stations.
        i = i - fixed;
        k = k - fixed;
        %
        % set indexes for array locations for the coefficients de and dn
        ide = 2*i-1;
        idn = 2*i;
        kde = 2*k-1;
        kdn = 2*k;
        %
        % fill the vector Line with appropriate coefficients depending upon
        % the STATUS of each observation
        if (OBS(m).status == 1) % AT = i = floating, TO = k = floating
            Line(ide,1) = dei;  
            Line(idn,1) = dni;  
            Line(kde,1) = dek;  
            Line(kdn,1) = dnk;  
        end  
        if (OBS(m).status == 2) % AT = i = fixed, TO = k = floating
            Line(kde,1) = dek;  
            Line(kdn,1) = dnk;  
        end  
        if (OBS(m).status == 3) % AT = i = floating, TO = k = fixed
            Line(ide,1) = dei;  
            Line(idn,1) = dni;  
        end  
        %
        % set the coefficient of the orientation constants
        if (OBS(m).code == 3)
            Line(2*floating+index) = -1.0/OBS(m).sigma;  
        end  
        %
        % store the numeric term
        Line(u+c+1,1) = f;
        %
        % calculate the contribution of each residual equation to
        % the normal equations (N) and the vector of numeric terms (t).
        % Both of these matrices are contained within N1.  Note that
        % substracting the contribution causes N1 to contain -N and -t.
        for i = 1:u
            for j = 1:u+c+1
                N1(i,j) = N1(i,j)-(Line(i,1)*Line(j,1));  
            end  
        end
        ftf = ftf + Line(u+c+1,1)^2;
    end  %-- end of m-loop for directions and distances
    %
    %---------------------
    % Constraint Equations
    %---------------------
    if fixed == 0
        % ADD INNER CONSTRAINTS for FREE NET ADJUSTMENT
        % add position constraints (2 equations) and rotation
        % constraint (1 equation)
        for m = 1:floating
            ei = STATION(m).east;  
            ni = STATION(m).north;  
            % set indexes for array locations for coefficients de and dn in matrix N1
            ide = 2*m-1;
            idn = 2*m;
            % fill out the elements of N1
            N1(u+1,ide) = 1;
            N1(u+2,idn) = 1;
            N1(u+3,ide) = ni;
            N1(u+3,idn) = -ei;
            N1(ide,u+1) = 1;
            N1(idn,u+2) = 1;
            N1(ide,u+3) = ni;
            N1(idn,u+3) = -ei;
        end    
        if dists == 0
            % add scale constraint (1 equation)
            for m = 1:floating
                ei = STATION(m).east;  
                ni = STATION(m).north;  
                % set indexes for array locations for coefficients de and dn in matrix N1
                ide = 2*m-1;
                idn = 2*m;
                % fill out the elements of N1
                N1(u+4,ide) = ei;
                N1(u+4,idn) = ni;
                N1(ide,u+4) = ei;
                N1(idn,u+4) = ni;
            end    
        end    
    else
        % ADD CONSTRAINT EQUATIONS to adjustment
        kk = u+1;
        for m = (dirns+dists+1):(dirns+dists+c)
            if (OBS(m).code ~= 7)
                % do constrained bearings (code = 5), then 
                % constrained distances (code = 6)
                % locate array location for station AT and TO
                for j = 1:fixed+floating
                    if (OBS(m).at == STATION(j).serial)
                        ei = STATION(j).east;  
                        ni = STATION(j).north;  
                        i  = j;
                    end  
                    if (OBS(m).to == STATION(j).serial)
                        ek = STATION(j).east;  
                        nk = STATION(j).north;  
                        k  = j;
                    end  
                end  
                if (OBS(m).code == 5)           % constrained bearing
                    phi = OBS(m).val/rhodeg;    % bearing in radians 
                    dek = -1.0;
                    dnk = tan(phi);
                    dei = -dek;
                    dni = -dnk;
                    g   = ((ek-ei) - (nk-ni)*dnk)*100.0;  % numeric term in cm's
                end  
                if (OBS(m).code == 6)           % constrained distance
                    [phi,s,aik,bik,cik,dik]=join(ei,ni,ek,nk);
                    dek = dik;
                    dnk = cik;
                    dei = -dek;
                    dni = -dnk;
                    g   = (OBS(m).val - s)*100.0;         % numeric term in cm's
                end  
                %
                % set indexes for array locations for coefficients de and dn in matrix N1
                i   = i - fixed;
                k   = k - fixed;
                ide = 2*i-1;
                idn = 2*i;
                kde = 2*k-1;
                kdn = 2*k;
                %
                % fill row kk and column kk of matrix N1 with appropriate
                % coefficients depending upon the status of each observation
                if (OBS(m).status == 1)  % AT = i = floating, TO = k = floating
                    N1(kk,ide) = dei;
                    N1(kk,idn) = dni;
                    N1(kk,kde) = dek;
                    N1(kk,kdn) = dnk;
                    N1(ide,kk) = dei;
                    N1(idn,kk) = dni;
                    N1(kde,kk) = dek;
                    N1(kdn,kk) = dnk;
                end  
                if (OBS(m).status == 2)  % AT = i = fixed, TO = k = floating
                    N1(kk,kde) = dek;
                    N1(kk,kdn) = dnk;
                    N1(kde,kk) = dek;
                    N1(kdn,kk) = dnk;
                end  
                if (OBS(m).status == 3)  % AT = i = floating, TO = k = fixed
                    N1(kk,ide) = dei;
                    N1(kk,idn) = dni;
                    N1(ide,kk) = dei;
                    N1(idn,kk) = dni;
                end  
                %
                % store numeric term
                N1(kk,u+c+1) = g;
                kk = kk+1;
            end  % end loop for code ~= 7  
            if (OBS(m).code == 7)
                % do constrained angles (code = 7)
                % locate the array locations of stations AT, LEFT and RIGHT
                for j = 1:fixed+floating
                    if (OBS(m).at == STATION(j).serial)
                        ei = STATION(j).east;  
                        ni = STATION(j).north;  
                        i  = j;
                    end  
                    if (OBS(m).left == STATION(j).serial)
                        el = STATION(j).east;  
                        nl = STATION(j).north;  
                        l  = j;
                    end  
                    if (OBS(m).right == STATION(j).serial)
                        er = STATION(j).east;  
                        nr = STATION(j).north;  
                        r  = j;
                    end  
                end  
                %
                % calculate bearing, distance and direction coefficients
                [phil,s,ail,bil,cil,dil]=join(ei,ni,el,nl);
                [phir,s,air,bir,cir,dir]=join(ei,ni,er,nr);
                dei = -(bir - bil);
                dni = -(air - ail);
                der = bir;
                dnr = air;
                del = -bil;
                dnl = -ail;
                g   = (OBS(m).val - (phir - phil))*3600.0;  % numeric terms in seconds
                %
                % set indexes for array locations for coefficients de and dn in matrix N1
                i   = i - fixed;
                l   = l - fixed;
                r   = r - fixed;
                ide = 2*i-1;
                idn = 2*i;
                lde = 2*l-1;
                ldn = 2*l;
                rde = 2*r-1;
                rdn = 2*r;
                %
                % fill row kk and column kk of matrix N1 with appropriate
                % coefficients depending upon the status of each observation
                if (OBS(m).status == 1)  % AT(float) LEFT(float) RIGHT(float)
                    N1(kk,ide) = dei;  
                    N1(kk,idn) = dni;  
                    N1(kk,lde) = del;  
                    N1(kk,ldn) = dnl;  
                    N1(kk,rde) = der;  
                    N1(kk,rdn) = dnr;  
                    N1(ide,kk) = dei;  
                    N1(idn,kk) = dni;  
                    N1(lde,kk) = del;  
                    N1(ldn,kk) = dnl;  
                    N1(rde,kk) = der;  
                    N1(rdn,kk) = dnr;  
                end  
                if (OBS(m).status == 2)  % AT(fixed) LEFT(float) RIGHT(float)
                    N1(kk,lde) = del;  
                    N1(kk,ldn) = dnl;  
                    N1(kk,rde) = der;  
                    N1(kk,rdn) = dnr;  
                    N1(lde,kk) = del;  
                    N1(ldn,kk) = dnl;  
                    N1(rde,kk) = der;  
                    N1(rdn,kk) = dnr;  
                end  
                if (OBS(m).status == 3)  % AT(float) LEFT(fixed) RIGHT(float)
                    N1(kk,ide) = dei;  
                    N1(kk,idn) = dni;  
                    N1(kk,rde) = der;  
                    N1(kk,rdn) = dnr;  
                    N1(ide,kk) = dei;  
                    N1(idn,kk) = dni;  
                    N1(rde,kk) = der;  
                    N1(rdn,kk) = dnr;  
                end  
                if (OBS(m).status == 4)  % AT(fixed) LEFT(fixed) RIGHT(float)
                    N1(kk,rde) = der;  
                    N1(kk,rdn) = dnr;  
                    N1(rde,kk) = der;  
                    N1(rdn,kk) = dnr;  
                end  
                if (OBS(m).status == 5)  % AT(float) LEFT(float) RIGHT(fixed)
                    N1(kk,ide) = dei;  
                    N1(kk,idn) = dni;  
                    N1(kk,lde) = del;  
                    N1(kk,ldn) = dnl;  
                    N1(ide,kk) = dei;  
                    N1(idn,kk) = dni;  
                    N1(lde,kk) = del;  
                    N1(ldn,kk) = dnl;  
                end  
                if (OBS(m).status == 6)  % AT(fixed) LEFT(float) RIGHT(fixed)
                    N1(kk,lde) = del;  
                    N1(kk,ldn) = dnl;  
                    N1(lde,kk) = del;  
                    N1(ldn,kk) = dnl;  
                end  
                if (OBS(m).status == 7)  % AT(float) LEFT(fixed) RIGHT(fixed)
                    N1(kk,ide) = dei;  
                    N1(kk,idn) = dni;  
                    N1(ide,kk) = dei;  
                    N1(idn,kk) = dni;  
                end
                %
                % store numeric term
                N1(kk,u+c+1) = g;
            end  %-- end loop for code == 7  
        end  %-- end of m-loop for constraints
    end
    %-----------------
    % solve the system
    %-----------------
    %
    %  Partitioned matrix N1
    %  ---------------------
    %
    %   <--- u+c+1 --->
    %  |--------|---|--|    N1 is a square matrix of order u+c+1
    %  |        |   |  |    The right-hand column of N1 is a vector
    %  |   -N   | C'|-t|    containing the u,1 vector -t and the c,1
    %  |        |   |  |    vector g.  The lower-right element of N1
    %  |--------|---|--|    is a single element equal to zero.
    %  |    C   | 0 | g|    The upper-left element of N1 is a square
    %  |--------|---|--|    matrix of order u containing -N (the 
    %  |--------|---|--|    normal equations multiplied by -1).
    %                       C is a c,u set of coefficients of the
    %                       constraint equations.  C' is the transpose
    % of C.  0 is a c,c block of zeros.  The last line of N1 is a
    % line of zeros.  A is a square matrix of order u+c
    %
    %  Partitioned matrix A
    %  --------------------
    %  
    %   <--- u+c --->
    %  |--------|---|    A is a square matrix of order u+c.  A contains
    %  |        |   |    the sub-matrices -N, C, C' and 0 described
    %  |   -N   | C'|    above.
    %  |        |   |    
    %  |--------|---|    
    %  |    C   | 0 |    
    %  |--------|---|     
    %
    %  Partitioned vector b
    %  --------------------
    %
    %  |--|    b is a u+c,1 vector containing the u,1 vector -t, the numeric
    %  |  |    terms of the normal equations multiplied by -1 and the c,1
    %  |-t|    vector g which are the numeric terms of the constraint
    %  |  |    equations
    %  |--|    
    %  | g|    
    %  |--|     
    %
    %
    % set up matrix A and vector b and copy elements from N1
    A  = zeros(u+c,u+c);
    b  = zeros(u+c,1);
    x1 = zeros(u+c,1);
    for i=1:u+c
        for j=1:u+c
            A(i,j) = N1(i,j);
        end  
        b(i,1) = N1(i,u+c+1);
    end
    %
    % calculate solution vectors x and k contained in vector x1
    Ainv = inv(A);
    x1 = Ainv*b;
    %
    % copy the portion of Ainv into Qxx and x1 into X
    Qxx = zeros(u,u);
    X   = zeros(u,1);
    for i=1:u
        for j=1:u
            Qxx(i,j) = -Ainv(i,j);  
        end
        X(i,1) = x1(i,1);
    end  
    % 
    % compute variance factor
    t = zeros(u,1);
    for i=1:u
        t(i,1) = -b(i,1);
    end
    xtt   = X'*t;
    VFACT = (ftf-xtt)/(n-u+c);
    % 
    % test to see if another iteration is required
    JOB(1).iter = JOB(1).iter + 1;
    test = 1;  % (true)
    for j = 1:2*floating
        corrn = X(j,1);
        if (corrn > 0.04 || corrn < -0.04)
            test = 0;  % (false)  
        end  
    end  
    if (test == 1 || JOB(1).iter > 10)
        break  
    end
    %
    % update coordinates of floating stations
    k = 1;
    for i = fixed+1:fixed+floating
        STATION(i).east  = STATION(i).east  + X(k,1)/100.0;
        STATION(i).north = STATION(i).north + X(k+1,1)/100;
        k = k+2;
    end  
  
end  % end while loop  

