/*
 * @file pppBayesTree.cpp
 * @brief Iterative GPS Range/Phase Estimator with collected data
 * @author Ryan Watson & Jason Gross
 */

#include "nonlinear/ISAM2.h"
#include "nonlinear/NonlinearFactorGraph.h"
#include "inference/Symbol.h"

#include "slam/PriorFactor.h"
#include "slam/BetweenFactor.h"
#include "pppbayestree/gnssNavigation/GnssData.h"
#include "pppbayestree/gnssNavigation/GnssTools.h"
#include "pppbayestree/gnssNavigation/PhaseFactor.h"
#include "pppbayestree/gnssNavigation/nonBiasStates.h"
#include "pppbayestree/configReader/ConfDataReader.hpp"

#include "pppbayestree/gnssNavigation/PseudorangeFactor.h"

// GPSTK
#include "pppbayestree/gpstk/MJD.hpp"
#include "pppbayestree/gpstk/PowerSum.hpp"
#include "pppbayestree/gpstk/Decimate.hpp"
#include "pppbayestree/gpstk/SolidTides.hpp"
#include "pppbayestree/gpstk/PoleTides.hpp"
#include "pppbayestree/gpstk/TropModel.hpp"
#include "pppbayestree/gpstk/BasicModel.hpp"
#include "pppbayestree/gpstk/CommonTime.hpp"
#include "pppbayestree/gpstk/PCSmoother.hpp"
#include "pppbayestree/gpstk/OceanLoading.hpp"
#include "pppbayestree/gpstk/CodeSmoother.hpp"
#include "pppbayestree/gpstk/SimpleFilter.hpp"
#include "pppbayestree/gpstk/MWCSDetector.hpp"
#include "pppbayestree/gpstk/SatArcMarker.hpp"
#include "pppbayestree/gpstk/DCBDataReader.hpp"
#include "pppbayestree/gpstk/ComputeWindUp.hpp"
#include "pppbayestree/gpstk/Rinex3NavData.hpp"
#include "pppbayestree/gpstk/GNSSconstants.hpp"
#include "pppbayestree/gpstk/ComputeLinear.hpp"
#include "pppbayestree/gpstk/GPSWeekSecond.hpp"
#include "pppbayestree/gpstk/LICSDetector2.hpp"
#include "pppbayestree/gpstk/DataStructures.hpp"
#include "pppbayestree/gpstk/RinexObsStream.hpp"
#include "pppbayestree/gpstk/Rinex3ObsStream.hpp"
#include "pppbayestree/gpstk/Rinex3NavStream.hpp"
#include "pppbayestree/gpstk/ComputeTropModel.hpp"
#include "pppbayestree/gpstk/SP3EphemerisStore.hpp"
#include "pppbayestree/gpstk/ComputeSatPCenter.hpp"
#include "pppbayestree/gpstk/EclipsedSatFilter.hpp"
#include "pppbayestree/gpstk/GPSEphemerisStore.hpp"
#include "pppbayestree/gpstk/CorrectCodeBiases.hpp"
#include "pppbayestree/gpstk/ComputeSatPCenter.hpp"
#include "pppbayestree/gpstk/RequireObservables.hpp"
#include "pppbayestree/gpstk/CorrectObservables.hpp"
#include "pppbayestree/gpstk/LinearCombinations.hpp"
#include "pppbayestree/gpstk/GravitationalDelay.hpp"
#include "pppbayestree/gpstk/PhaseCodeAlignment.hpp"
#include "pppbayestree/slam/dataset.h"

// BOOST
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <boost/serialization/export.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>

// STD
#include <chrono>
#include <iomanip>
#include <fstream>
#include <iostream>
#include <unistd.h>
#include <algorithm>
#include <Eigen/Core>

using namespace std;
using namespace gpstk;
using namespace boost;
using namespace std::chrono;
using namespace minisam;
namespace po = boost::program_options;

