/*!
 * \file ls_pvt.cc
 * \brief Implementation of a base class for Least Squares PVT solutions
 * \author Carles Fernandez-Prades, 2015. cfernandez(at)cttc.es
 *
 *
 * -------------------------------------------------------------------------
 *
 * Copyright (C) 2010-2015  (see AUTHORS file for a list of contributors)
 *
 * GNSS-SDR is a software defined Global Navigation
 *          Satellite Systems receiver
 *
 * This file is part of GNSS-SDR.
 *
 * GNSS-SDR is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * GNSS-SDR is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
 *
 * -------------------------------------------------------------------------
 */

#include "ls_pvt.h"
#include <exception>
#include "GPS_L1_CA.h"
#include <gflags/gflags.h>
#include <glog/logging.h>


using google::LogMessage;


Ls_Pvt::Ls_Pvt() : Pvt_Solution()
{

}

arma::vec Ls_Pvt::bancroftPos(const arma::mat& satpos, const arma::vec& obs) {

//    %BANCROFT Calculation of preliminary coordinates
//    %           for a GPS receiver based on pseudoranges
//    %          to 4 or more satellites. The ECEF
//    %          coordinates are stored in satpos. The observed pseudoranges are stored in obs
//    %Reference: Bancroft, S. (1985) An Algebraic Solution
//    %              of the GPS Equations, IEEE Trans. Aerosp.
//    %              and Elec. Systems, AES-21, 56--59
//    %Kai Borre 04-30-95, improved by C.C. Goad 11-24-96
//    %Copyright (c) by Kai Borre
//    %$Revision: 1.0 $  $Date: 1997/09/26  $
//
//    % Test values to use in debugging
//    % B_pass =[ -11716227.778 -10118754.628 21741083.973 22163882.029;
//    %          -12082643.974 -20428242.179  11741374.154 21492579.823;
//    %             14373286.650 -10448439.349    19596404.858 21492492.771;
//    %             10278432.244 -21116508.618  -12689101.970 25284588.982];
//    % Solution:    595025.053  -4856501.221  4078329.981
//
//    % Test values to use in debugging
//    % B_pass = [14177509.188  -18814750.650   12243944.449  21119263.116;
//    %           15097198.146   -4636098.555   21326705.426  22527063.486;
//    %            23460341.997   -9433577.991    8174873.599  23674159.579;
//    %            -8206498.071  -18217989.839   17605227.065  20951643.862;
//    %             1399135.830  -17563786.820   19705534.862  20155386.649;
//    %          6995655.459  -23537808.269   -9927906.485  24222112.972];
//    % Solution:     596902.683   -4847843.316    4088216.740

    arma::vec pos = arma::zeros(4,1);
    arma::mat B_pass=arma::zeros(obs.size(),4);
    B_pass.submat(0,0,obs.size()-1,2)=satpos;
    B_pass.col(3)=obs;

    arma::mat B;
    arma::mat BBB;
    double traveltime=0;
    for (int iter = 0; iter<2; iter++)
    {
       B = B_pass;
       int m=arma::size(B,0);
       for (int i=0;i<m;i++)
       {
          int x = B(i,0);
          int y = B(i,1);
          if (iter == 0)
          {
             traveltime = 0.072;
          }
          else
          {
             int z = B(i,2);
             double rho = (x-pos(0))*(x-pos(0))+(y-pos(1))*(y-pos(1))+(z-pos(2))*(z-pos(2));
             traveltime = sqrt(rho)/GPS_C_m_s;
          }
          double angle = traveltime*7.292115147e-5;
          double cosa = cos(angle);
          double sina = sin(angle);
          B(i,0) =  cosa*x + sina*y;
          B(i,1) = -sina*x + cosa*y;
       }// % i-loop

       if (m > 3)
       {
          BBB = arma::inv(B.t()*B)*B.t();
       }
       else
       {
           BBB = arma::inv(B);
       }
       arma::vec e = arma::ones(m,1);
       arma::vec alpha = arma::zeros(m,1);
       for (int i =0; i<m;i++)
       {
          alpha(i) = lorentz(B.row(i).t(),B.row(i).t())/2.0;
       }
       arma::mat BBBe = BBB*e;
       arma::mat BBBalpha = BBB*alpha;
       double a = lorentz(BBBe,BBBe);
       double b = lorentz(BBBe,BBBalpha)-1;
       double c = lorentz(BBBalpha,BBBalpha);
       double root = sqrt(b*b-a*c);
       arma::vec r = {{(-b-root)/a},{(-b+root)/a}};
       arma::mat possible_pos = arma::zeros(4,2);
       for (int i =0;i<2; i++)
       {
          possible_pos.col(i) = r(i)*BBBe+BBBalpha;
          possible_pos(3,i) = -possible_pos(3,i);
       }

       arma::vec abs_omc=arma::zeros(2,1);
       for (int j=0; j<m; j++)
       {
          for (int i =0;i<2;i++)
          {
             double c_dt = possible_pos(3,i);
             double calc = arma::norm(satpos.row(i).t() -possible_pos.col(i).rows(0,2))+c_dt;
             double omc = obs(j)-calc;
             abs_omc(i) = std::abs(omc);
          }
       }// % j-loop

       //discrimination between roots
       if (abs_omc(0) > abs_omc(1))
       {
           pos = possible_pos.col(1);
       }
       else
       {
          pos = possible_pos.col(0);
       }
    }// % iter loop
    return pos;
}