int main(int argc, char* argv[])
{
    // define std out print color
    vector<int> prn_vec;
    vector<rnxData> data;
    const string red("\033[0;31m");
    const string green("\033[0;32m");
    string confFile, gnssFile, station, sp3File, p1p2File, p1c1File, antennaModel;
    string rnx_file, nav_file, sp3_file, out_file, antexFile;
    double xn, yn, zn, xp, yp, range, phase, rho, minElev, weightFactor;
    double antennaOffSetH, antennaOffSetE, antennaOffSetN;
    int startKey(0), currKey, startEpoch(0), svn, doy;
    int nThreads(-1), phase_break, break_count(0), nextKey, dec_int, itsBelowThree=0, count=0;
    bool printECEF, printENU, printAmb, printUpdateRate, first_ob(true), usingP1(false);

    FILE *fprealtime=fopen("examples_tuning/gpsdata/pppgpsposbackcount.txt","w+");

    cout.precision(12);

    /*po::options_description desc("Available options");
    desc.add_options()
            ("help,h", "Print help message")
            ("confFile,c", po::value<string>(&confFile)->default_value(""),
            "Input config file" )
            ("out", po::value<string>(&out_file)->default_value(""),
            "output file.")
            ("usingP1", "Are you using P1 instead of C1?");
    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
    po::notify(vm);*/

    ConfDataReader confReader;
    confReader.open("examples_tuning/gpsdata/phastball.conf");
    ISAM2Data isam2data;

    /*if (confFile.empty() ) {
            cout << red << "\n\n Currently, you need to provide a conf file \n"
                 << "\n\n"  << green << desc << endl;
    }*/

    while ( (station = confReader.getEachSection()) != "" )
    {
        // Fetch nominal station location [m]
        xn = confReader.fetchListValueAsDouble("nominalECEF",station);
        yn = confReader.fetchListValueAsDouble("nominalECEF",station);
        zn = confReader.fetchListValueAsDouble("nominalECEF",station);
        // day of year ( used for Niell Trop model)
        doy = confReader.getValueAsInt("DOY", station);
        // Elevation cut-off
        minElev = confReader.getValueAsDouble("minElev", station);
        // Code/carrier ratio
        weightFactor = confReader.getValueAsDouble("weightFactor", station);
        // Data file names
        gnssFile = confReader("rnxFile", station);
        sp3File = confReader("sp3File", station);
        p1p2File = confReader("p1p2", station);
        p1c1File = confReader("p1c1", station);
        // Print statements
        printENU = confReader.getValueAsBoolean("printENU", station);
        printAmb = confReader.getValueAsBoolean("printAmb", station);
        printECEF = confReader.getValueAsBoolean("printECEF", station);
        printUpdateRate = confReader.getValueAsBoolean("printUpdateRate", station);
    }

    //usingP1 = (vm.count("usingP1")>0);

    minivector nomXYZ(xn, yn, zn);
    minivector prop_xyz = nomXYZ;

    /*#ifdef USE_TBB
    std::auto_ptr<tbb::task_scheduler_init> init;
    if(nThreads > 0) {
            init.reset(new tbb::task_scheduler_init(nThreads));
    }
    else
            cout << green << " \n\n Using threads for all processors" << endl;
    #else
    */
    if(nThreads > 0)
    {
        cout << red <<" \n\n not compiled with TBB, so threading is"
             << " disabled and the --threads option cannot be used."
             << endl;
        exit(1);
    }
    //#endif

    //ISAM2DoglegParams doglegParams;
    ISAM2Params parameters;
    //parameters.optimizationParamsDogleg=new ISAM2DoglegParams;
    parameters.optimizationParamsGaussNewton=new ISAM2GaussNewtonParams;
    parameters.relinearizeThresholdDouble = 0.1;
    parameters.relinearizeSkip = 10;
    ISAM2 isam(parameters);

    double output_time = 0.0;
    double rw = 2.5;
    double rangeWeight = pow(rw,2);
    double phaseWeight = pow(rw*1/weightFactor,2);

    string value;

    minivector prior_nonBias(5,0.0);
    //prior_nonBias<<0.0, 0.0, 0.0, 0.0, 0.0;

    double bias_state(0.0);
    minivector phase_arc(34,0.0);
    //phase_arc.setZero();
    minivector bias_counter(34,0.0);
   // bias_counter.setZero();

    for (int i=1; i<34; i++)
    {
        bias_counter.data[i] = bias_counter.data[i-1] + 10000;
    }

    minivector* initEst5=new minivector(5);
    minivector_set_zero(initEst5);
   // initEst5<<0.0, 0.0, 0.0, 0.0, 0.0;

    minivector*  between_nonBias_State=new minivector(5);
    minivector_set_zero(between_nonBias_State);
    //between_nonBias_State<<0.0, 0.0, 0.0, 0.0, 0.0;

    //Values initial_values;
    std::map<int,minimatrix*> initial_values;
   // std::map<int,Pose3> initial_valuesP;
    //Values result;
   // std::map<int,Eigen::VectorXd> result;

    minivector BiasinitNoise(5);
    BiasinitNoise.data[0]=10.0;BiasinitNoise.data[1]=10.0;BiasinitNoise.data[2]=10.0;
    BiasinitNoise.data[3]=3e8;BiasinitNoise.data[4]=1e-1;
    //BiasinitNoise<<10.0, 10.0, 10.0, 3e8, 1e-1;
    minivector_cwisesqrt(&BiasinitNoise);
   // GaussianNoiseModel* nonBias_InitNoise=new GaussianNoiseModel(BiasinitNoise.cwiseSqrt());
    GaussianNoiseModel* nonBias_InitNoise=new GaussianNoiseModel(BiasinitNoise);

    minivector nonBiasProcessNoise(5);
    nonBiasProcessNoise.data[0]=0.1;nonBiasProcessNoise.data[1]=0.1;nonBiasProcessNoise.data[2]=0.1;
    nonBiasProcessNoise.data[3]=3e6;nonBiasProcessNoise.data[4]=3e-5;
    minivector_cwisesqrt(&nonBiasProcessNoise);

     GaussianNoiseModel* nonBias_ProcessNoise=new GaussianNoiseModel(nonBiasProcessNoise);

    //0.1, 0.1, 0.1, 3e6, 3e-5;
    minivector init_Noise(1,100);//init_Noise[0]=100;
    //init_Noise<<100;
    minivector_cwisesqrt(&init_Noise);

     GaussianNoiseModel* initNoise=new GaussianNoiseModel(init_Noise);

    string obs_path = findExampleDataFile(gnssFile,"examples_tuning/gpsdata");
    string p1p2_path = findExampleDataFile(p1p2File,"examples_tuning/gpsdata");
    string p1c1_path = findExampleDataFile(p1c1File,"examples_tuning/gpsdata");

    if ( obs_path.empty() )
    {
        // cout << " Must pass in obs file !!! " << desc << endl;
        exit(1);
    }
    if ( sp3File.empty() )
    {
        //cout << " Must pass in ephemeris file !!! " << desc << endl;
        exit(1);
    }
    // Declare a "SP3EphemerisStore" object to handle precise ephemeris
    SP3EphemerisStore SP3EphList;

    size_t pos = 0;
    string path, token;
    string delimiter = " ";
    while ((pos = sp3File.find(delimiter)) != string::npos)
    {
        path = findExampleDataFile(sp3File.substr(0,pos),"examples_tuning/gpsdata");
        SP3EphList.loadFile(path);
        sp3File.erase(0, pos + delimiter.length());
    }
    path = findExampleDataFile(sp3File,"examples_tuning/gpsdata");
    SP3EphList.loadFile(path);

    // Set flags to reject satellites with bad or absent positional
    // values or clocks
    SP3EphList.rejectBadPositions(true);
    SP3EphList.rejectBadClocks(true);

    // Create the input observation file stream
    Rinex3ObsStream rin(obs_path);

    // station nominal position
    Position nominalPos(xn, yn, zn);

    CorrectCodeBiases corrCode;
    corrCode.setDCBFile(p1p2_path, p1c1_path);

    if (!usingP1)
    {
        corrCode.setUsingC1(true);
    }

    // This is the GNSS data structure that will hold all the
    // GNSS-related information
    gnssRinex gRin;

    RequireObservables requireObs;
    requireObs.addRequiredType(TypeID::L1);
    requireObs.addRequiredType(TypeID::L2);

    SimpleFilter pObsFilter;
    pObsFilter.setFilteredType(TypeID::C1);

    if ( usingP1 )
    {
        requireObs.addRequiredType(TypeID::P1);
        pObsFilter.addFilteredType(TypeID::P1);
        requireObs.addRequiredType(TypeID::P2);
        pObsFilter.addFilteredType(TypeID::P2);
    }
    else
    {
        requireObs.addRequiredType(TypeID::C1);
        pObsFilter.addFilteredType(TypeID::C1);
        requireObs.addRequiredType(TypeID::P2);
        pObsFilter.addFilteredType(TypeID::P2);
    }

    // Declare a couple of basic modelers
    BasicModel basic(nominalPos, SP3EphList);
    basic.setMinElev(minElev);

    // Object to correct for SP3 Sat Phase-center offset
    ComputeSatPCenter svPcenter(SP3EphList, nominalPos);

    // Objects to mark cycle slips
    MWCSDetector markCSMW;  // Checks Merbourne-Wubbena cycle slip

    // object def several linear combinations
    LinearCombinations comb;

    // Object to compute linear combinations for cycle slip detection
    ComputeLinear linear1;
    if ( usingP1 )
    {
        linear1.addLinear(comb.pdeltaCombination);
        linear1.addLinear(comb.mwubbenaCombination);
    }
    else
    {
        linear1.addLinear(comb.pdeltaCombWithC1);
        linear1.addLinear(comb.mwubbenaCombWithC1);
    }
    linear1.addLinear(comb.ldeltaCombination);
    linear1.addLinear(comb.liCombination);

    ComputeLinear linear2;

    // Read if we should use C1 instead of P1
    if ( usingP1 )
    {
        linear2.addLinear(comb.pcCombination);
    }
    else
    {
        linear2.addLinear(comb.pcCombWithC1);
    }
    linear2.addLinear(comb.lcCombination);

    LICSDetector2 markCSLI2;       // Checks LI cycle slips

    // Object to keep track of satellite arcs
    SatArcMarker markArc;
    markArc.setDeleteUnstableSats(true);

    // Objects to compute gravitational delay effects
    GravitationalDelay grDelay(nominalPos);

    // Object to remove eclipsed satellites
    EclipsedSatFilter eclipsedSV;

    //Object to compute wind-up effect
    ComputeWindUp windup( SP3EphList, nominalPos );

    // Object to compute prefit-residuals
    ComputeLinear linear3(comb.pcPrefit);
    linear3.addLinear(comb.lcPrefit);

    TypeIDSet tset;
    tset.insert(TypeID::prefitC);
    tset.insert(TypeID::prefitL);

    // Declare a NeillTropModel object, setting the defaults
    NeillTropModel neillTM( nominalPos.getAltitude(),
                            nominalPos.getGeodeticLatitude(),
                            doy);

    // Objects to compute the tropospheric data
    ComputeTropModel computeTropo(neillTM);

    // initialize factor graph
    //NonlinearFactorGraph *graph = new NonlinearFactorGraph();
    NonlinearFactorGraph graph;

    auto start = std::chrono::steady_clock::now();
    auto end = std::chrono::steady_clock::now();
    // Loop over all data epochs
    while(rin >> gRin)
    {
        TimeSystem sys;
        sys.fromString("GPS");
        CommonTime time(gRin.header.epoch);
        time.setTimeSystem(sys);
        GPSWeekSecond gpstime( time );

        // update nominal ECEF with propogated pos.
        NeillTropModel neillTM( nominalPos.getAltitude(),
                                nominalPos.getGeodeticLatitude(),
                                doy);
        try
        {
            gRin >> requireObs // Check if required observations are present
                 >> pObsFilter // Filter out spurious data
                 >> linear1 // Compute linear combinations to detect CS
                 >> markCSLI2 // Mark cycle slips
                 >> markArc // Keep track of satellite arcs
                 >> basic // Compute the basic components of model
                 >> eclipsedSV // Remove satellites in eclipse
                 >> grDelay // Compute gravitational delay
                 >> svPcenter // Computer delta for sat. phase center
                 >> corrCode // Correct for differential code biases
                 >> windup // phase windup correction
                 >> computeTropo // neill trop function
                 >> linear2  // Compute ionosphere-free combinations
                 >> linear3;   // Compute prefit residuals
        }
        catch(Exception& e)
        {
            continue;
        }
        catch(...)
        {
            cerr << "Unknown exception at epoch: " << time << endl;
            continue;
        }

        // Iterate through the GNSS Data Structure
        satTypeValueMap::const_iterator it;
        typeValueMap::const_iterator itObs;
        if ( itsBelowThree > 0 )
        {
            itsBelowThree = 0;
            continue;
        }

        if (gRin.body.size() == 0)
        {
            continue;
        }

        // Loop over all observed sats at current epoch
        for (it = gRin.body.begin(); it!= gRin.body.end(); it++)
        {

            start = std::chrono::steady_clock::now();
            svn = ((*it).first).id;
            double satX, satY, satZ;
            satX = (*it).second.getValue(TypeID::satX);
            satY = (*it).second.getValue(TypeID::satY);
            satZ = (*it).second.getValue(TypeID::satZ);
            minivector satXYZ(satX,satY,satZ);
            double range, rangeRes;
            range = (*it).second.getValue(TypeID::PC);
            rangeRes = (*it).second.getValue(TypeID::prefitC);
            double phase, phaseRes;
            phase = (*it).second.getValue(TypeID::LC);
            phaseRes = (*it).second.getValue(TypeID::prefitL);
            int phase_break;
            phase_break = (*it).second.getValue(TypeID::satArc);

            if (first_ob)
            {
                startKey = count;
                first_ob=false;
                PriorFactor* npn=new PriorFactor(Symbol('X',count).key(), initEst5,  nonBias_InitNoise);

                graph.push_back(npn);
                initial_values.insert(std::make_pair(Symbol('X',count).key(), new minivector(initEst5)));

            }

            if (phase_arc.data[svn]!=phase_break)
            {
                bias_state = phase - range;
                if (count > startKey)
                {
                    bias_counter.data[svn] = bias_counter.data[svn] +1;
                }
                minivector *biasb=new minivector(1);
                biasb->data[0]=bias_state;
               // biasb<<bias_state;

                //GaussianNoiseModel* initNoise=new GaussianNoiseModel(init_Noise.cwiseSqrt());

                PriorFactor* nphb=new PriorFactor(Symbol('B',bias_counter.data[svn]).key(),
                biasb,initNoise);
                graph.push_back(nphb);
                initial_values.insert(std::make_pair(Symbol('B',bias_counter.data[svn]).key(), new minivector(biasb)));
                phase_arc.data[svn] = phase_break;
            }
            // Generate pseudorange factor
            minivector gpsRangeFactorvec(1,sqrt(elDepWeight(satXYZ, nomXYZ, rangeWeight)));

            //gpsRangeFactorvec<<elDepWeight(satXYZ, nomXYZ, rangeWeight);
            //GaussianNoiseModel* ngpsrfn=new GaussianNoiseModel(gpsRangeFactorvec.cwiseSqrt());
            GaussianNoiseModel* ngpsrfn=new GaussianNoiseModel(gpsRangeFactorvec);
            PseudorangeFactor* ngpsrf=new PseudorangeFactor(Symbol('X',count).key(), rangeRes,
             new minivector(satXYZ), new minivector(nomXYZ),ngpsrfn);

            graph.push_back(ngpsrf);
            gpsRangeFactorvec.data[0]=sqrt(elDepWeight(satXYZ, nomXYZ, phaseWeight));
            GaussianNoiseModel* ngpspfn2=new GaussianNoiseModel(gpsRangeFactorvec);
            //GaussianNoiseModel* ngpspfn2=new GaussianNoiseModel(gpsRangeFactorvec.cwiseSqrt());
            PhaseFactor* ngpspf=new PhaseFactor(Symbol('X',count).key(),
             Symbol('B',bias_counter.data[svn]).key(),phaseRes, new minivector(satXYZ),
             new minivector( nomXYZ),ngpspfn2);
            graph.push_back(ngpspf);

            prn_vec.push_back(svn);
        }
        if (count > startKey )
        {
           // GaussianNoiseModel* nonBias_ProcessNoise=
           // new GaussianNoiseModel(nonBiasProcessNoise.cwiseSqrt());

            BetweenFactor* nbfnbs=new BetweenFactor(Symbol('X',count).key(),
            Symbol('X',count-1).key(), new minivector(between_nonBias_State), nonBias_ProcessNoise);
            graph.push_back(nbfnbs);
        }

        isam.update(graph, initial_values,isam2data);
        //result = isam.calculateEstimate(isam2data);
        isam.calculateEstimate(isam2data);

        end = std::chrono::steady_clock::now();

        //prior_nonBias =result.at(Symbol('X',count).key());
        //prior_nonBias =isam2data.result.at(Symbol('X',count).key());
        minimatrix_memcpy(&prior_nonBias,isam2data.resulttheta_.at(Symbol('X',count).key()));

        minivector delta_xyz(prior_nonBias.data[0], prior_nonBias.data[1], prior_nonBias.data[2]);
        Position deltaPos(prior_nonBias.data[0], prior_nonBias.data[1], prior_nonBias.data[2]);
        //prop_xyz = nomXYZ - delta_xyz;
        minivector_sub(&prop_xyz,nomXYZ,delta_xyz);
        nominalPos -= deltaPos;

        if (printECEF)
        {
            cout << "xyz " << gpstime.week << " " << gpstime.sow << " " << prop_xyz.x() <<
            " " << prop_xyz.y() << " " << prop_xyz.data[2] << endl;
        }

        if (printENU)
        {
            minivector enu = xyz2enu(prop_xyz, nomXYZ);
            cout << "enu " << gpstime.week << " " << gpstime.sow << " " <<
            enu.x() << " " << enu.y() << " " << enu.data[2] << endl;
            fprintf(fprealtime,"%d %.15f %.15f %.15f %.15f %d\n",
                    gpstime.week,gpstime.sow,enu.x(),enu.y(),enu.data[2],
                    isam.lastBacksubVariableCount);
        }

        if (printAmb)
        {
            for (int k=0; k<prn_vec.size(); k++)
            {
                cout << "amb. " << gpstime.week << " " << gpstime.sow << " ";
                cout << prn_vec[k] << " ";
                //cout << result.at(Symbol('B',bias_counter[prn_vec[k]]).key()) << endl;
                minimatrix_print(isam2data.resulttheta_.at(Symbol('B',
                bias_counter.data[prn_vec[k]]).key()));
                cout << endl;
            }
        }

        if (printUpdateRate)
        {
            cout << "Elapsed time "
                 << std::chrono::duration_cast<std::chrono::microseconds>(end - start).count()
                 << " µs" << endl;
        }

        output_time = output_time +1;
        graph.resize(0);
        initial_values.clear();
        prn_vec.clear();
        count++;
        initial_values.insert(std::make_pair(Symbol('X',count).key(),
        new minivector(prior_nonBias)));
       if(count>20000)
        {
         isam.clearall();
    isam2data.clearvalues();
    isam2data.clearfactors();
    delete parameters.optimizationParamsGaussNewton;
    delete between_nonBias_State;
    delete nonBias_InitNoise;
    delete initNoise;
     delete nonBias_ProcessNoise;
     for(auto& dlt:initial_values)
    {
       if(dlt.second!=NULL)
       delete dlt.second;
    }
    return 0;
        }
    }
   // ofstream bs("examples_tuning/data/gnsstree.dot");
    //isam.saveGraph(bs);

    isam.clearall();
    isam2data.clearvalues();
    isam2data.clearfactors();
    delete parameters.optimizationParamsGaussNewton;
    delete between_nonBias_State;
    delete nonBias_InitNoise;
    delete initNoise;
    delete nonBias_ProcessNoise;
    for(auto& dlt:initial_values)
    {
       if(dlt.second!=NULL)
       delete dlt.second;
    }

    return 0;
}
/* ************************************************************************* */


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *
 *    Notes:
 *      Test tolerance was set at 5.0e-15 (or 5.0e-7 for floats) instead of epsilon due to
 *      rounding errors in Eigen types with entries over a number of orders of magnitude,
 *      presumably causing the observed larger than epsilon relative differences between
 *      expected and computed values.
 *
 */