double Ls_Pvt::lorentz(const arma::vec& x, const arma::vec& y) {
//    %LORENTZ  Calculates the Lorentz inner product of the two
//    %         4 by 1 vectors x and y
//
//    %Kai Borre 04-22-95
//    %Copyright (c) by Kai Borre
//    %$Revision: 1.0 $ $Date: 1997/09/26  $
//
//    % M = diag([1 1 1 -1]);
//    % p = x'*M*y;

    return(x(0)*y(0) + x(1)*y(1) + x(2)*y(2) - x(3)*y(3));
}

arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::vec & w_vec)
{
    /* Computes the Least Squares Solution.
     *   Inputs:
     *       satpos      - Satellites positions in ECEF system: [X; Y; Z;]
     *       obs         - Observations - the pseudorange measurements to each satellite
     *       w           - weigths vector
     *
     *   Returns:
     *       pos         - receiver position and receiver clock error
     *                   (in ECEF system: [X, Y, Z, dt])
     */

    //=== Initialization =======================================================
    int nmbOfIterations = 10; // TODO: include in config
    int nmbOfSatellites;
    nmbOfSatellites = satpos.n_cols;    //Armadillo
    arma::mat w=arma::zeros(nmbOfSatellites,nmbOfSatellites);
    w.diag()=w_vec; //diagonal weight matrix

    arma::vec pos = {{d_rx_pos(0)},{d_rx_pos(0)},{d_rx_pos(0)},0}; //time error in METERS (time x speed)
    arma::mat A;
    arma::mat omc;
    arma::mat az;
    arma::mat el;
    A = arma::zeros(nmbOfSatellites, 4);
    omc = arma::zeros(nmbOfSatellites, 1);
    az = arma::zeros(1, nmbOfSatellites);
    el = arma::zeros(1, nmbOfSatellites);
    arma::mat X = satpos;
    arma::vec Rot_X;
    double rho2;
    double traveltime;
    double trop = 0.0;
    double dlambda;
    double dphi;
    double h;
    arma::mat mat_tmp;
    arma::vec x;

    //=== Iteratively find receiver position ===================================
    for (int iter = 0; iter < nmbOfIterations; iter++)
        {
            for (int i = 0; i < nmbOfSatellites; i++)
                {
                    if (iter == 0)
                        {
                            //--- Initialize variables at the first iteration --------------
                            Rot_X = X.col(i); //Armadillo
                            trop = 0.0;
                        }
                    else
                        {
                            //--- Update equations -----------------------------------------
                            rho2 = (X(0, i) - pos(0)) *
                                   (X(0, i) - pos(0)) + (X(1, i) - pos(1)) *
                                   (X(1, i) - pos(1)) + (X(2, i) - pos(2)) *
                                   (X(2, i) - pos(2));
                            traveltime = sqrt(rho2) / GPS_C_m_s;

                            //--- Correct satellite position (do to earth rotation) --------
                            Rot_X = Ls_Pvt::rotateSatellite(traveltime, X.col(i)); //armadillo

                            //--- Find DOA and range of satellites
                            Ls_Pvt::topocent(&d_visible_satellites_Az[i],
                                     &d_visible_satellites_El[i],
                                     &d_visible_satellites_Distance[i],
                                     pos.subvec(0,2),
                                     Rot_X - pos.subvec(0, 2));
                            if(traveltime < 0.1 && nmbOfSatellites > 3)
                                {
                                    //--- Find receiver's height
                                    Ls_Pvt::togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2));
                                    // Add troposphere correction if the receiver is below the troposphere
                                    if (h > 15000)
                                        {
                                            //receiver is above the troposphere
                                            trop = 0.0;
                                        }else{
                                            //--- Find delay due to troposphere (in meters)
                                            Ls_Pvt::tropo(&trop, sin(d_visible_satellites_El[i] * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
                                            if(trop > 5.0 ) trop = 0.0; //check for erratic values
                                        }

                                }
                        }
                    //--- Apply the corrections ----------------------------------------
                    omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0, 2), 2) - pos(3) - trop); // Armadillo

                    //--- Construct the A matrix ---------------------------------------
                    //Armadillo
                    A(i,0) = (-(Rot_X(0) - pos(0))) / obs(i);
                    A(i,1) = (-(Rot_X(1) - pos(1))) / obs(i);
                    A(i,2) = (-(Rot_X(2) - pos(2))) / obs(i);
                    A(i,3) = 1.0;
                }

            //--- Find position update ---------------------------------------------
            x = arma::solve(w*A, w*omc); // Armadillo

            //--- Apply position update --------------------------------------------
            pos = pos + x;
            if (arma::norm(x,2) < 1e-4)
            {
                break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm)
            }
        }

    try
    {
            //-- compute the Dilution Of Precision values
            d_Q = arma::inv(arma::htrans(A)*A);
    }
    catch(std::exception& e)
    {
            d_Q = arma::zeros(4,4);
    }

    return pos;
}