#define BOOST_TEST_MAIN

#include <vector>

#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <memory>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

#include <Eigen/Core>

#include "Tudat/Basics/testMacros.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/UnitTests/testAccelerationModels.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/UnitTests/testBody.h"
#include "Tudat/Basics/basicTypedefs.h"

namespace tudat
{
namespace unit_tests
{

using basic_astrodynamics::AccelerationModel;
using basic_astrodynamics::updateAndGetAcceleration;

BOOST_AUTO_TEST_SUITE( test_accelerationModel )

//! Test whether DerivedAccelerationModel (acceleration data type=Eigen::Vector3d) functions
//! correctly.
BOOST_AUTO_TEST_CASE( test_derived3dAccelerationModel )
{
    using basic_astrodynamics::AccelerationModel3dPointer;

    // Shortcuts.
    typedef TestBody< 3, double > TestBody3d;
    typedef std::shared_ptr< TestBody3d > TestBody3dPointer;
    typedef DerivedAccelerationModel< > DerivedAccelerationModel3d;

    // Create body with initial state and time.
    TestBody3dPointer body = std::make_shared< TestBody3d >(
                ( Eigen::Vector6d( ) << 1.1, 2.2, 3.3, -0.1, 0.2, 0.3 ).finished( ), 2.0 );

    // Create acceleration model using DerivedAccelerationModel class, and pass pointers to
    // functions in body.
    AccelerationModel3dPointer accelerationModel3d
            = std::make_shared< DerivedAccelerationModel3d >(
                std::bind( &TestBody3d::getCurrentPosition, body ),
                std::bind( &TestBody3d::getCurrentTime, body ) );

    // Declare container of computed accelerations.
    std::vector< Eigen::Vector3d > computedAccelerations( 3 );

    // Get acceleration vector before members are updated.
    computedAccelerations.at( 0 ) = accelerationModel3d->getAcceleration( );

    // Update time and state.
    body->setCurrentTimeAndState(
                -1.1, ( Eigen::Vector6d( )
                        << -0.45, 10.63, -9.81, 0.11, 0.22, 0.33 ).finished( ) );

    // Update acceleration model members.
    accelerationModel3d->updateMembers( );

    // Get acceleration vector, now after members have been updated.
    computedAccelerations.at( 1 ) = accelerationModel3d->getAcceleration( );

    // Update time and state.
    body->setCurrentTimeAndState(
                4.6, ( Eigen::Vector6d( )
                       << -87.685, 101.44, -1.38, -0.12, 0.23, -0.34 ).finished( ) );

    // Update and get acceleration with single function.
    computedAccelerations.at( 2 ) = updateAndGetAcceleration( accelerationModel3d );

    // Set expected accelerations.
    std::vector< Eigen::Vector3d > expectedAccelerations;
    expectedAccelerations.push_back( Eigen::Vector3d(   1.1,     2.2,   3.3  ) / (  2.0 *  2.0 ) );
    expectedAccelerations.push_back( Eigen::Vector3d(  -0.45,   10.63, -9.81 ) / ( -1.1 * -1.1 ) );
    expectedAccelerations.push_back( Eigen::Vector3d( -87.685, 101.44, -1.38 ) / (  4.6 *  4.6 ) );

    // Check that the acceleration vectors before and after the update match expected values.
    for ( unsigned int i = 0; i < computedAccelerations.size( ); i++ )
    {
        TUDAT_CHECK_MATRIX_BASE( computedAccelerations.at( i ), expectedAccelerations.at( i ) )
                BOOST_CHECK_CLOSE_FRACTION( computedAccelerations.at( i ).coeff( row, col ),
                                   expectedAccelerations.at( i ).coeff( row, col ), 5.0e-15 );
    }
}

//! Test whether AnotherDerivedAccelerationModel (acceleration data type=Eigen::Vector2f) functions
//! correctly.
BOOST_AUTO_TEST_CASE( test_derived2fAccelerationModel )
{
    // NOTE: For this test, it is imperative that the float values are given with the "f"
    // suffix, to ensure that their precision is indeed of float-type.

    // Shortcuts.
    typedef TestBody< 2, float > TestBody2f;
    typedef std::shared_ptr< TestBody2f > TestBody2fPointer;
    typedef AccelerationModel< Eigen::Vector2f > AccelerationModel2f;
    typedef std::shared_ptr< AccelerationModel2f > AccelerationModel2fPointer;
    typedef AnotherDerivedAccelerationModel< Eigen::Vector2f, Eigen::Vector2f,
            Eigen::Vector2f, float > AnotherDerivedAccelerationModel2f;

    // Create body with initial state and time.
    TestBody2fPointer body = std::make_shared< TestBody2f >(
                ( Eigen::VectorXf( 4 ) << -0.3f, 4.5f, 0.1f, 0.2f ).finished( ), -2.3f );

    // Create acceleration model using AnotherDerivedAccelerationModel class, and pass pointers to
    // functions in body.
    AccelerationModel2fPointer accelerationModel2f
            = std::make_shared< AnotherDerivedAccelerationModel2f >(
                std::bind( &TestBody2f::getCurrentPosition, body ),
                std::bind( &TestBody2f::getCurrentVelocity, body ),
                std::bind( &TestBody2f::getCurrentTime, body ) );

    // Declare container of computed accelerations.
    std::vector< Eigen::Vector2f > computedAccelerations( 3 );

    // Get acceleration vector before members are updated.
    computedAccelerations.at( 0 ) = accelerationModel2f->getAcceleration( );

    // Update time and state.
    body->setCurrentTimeAndState(
                0.33f, ( Eigen::VectorXf( 4 ) << 1.34f, 2.65f, -0.23f, 0.1f ).finished( ) );

    // Update acceleration model members.
    accelerationModel2f->updateMembers( );

    // Get acceleration vector, now after members have been updated.
    computedAccelerations.at( 1 ) = accelerationModel2f->getAcceleration( );

    // Update time and state.
    body->setCurrentTimeAndState(
                -10.3f, ( Eigen::VectorXf( 4 ) << -98.99f, 1.53f, 1.23f, -0.11f ).finished( ) );

    // Update and get acceleration with single function.
    computedAccelerations.at( 2 ) = updateAndGetAcceleration( accelerationModel2f );

    // Set expected accelerations.
    std::vector< Eigen::Vector2f > expectedAccelerations;
    expectedAccelerations.push_back( 0.5 * Eigen::Vector2f( -0.3f, 4.5f )
                                     / ( 3.2 * ( -2.3f + 3.4 ) * -2.3f )
                                     + Eigen::Vector2f( 0.1f, 0.2f ) / -2.3f );
    expectedAccelerations.push_back( 0.5 * Eigen::Vector2f( 1.34f, 2.65f )
                                     / ( 3.2 * ( 0.33f + 3.4 ) * 0.33f )
                                     + Eigen::Vector2f( -0.23f, 0.1f ) / 0.33f );
    expectedAccelerations.push_back( 0.5 * Eigen::Vector2f( -98.99f, 1.53f )
                                     / ( 3.2 * ( -10.3f + 3.4 ) * -10.3f )
                                     + Eigen::Vector2f( 1.23f, -0.11f ) / -10.3f );

    // Check that the acceleration vectors before and after the update match expected values.
    for ( unsigned int i = 0; i < computedAccelerations.size( ); i++ )
    {
        TUDAT_CHECK_MATRIX_BASE( computedAccelerations.at( i ), expectedAccelerations.at( i ) )
                BOOST_CHECK_CLOSE_FRACTION( computedAccelerations.at( i ).coeff( row, col ),
                                   expectedAccelerations.at( i ).coeff( row, col ), 5.0e-7 );
    }
}

//! Test whether DerivedAccelerationModel (acceleration data type=Eigen::Vector1i) functions
//! correctly.
BOOST_AUTO_TEST_CASE( test_derived1iAccelerationModel )
{
    // Shortcuts.
    typedef TestBody< 1, int > TestBody1i;
    typedef std::shared_ptr< TestBody1i > TestBody1iPointer;
    typedef AccelerationModel< int > AccelerationModel1i;
    typedef std::shared_ptr< AccelerationModel1i > AccelerationModel1iPointer;
    typedef DerivedAccelerationModel< int, int, int > DerivedAccelerationModel1i;

    // Create body with initial state and time.
    TestBody1iPointer body = std::make_shared< TestBody1i >( Eigen::Vector2i( 20, 1 ), 2 );

    // Create acceleration model using DerivedAccelerationModel class, and pass pointers to
    // functions in body.
    AccelerationModel1iPointer accelerationModeli1
            = std::make_shared< DerivedAccelerationModel1i >(
                std::bind( ( &TestBody1i::getCurrentPosition ), body ),
                std::bind( ( &TestBody1i::getCurrentTime ), body ) );

    // Declare container of computed accelerations.
    std::vector< int > computedAccelerations( 3 );

    // Get scalar acceleration before members are updated.
    computedAccelerations.at( 0 ) = accelerationModeli1->getAcceleration( );

    // Update time and state.
    body->setCurrentTimeAndState( -3, Eigen::Vector2i( 90, 3 ) );

    // Update acceleration model members.
    accelerationModeli1->updateMembers( );

    // Get scalar acceleration, now after members have been updated.
    computedAccelerations.at( 1 ) = accelerationModeli1->getAcceleration( );

    // Update time and state.
    body->setCurrentTimeAndState( 4, Eigen::Vector2i( 16, -4 ) );

    // Update and get scalar acceleration with single function.
    computedAccelerations.at( 2 ) = updateAndGetAcceleration( accelerationModeli1 );

    // Set expected accelerations.
    const std::vector< int > expectedAccelerations = { 20 / ( 2 * 2 ), 90 / ( -3 * -3 ), 16 / ( 4 * 4 ) };

    // Check that the acceleration vectors before and after the update match expected values.
    for ( unsigned int i = 0; i < computedAccelerations.size( ); i++ )
    {
        BOOST_CHECK_EQUAL( computedAccelerations.at( i ), expectedAccelerations.at( i ) );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include <Eigen/Dense>
#include <nnls.h>

int main() {
}


/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\
|  Phycas: Python software for phylogenetic analysis                          |
|  Copyright (C) 2006 Mark T. Holder, Paul O. Lewis and David L. Swofford     |
|                                                                             |
|  This program 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 2 of the License, or          |
|  (at your option) any later version.                                        |
|                                                                             |
|  This program 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 this program; if not, write to the Free Software Foundation, Inc.,    |
|  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.                |
\~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

#if ! defined(MCMC_PARAM_HPP)
#define MCMC_PARAM_HPP

#include <vector>									// for std::vector
#include <boost/shared_ptr.hpp>						// for boost::shared_ptr
#include <boost/weak_ptr.hpp>						// for boost::weak_ptr
#include <boost/enable_shared_from_this.hpp>		// for boost::enable_shared_from_this
#include "mcmc_updater.hpp"				// for MCMCUpdater base class
#include "split.hpp"

namespace phycas
{

class HKY;
class Codon;

/*----------------------------------------------------------------------------------------------------------------------
|	Encapsulates the transition/transversion rate ratio parameter of models such as the HKY model. This parameter is
|	commonly symbolized using the Greek letter kappa, hence the name.
*/
class KappaParam : public MCMCUpdater
	{
	public:
						KappaParam();
						virtual ~KappaParam();

        virtual void	sendCurrValueToModel(double v);
        virtual double  getCurrValueFromModel() const;
		virtual void	setModel(ModelShPtr p);
		virtual bool	update();				// override virtual from MCMCUpdater base class
		virtual double	operator()(double k);	// override pure virtual from AdHocDensity (base class of MCMCUpdater)

	private:

		HKY *			hky;
		Codon *			codon;
	};

/*----------------------------------------------------------------------------------------------------------------------
|	Encapsulates the nonsynonymous/synonymous rate ratio parameter of a codon model. This parameter is commonly
|	symbolized using the Greek letter omega, hence the name.
*/
class OmegaParam : public MCMCUpdater
	{
	public:
						OmegaParam();
						virtual ~OmegaParam()
							{
							//std::cerr << "OmegaParam dying..." << std::endl;
							}

        virtual void	sendCurrValueToModel(double v);
        virtual double 	getCurrValueFromModel() const;
		virtual void	setModel(ModelShPtr p);
		virtual bool	update();				// override virtual from MCMCUpdater base class
		virtual double	operator()(double k);	// override pure virtual from AdHocDensity (base class of MCMCUpdater)

	private:

		Codon *			codon;
	};

class GTR;

/*----------------------------------------------------------------------------------------------------------------------
|	Encapsulates one of the relative rate parameters in the GTR model.
*/
class GTRRateParam : public MCMCUpdater
	{
	public:
						GTRRateParam(unsigned w);
						virtual ~GTRRateParam()
							{
							//std::cerr << "GTRRateParam dying..." << std::endl;
							}

        virtual void	sendCurrValueToModel(double v);
        virtual double  getCurrValueFromModel() const;
		virtual void	setModel(ModelShPtr p);
		virtual bool	update();				// override virtual from MCMCUpdater base class
		virtual double	operator()(double k);	// override pure virtual from AdHocDensity (base class of MCMCUpdater)

	private:

		GTR *			gtr;		/**< */
		unsigned		which;		/**< */
	};

/*----------------------------------------------------------------------------------------------------------------------
|	Encapsulates the discrete gamma shape parameter.
*/
class DiscreteGammaShapeParam : public MCMCUpdater
	{
	public:
						DiscreteGammaShapeParam();
						virtual ~DiscreteGammaShapeParam();

		void			finalizeWorkingPrior();
        virtual void	sendCurrValueToModel(double v);
        virtual double	getCurrValueFromModel() const;
		virtual bool	update();				// override virtual from MCMCUpdater base class
		virtual double	operator()(double k);	// override pure virtual from AdHocDensity (base class of MCMCUpdater)
	};

/*----------------------------------------------------------------------------------------------------------------------
|	Encapsulates the proportion of invariable sites parameter.
*/
class PinvarParam : public MCMCUpdater
	{
	public:
						PinvarParam();
						virtual ~PinvarParam()
							{
							//std::cerr << "PinvarParam dying..." << std::endl;
							}

        virtual void	sendCurrValueToModel(double v);
        virtual double 	getCurrValueFromModel() const;
		virtual bool	update();				// override virtual from MCMCUpdater base class
		virtual double	operator()(double k);	// override pure virtual from AdHocDensity (base class of MCMCUpdater)
	};

/*----------------------------------------------------------------------------------------------------------------------
|	Encapsulates a state frequency parameter. More precisely, this class encapsulates a parameter that governs a state
|	frequency. Because all state frequencies must add to 1.0, the value of this parameter and the other parameter values
|	must be normalized so that the model has a correct set of state frequencies.
*/
class StateFreqParam : public MCMCUpdater
{
	public:
							StateFreqParam(unsigned w);
							virtual ~StateFreqParam();

        virtual void		sendCurrValueToModel(double v);
        virtual double      getCurrValueFromModel() const;
		virtual bool		update();				// override virtual from MCMCUpdater base class
		virtual double		operator()(double k);

	private:

		unsigned			which;
};

/*----------------------------------------------------------------------------------------------------------------------
|	Structure combining a vector of doubles (`fs') for storing a fitting sample and a probability distribution shared
|	pointer to store the working prior (`wp') that is parameterized using the fitting sample. This structure is used
|	as the value for a map (data member `edge_ref_dist') in EdgeLenMasterParam objects.
*/
struct EdgeWorkingPrior
	{
	double_vect_t	fs;		/**< vector of doubles representing samples upon which the `edge_ref_dist' will be based */
	ProbDistShPtr	wp;		/**< Gamma working prior distribution */
	};

/*----------------------------------------------------------------------------------------------------------------------
|	Encapsulates an edge length parameter. It is only used in the case of a fixed topology, otherwise edge lengths are
|	updated using a `LargetSimonMove' or a `BushMove' updater.
*/
class EdgeLenParam : public MCMCUpdater
	{
	public:
                                EdgeLenParam();
                                EdgeLenParam(unsigned i);
        virtual                 ~EdgeLenParam();

		bool                    isInternalEdge();
		bool                    isExternalEdge();

        std::string             getSplitReprAsString() const;

        virtual bool            computesTreeLengthPrior() const;
        virtual void            sendCurrValueToModel(double v);
        virtual double          getCurrValueFromModel() const;
		void                    setTreeNode(TreeNode & nd);
		virtual bool            update();				// override virtual from MCMCUpdater base class
		virtual double          operator()(double k);	// override pure virtual from AdHocDensity (base class of MCMCUpdater)

	//temp private:
		TreeNode *		_my_node;
        unsigned        _edge_len_index;                /**< is the index of this slave used when sending edge length values to the master */
	};

/*----------------------------------------------------------------------------------------------------------------------
|	Represents all edge lengths in the associated Tree. An edge length master parameter does not update any particular
|	edge length (that is left up to one of the defined moves, such as a `LargetSimonMove'); instead, its	job is to
|	compute the joint prior over all edge lengths. The update() member function and operator() of an EdgeLenMasterParam
|	are not overridden because they do nothing.
*/
class EdgeLenMasterParam : public MCMCUpdater
	{
	public:
        enum                EdgeLenType
                                {
                                internal,   /**> only computes prior for internal edges */
                                external,   /**> only computes prior for external edges */
                                both        /**> computes prior for all edges */
                                };

                            EdgeLenMasterParam(EdgeLenMasterParam::EdgeLenType t = EdgeLenMasterParam::both);
							virtual ~EdgeLenMasterParam();

        virtual bool        computesTreeLengthPrior() const;

		double				recalcWorkingPrior() const;
		double				lnWorkingPriorOneEdge(const TreeNode & nd, double v) const;
		std::string 		getWorkingPriorDescr() const;

		void				useEdgeSpecificWorkingPriors(bool use_it);

	protected:

		double				edgeLength(TreeNode & nd) const;
		double				lnPriorOneEdge(TreeNode & nd) const;

    private:

        EdgeLenType         				edgeLenType;   						/**> holds the edge length type, which determines for which edge lengths the prior is computed when recalcPrior is called */

		bool								use_edge_specific_ref_dists;	/**< if true, `edge_ref_dist' will be used; otherwise, a single generic working prior will be used for all edge lengths */
		std::map<Split,EdgeWorkingPrior>	edge_ref_dist;					/**< maps splits (keys) to EdgeWorkingPrior structs (values) so that the working prior distribution can be fetched given the split corresponding to any given node in the tree */
	};

typedef std::map<Split,EdgeWorkingPrior>::iterator			WorkingPriorMapIter;
typedef std::map<Split,EdgeWorkingPrior>::const_iterator	WorkingPriorMapConstIter;
typedef std::pair<Split,EdgeWorkingPrior> 					WorkingPriorMapPair;
typedef boost::shared_ptr<EdgeLenMasterParam> EdgeLenMasterParamShPtr;

/*----------------------------------------------------------------------------------------------------------------------
|	Represents the mean of the edge length prior. This parameter is used in models employing a hyperprior governing the
|	mean of the edge length priors. Whenever the HyperPriorParam value changes, the means of the priors affecting all
|	EdgeLenParam objects in the MCMCChainManager are reset to this new value.
*/
class HyperPriorParam : public MCMCUpdater
	{
	public:
							HyperPriorParam();
							enum EdgeTypeEnum {all, internal, external};
							HyperPriorParam(EdgeTypeEnum t);
							virtual ~HyperPriorParam();

		void				setEdgeLenMasterParam(EdgeLenMasterParamShPtr p) {edgelen_master_param = p;}
        virtual void		sendCurrValueToModel(double v);
        virtual double      getCurrValueFromModel() const;
		virtual bool		update();				// override virtual from MCMCUpdater base class
		virtual double		operator()(double k);

    private:

		EdgeTypeEnum				edge_type;				/**< governs the type of edges (internal, external, all) affected by this hyperparam */
        EdgeLenMasterParamShPtr		edgelen_master_param;   /**> is the edge length master parameter whose prior this parameter controls */
	};

} // namespace phycas

//#include "phycas/src/mcmc_param.inl"

#endif