/*
 * seqgen.cpp
 *
 *  Created on: Jun 23, 2015
 *      Author: joe
 */

#include <iostream>
#include <string>
#include <vector>
#include <nlopt.hpp>
#include <armadillo>
#include <random>
#include <numeric>

using namespace std;
//using namespace arma;
using arma::randn;
using arma::mat;
using arma::expmat;

#include "seq_gen.h"
#include "utils.h"
#include "node.h"
#include "tree.h"
#include "tree_reader.h"
#include "tree_utils.h"

// TODO: do we want this order?
/*Default Rate Matrix looks like this, I don't know why but I always go A,T,C,G
 *
 *    A   T   C   G
 * A -1  .33 .33 .33
 * T .33 -1  .33 .33
 * G .33 .33  -1 .33
 * G .33 .33 .33  -1
 */

// this should enable easy changing of order, if desired

map <char, int> SequenceGenerator::nuc_map_ = {
   {'A', 0},
   {'C', 1},
   {'G', 2},
   {'T', 3}
};

string SequenceGenerator::nucleotides_ = "ACGT";
string SequenceGenerator::amino_acids_ = "ARNDCQEGHILKMFPSTWYV";

map <char, int> SequenceGenerator::aa_map_ = {

   {'A', 0},
   {'R', 1},
   {'N', 2},
   {'D', 3},
   {'C', 4},
   {'Q', 5},
   {'E', 6},
   {'G', 7},
   {'H', 8},
   {'I', 9},
   {'L', 10},
   {'K', 11},
   {'M', 12},
   {'F', 13},
   {'P', 14},
   {'S', 15},
   {'T', 16},
   {'W', 17},
   {'Y', 18},
   {'V', 19}
};

/*
map <char, int> SequenceGenerator::nucMap = {
   {'A', 0},
   {'T', 1},
   {'C', 2},
   {'G', 3}
};
string SequenceGenerator::nucleotides = "ATCG";
*/

SequenceGenerator::SequenceGenerator (int const &seqlength, vector <double> const& basefreq,
    vector < vector<double> >& rmatrix, Tree * tree, bool const& showancs,
    int const& nreps, int const& seed, float const& alpha, float const& pinvar,
    string const& ancseq, bool const& printpost, vector<double> const& multirates,
    vector <double> const& aabasefreq, bool const& is_dna):tree_(tree),
    seqlen_(seqlength), nreps_(nreps), seed_(seed), alpha_(alpha), pinvar_(pinvar),
    root_sequence_(ancseq), base_freqs_(basefreq), aa_freqs_(aabasefreq), rmatrix_(rmatrix),
    multi_rates_(multirates), show_ancs_(showancs), print_node_labels_(printpost),
    multi_model_(false), is_dna_(is_dna)  {
    /*
     for (unsigned int i = 0; i < rmatrix.size(); i++) {
        for (unsigned int j = 0; j < rmatrix.size(); j++) {
            cout << rmatrix[i][j] << " ";
        }
        cout << "\n";
    }*/
    initialize();
    if (is_dna_) {
        nstates_ = 4;
    } else {
        nstates_ = 20;
    }
    // Print out the nodes names
    if (print_node_labels_) {
        label_internal_nodes();
        print_node_labels();
        exit(0);
    }
    preorder_tree_traversal();
}

// set all values
void SequenceGenerator::initialize () {
    // set the number generator being used
    if (seed_ != -1) { // user provided seed
        generator_ = mt19937(seed_);
    } else {
        generator_ = mt19937(get_clock_seed());
    }

    // construct uniform distribution from which random numbers will be generated
    // this happens to be the default distribution, but good to be explicit
    uniformDistrib_ = uniform_real_distribution<float>(0.0, 1.0);

    // construct gamma distribution (if necessary)
    if (alpha_ != -1.0) {
        gammaDistrib_ = gamma_distribution<float>(alpha_, (1/alpha_));
    }

    if (show_ancs_) {
        label_internal_nodes();
    }
    if (root_sequence_.length() == 0) {
        root_sequence_ = generate_random_sequence();
    } else {
        check_valid_sequence();
        // if root sequence is provided, set length to this
        seqlen_ = root_sequence_.size();
    }
    // set site-specific rate (pinvar and gamma)
    site_rates_ = set_site_rates();

    if (multi_rates_.size() != 0) {
        multi_model_ = true;
    }
}

/* Use the P matrix probabilities and randomly draw numbers to see
 * if each individual state will undergo some type of change
 */
string SequenceGenerator::simulate_sequence (string const& anc,
    vector < vector <double> >& QMatrix, float const& brlength) {
    std::vector<double>::iterator low;
    vector < vector <double> > PMatrix(nstates_, vector <double>(nstates_, 0.0));
    //int ancChar = 0;
    string newstring = anc; // instead of building, set size and replace
    for (int i = 0; i < seqlen_; i++) {
        float RandNumb = get_uniform_random_deviate();
        int ancChar = 0;
        if (is_dna_) {
            ancChar = nuc_map_[anc[i]];
        } else {
            ancChar = aa_map_[anc[i]];
            //cout << ancChar << endl;
        }
        float brnew = brlength * site_rates_[i];
        PMatrix = calculate_p_matrix(QMatrix, brnew);
        for (int i = 0; i < nstates_; i++) {
            // this calculates a cumulative sum
            std::partial_sum(PMatrix[i].begin(), PMatrix[i].end(), PMatrix[i].begin(), plus<double>());
        }
        low = std::lower_bound (PMatrix[ancChar].begin(), PMatrix[ancChar].end(), RandNumb);

        if (is_dna_) {
            newstring[i] = nucleotides_[low - PMatrix[ancChar].begin()];
        } else {
            newstring[i] = amino_acids_[low - PMatrix[ancChar].begin()];
        }
    }
    //cout << newstring << endl;
    return newstring;
}

/*
 * Calculate the Q Matrix (Substitution rate matrix)
 */
vector < vector <double> > SequenceGenerator::calculate_q_matrix () {

    vector < vector <double> > bigpi(nstates_, vector <double>(nstates_, 1.0));
    vector < vector <double> > t(nstates_, vector <double>(nstates_, 0.0));

    double tscale = 0.0;

    // doing the same looping multiple times here. simplify?
    for (unsigned int i = 0; i < rmatrix_.size(); i++) {
        for (unsigned int j = 0; j < rmatrix_.size(); j++) {
            if (i != j) {
                if (is_dna_) {
                    bigpi[i][j] *= base_freqs_[i] * base_freqs_[j] * rmatrix_[i][j];
                } else {
                    bigpi[i][j] *= aa_freqs_[i] * aa_freqs_[j] * rmatrix_[i][j];
                }
                tscale += bigpi[i][j];
            } else {
                bigpi[i][j] = 0.0;
            }
        }
    }
    for (unsigned int i = 0; i < rmatrix_.size(); i++) {
        for (unsigned int j = 0; j < rmatrix_.size(); j++) {
            if (i != j) {
                bigpi[i][j] /= tscale;
            } else {
                // set the diagnols to zero *** are they not set to zero above?
                bigpi[i][j] = 0.0;
            }
        }
    }
    for (unsigned int i = 0; i < rmatrix_.size(); i++) {
        double diag = 0.0;
        for (unsigned int j = 0; j < rmatrix_.size(); j++) {
            if (i != j) {
                diag -= bigpi[i][j];
            }
        }
        bigpi[i][i] = diag;
    }
    //Divide and Transpose
    for (unsigned int i = 0; i < rmatrix_.size(); i++) {
        for (unsigned int j = 0; j < rmatrix_.size(); j++) {
            if (is_dna_) {
                bigpi[i][j] /= base_freqs_[i];
            } else {
                bigpi[i][j] /= aa_freqs_[i];
            }
        }
    }
    return bigpi;
}

/* Calculate the P Matrix (Probability Matrix)
 * Changes to armadillos format then back I don't like the way could be more
 * efficient but yeah...
 */
vector < vector <double> > SequenceGenerator::calculate_p_matrix (vector < vector <double> > const& QMatrix,
    float br) {

    vector < vector <double> > Pmatrix(nstates_, vector <double>(nstates_, 0.0));
    mat A = randn<mat>(nstates_, nstates_);
    mat B = randn<mat>(nstates_, nstates_); // why not just copy A?
    int count = 0;
    //Q * t moved into Matrix form for armadillo
    for (unsigned int i = 0; i < QMatrix.size(); i++) {
        for (unsigned int j = 0; j < QMatrix.size(); j++) {
            A[count] = (QMatrix[i][j] * br);
            count++;
        }
    }
   //exponentiate the matrix
   B = expmat(A);
   //cout << B << endl;
   count = 0;
   //convert the matrix back to C++ vector
   for (unsigned int i = 0; i < Pmatrix.size(); i++) {
        for (unsigned int j = 0; j < Pmatrix.size(); j++) {
            Pmatrix[i][j] = B[count];
            count++;
        }
   }
   return Pmatrix;
}

/*
 * Pre-Order traversal works
 * Calculates the JC Matrix
 */
// TODO: how to name ancestor nodes (sequences)
//       - if we have this we can add to results (if desired))
void SequenceGenerator::preorder_tree_traversal () {

    double brlength = 0.0;
    int rate_count = 0;
    int check = 0;
    vector < vector <double> > QMatrix(nstates_, vector <double>(nstates_, 0.0));
    //vector < vector <double> > PMatrix(4, vector <double>(4, 0.0));
    // NOTE: this uses order: A,T,C,G
    if (multi_model_) {
        rmatrix_[0][2] = multi_rates_[0]; // A->C
        rmatrix_[2][0] = multi_rates_[0]; // C->A
        rmatrix_[0][3] = multi_rates_[1]; // A->G
        rmatrix_[3][0] = multi_rates_[1]; // G->A
        rmatrix_[0][1] = multi_rates_[2]; // A->T
        rmatrix_[1][0] = multi_rates_[2]; // T->A
        rmatrix_[2][3] = multi_rates_[3]; // C->G
        rmatrix_[3][2] = multi_rates_[3]; // G->C
        rmatrix_[1][2] = multi_rates_[4]; // C->T
        rmatrix_[2][1] = multi_rates_[4]; // T->C
        rmatrix_[1][3] = multi_rates_[5]; // G->T
        rmatrix_[3][1] = multi_rates_[5]; // T->G
        rmatrix_[0][0] = (multi_rates_[0]+multi_rates_[1]+multi_rates_[2]) * -1;
        rmatrix_[1][1] = (multi_rates_[2]+multi_rates_[4]+multi_rates_[5]) * -1;
        rmatrix_[2][2] = (multi_rates_[0]+multi_rates_[3]+multi_rates_[4]) * -1;
        rmatrix_[3][3] = (multi_rates_[1]+multi_rates_[3]+multi_rates_[5]) * -1;
        for (unsigned int i = 0; i < 6; i++) {
            multi_rates_.erase (multi_rates_.begin() + 0);
        }
        //QMatrix = calcQmatrix(rate_matrix);
        //QMatrix = calculate_q_matrix();
    }
    QMatrix = calculate_q_matrix();
    Node * root = tree_->getRoot();
    seqs_[root] = root_sequence_;
    ancq_[root] = QMatrix;

    if (show_ancs_) {
        string tname = root->getName();
        Sequence seq(tname, root_sequence_);
        res.push_back(seq);
    }

    // Pre-Order Traverse the tree
    for (int k = (tree_->getNodeCount() - 2); k >= 0; k--) {
        brlength = tree_->getNode(k)->getBL();
        /*
        for (unsigned int i = 0; i < QMatrix.size(); i++) {
            for (unsigned int j = 0; j < QMatrix.size(); j++) {
                cout << QMatrix[i][j] << " ";
            }
            cout << "\n";
        }
        cout << "\n";
        */
        if (multi_model_) {
            check = (int)round(multi_rates_[0]);
            //cout << check << " " << rate_count << endl;
            if (tree_->getNode(k)->isInternal() == true && multi_rates_.size() != 0) {
                if (check == rate_count) {
                    rmatrix_[0][2] = multi_rates_[1];
                    rmatrix_[2][0] = multi_rates_[1];
                    rmatrix_[0][3] = multi_rates_[2];
                    rmatrix_[3][0] = multi_rates_[2];
                    rmatrix_[0][1] = multi_rates_[3];
                    rmatrix_[1][0] = multi_rates_[3];
                    rmatrix_[2][3] = multi_rates_[4];
                    rmatrix_[3][2] = multi_rates_[4];
                    rmatrix_[1][2] = multi_rates_[5];
                    rmatrix_[2][1] = multi_rates_[5];
                    rmatrix_[1][3] = multi_rates_[6];
                    rmatrix_[3][1] = multi_rates_[6];
                    rmatrix_[0][0] = (multi_rates_[1]+multi_rates_[2]+multi_rates_[3]) * -1;
                    rmatrix_[1][1] = (multi_rates_[3]+multi_rates_[5]+multi_rates_[6]) * -1;
                    rmatrix_[2][2] = (multi_rates_[1]+multi_rates_[4]+multi_rates_[5]) * -1;
                    rmatrix_[3][3] = (multi_rates_[2]+multi_rates_[4]+multi_rates_[6]) * -1;

                    for (unsigned int i = 0; i < 7; i++) {
                        multi_rates_.erase(multi_rates_.begin() + 0);
                    }
                    //cout << "Size " << multi_rates_.size() << endl;
                    //cout << multi_rates_[0] << endl;
                    /*
                    for (unsigned int i = 0; i < multi_rates_.size(); i++) {
                        cout << multi_rates_[i] << endl;
                    }*/
                }
                rate_count++;
            }
        }
        QMatrix = calculate_q_matrix();
        //PMatrix = calculate_p_matrix(QMatrix, brlength);
        Node * dec = tree_->getNode(k);
        Node * parent = tree_->getNode(k)->getParent();
        //ancq[dec] = QMatrix;
        vector < vector <double> > Qparent = ancq_[parent];
        string ancSeq = seqs_[parent];
        string decSeq = simulate_sequence(ancSeq, Qparent, brlength);
        /*
        for (unsigned int i = 0; i < Qparent.size(); i++) {
            for (unsigned int j = 0; j < Qparent.size(); j++) {
                cout << Qparent[i][j] << " ";
            }
            cout << "\n";
        }
        cout << "\n";*/

        seqs_[dec] = decSeq;
        ancq_[dec] = QMatrix; // why store this?
        if (show_ancs_ && tree_->getNode(k)->isInternal() == true) {
            string tname = tree_->getNode(k)->getName();
            Sequence seq(tname, decSeq);
            res.push_back(seq);
        }
        // If its a tip print the name and the sequence
        if (tree_->getNode(k)->isInternal() != true) {
            string tname = tree_->getNode(k)->getName();
            Sequence seq(tname, decSeq);
            res.push_back(seq);
        }
    }
}

// this should probably be returned on its own
void SequenceGenerator::print_node_labels() {
    cout << getNewickString(tree_) << endl;
    //cout << tree_->getRoot()->getNewick(true) <<";" << endl;
}

void SequenceGenerator::label_internal_nodes() {

    int count = 1;
    string str = "Node";
    string nlabel = "";
    Node * root = tree_->getRoot();
    root->setName("Node_0");
    for (int k = (tree_->getNodeCount() - 2); k >= 0; k--) {
        if (tree_->getNode(k)->isInternal() == true) {
            //cout << k << endl;
            str = to_string(count);
            nlabel = "Node_" + str;
            tree_->getNode(k)->setName(nlabel);
            count++;
        }
    }
}

// involves both gamma and pinvar
vector <float> SequenceGenerator::set_site_rates () {
    vector <float> srates(seqlen_, 1.0);

    // invariable sites
    if (pinvar_ != 0.0) {
        int numsample = seqlen_ * pinvar_ + 0.5;
        // sample invariable sites
        vector <int> randsites = sample_without_replacement(seqlen_, numsample);
        // must be a more elegant way of doing this
        for (int i = 0; i < numsample; i++) {
            srates[randsites[i]] = 0.0;
        }
    }

    // gamma-distributed rate variation. could explore other distributions...
    if (alpha_ != -1.0) { // default i.e. no rate variation
        for (int i = 0; i < seqlen_; i++) {
            // want to skip over sites that are set to invariable
            if (srates[i] != 0.0) {
                srates[i] = get_gamma_random_deviate(alpha_);
            }
        }
    }
    return srates;
}

// initialized as string of length seqlength, all 'G'
string SequenceGenerator::generate_random_sequence () {

    string ancseq(seqlen_, 'G');
    if (is_dna_) {
        //string ancseq(seqlen, 'G');
        vector <double> cumsum(4);
        std::vector <double>::iterator low;
        // cumulative sum
        std::partial_sum(base_freqs_.begin(), base_freqs_.end(), cumsum.begin(), plus<double>());

        for (int i = 0; i < seqlen_; i++) {
            float RandNumb = get_uniform_random_deviate();
            low = std::lower_bound (cumsum.begin(), cumsum.end(), RandNumb);
            ancseq[i] = nucleotides_[low - cumsum.begin()];
        }
    } else {
        //string ancseq(seqlen, 'G');
        vector <double> cumsum(20);
        std::vector <double>::iterator low;
        // cumulative sum
        std::partial_sum(aa_freqs_.begin(), aa_freqs_.end(), cumsum.begin(), plus<double>());

        for (int i = 0; i < seqlen_; i++) {
            float RandNumb = get_uniform_random_deviate();
            low = std::lower_bound (cumsum.begin(), cumsum.end(), RandNumb);
            ancseq[i] = amino_acids_[low - cumsum.begin()];
        }
    }
    //cout << ancseq << endl;
    return ancseq;
}

// rates are in order: A<->C,A<->G,A<->T,C<->G,C<->T,G<->T
vector < vector <double> > SequenceGenerator::construct_rate_matrix (vector <double> const& rates) {

    // initialize
    vector < vector <double> > ratemat(nstates_, vector<double>(4, 0.33));

    // planning ahead here for potential non-reversible matrices
    if (rates.size() == 6) {
        ratemat[0][1] = rates[0];
        ratemat[1][0] = rates[0];
        ratemat[0][2] = rates[1];
        ratemat[2][0] = rates[1];
        ratemat[0][3] = rates[2];
        ratemat[3][0] = rates[2];
        ratemat[1][2] = rates[3];
        ratemat[2][1] = rates[3];
        ratemat[1][3] = rates[4];
        ratemat[3][1] = rates[4];
        ratemat[2][3] = rates[5];
        ratemat[3][2] = rates[5];

        ratemat[0][0] = (rates[0] + rates[1] + rates[2]) * -1;
        ratemat[1][1] = (rates[0] + rates[3] + rates[4]) * -1;
        ratemat[2][2] = (rates[1] + rates[3] + rates[5]) * -1;
        ratemat[3][3] = (rates[2] + rates[4] + rates[5]) * -1;

    } else {
        cout << "Er, we don't deal with " << rates.size() << " rates at the moment..." << endl;
        exit(0);
    }
    return ratemat;
}

// make sure sequence contains only valid nucleotide characters
void SequenceGenerator::check_valid_sequence () {
    // make sure uppercase
    root_sequence_ = string_to_upper(root_sequence_);
    if (is_dna_) {
        std::size_t found = root_sequence_.find_first_not_of(nucleotides_);
        if (found != std::string::npos) {
            cout << "Error: illegal character '" << root_sequence_[found] << "' at position "
                << found+1 << " (only A,C,G,T allowed). Maybe specify AA with -c? Exiting." << endl;
            exit(0);
        }
    } else {
        std::size_t found = root_sequence_.find_first_not_of(amino_acids_);
        if (found != std::string::npos) {
            cout << "Error: illegal character '" << root_sequence_[found] << "' at position "
                << found+1 << " (only AA chars allowed). Exiting." << endl;
            exit(0);
        }

    }
}

// not sure of a more elegant way to do this...
vector<Sequence> SequenceGenerator::get_sequences () {
    return res;
}

// call this whenever a random float is needed
float SequenceGenerator::get_uniform_random_deviate () {
    return uniformDistrib_(generator_);
}

float SequenceGenerator::get_gamma_random_deviate (float alpha) {
    return gammaDistrib_(generator_);
}

//SEQGEN::~SEQGEN() {
//    // TODO Auto-generated destructor stub
//}


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <qle/math/flatextrapolation.hpp>
#include <qle/termstructures/datedstrippedoptionletadapter.hpp>

#include <ql/math/interpolations/linearinterpolation.hpp>
#include <ql/termstructures/volatility/interpolatedsmilesection.hpp>

#include <algorithm>
#include <boost/make_shared.hpp>

using std::max;
using std::min;

namespace QuantExt {

DatedStrippedOptionletAdapter::DatedStrippedOptionletAdapter(const boost::shared_ptr<DatedStrippedOptionletBase>& s,
                                                             const bool flatExtrapolation)
    : OptionletVolatilityStructure(s->referenceDate(), s->calendar(), s->businessDayConvention(), s->dayCounter()),
      optionletStripper_(s), nInterpolations_(s->optionletMaturities()), strikeInterpolations_(nInterpolations_),
      flatExtrapolation_(flatExtrapolation) {
    registerWith(optionletStripper_);
}

boost::shared_ptr<SmileSection> DatedStrippedOptionletAdapter::smileSectionImpl(Time t) const {

    // Arbitrarily choose the first row of strikes for the smile section independent variable
    // Generally a reasonable choice since:
    // 1) OptionletStripper1: all strike rows are the same
    // 2) OptionletStripper2: optionletStrikes(i) is a decreasing sequence
    // Still possibility of arbitrary externally provided strike rows where (0) does not include all
    vector<Rate> optionletStrikes = optionletStripper_->optionletStrikes(0);
    vector<Real> stdDevs(optionletStrikes.size());
    Real tEff = flatExtrapolation_ ? std::min(t, optionletStripper_->optionletFixingTimes().back()) : t;
    for (Size i = 0; i < optionletStrikes.size(); ++i) {
        stdDevs[i] = volatilityImpl(tEff, optionletStrikes[i]) * sqrt(tEff);
    }

    // Use a linear interpolated smile section.
    // TODO: possibly make this configurable?
    if (flatExtrapolation_)
        return boost::make_shared<InterpolatedSmileSection<LinearFlat> >(t, optionletStrikes, stdDevs, Null<Real>(),
                                                                         LinearFlat(), Actual365Fixed(),
                                                                         volatilityType(), displacement());
    else
        return boost::make_shared<InterpolatedSmileSection<Linear> >(
            t, optionletStrikes, stdDevs, Null<Real>(), Linear(), Actual365Fixed(), volatilityType(), displacement());
}

Volatility DatedStrippedOptionletAdapter::volatilityImpl(Time length, Rate strike) const {
    calculate();

    vector<Volatility> vol(nInterpolations_);
    for (Size i = 0; i < nInterpolations_; ++i)
        vol[i] = strikeInterpolations_[i]->operator()(strike, true);

    const vector<Time>& optionletTimes = optionletStripper_->optionletFixingTimes();
    boost::shared_ptr<LinearInterpolation> timeInterpolator =
        boost::make_shared<LinearInterpolation>(optionletTimes.begin(), optionletTimes.end(), vol.begin());
    Real lengthEff = flatExtrapolation_ ? std::max(std::min(length, optionletStripper_->optionletFixingTimes().back()),
                                                   optionletStripper_->optionletFixingTimes().front())
                                        : length;
    return timeInterpolator->operator()(lengthEff, true);
}

void DatedStrippedOptionletAdapter::performCalculations() const {
    for (Size i = 0; i < nInterpolations_; ++i) {
        const vector<Rate>& optionletStrikes = optionletStripper_->optionletStrikes(i);
        const vector<Volatility>& optionletVolatilities = optionletStripper_->optionletVolatilities(i);
        boost::shared_ptr<Interpolation> tmp = boost::make_shared<LinearInterpolation>(
            optionletStrikes.begin(), optionletStrikes.end(), optionletVolatilities.begin());
        if (flatExtrapolation_)
            strikeInterpolations_[i] = boost::make_shared<FlatExtrapolation>(tmp);
        else
            strikeInterpolations_[i] = tmp;
    }
}

Rate DatedStrippedOptionletAdapter::minStrike() const {
    Rate minStrike = optionletStripper_->optionletStrikes(0).front();
    for (Size i = 1; i < nInterpolations_; ++i) {
        minStrike = min(optionletStripper_->optionletStrikes(i).front(), minStrike);
    }
    return minStrike;
}

Rate DatedStrippedOptionletAdapter::maxStrike() const {
    Rate maxStrike = optionletStripper_->optionletStrikes(0).back();
    for (Size i = 1; i < nInterpolations_; ++i) {
        maxStrike = max(optionletStripper_->optionletStrikes(i).back(), maxStrike);
    }
    return maxStrike;
}

Date DatedStrippedOptionletAdapter::maxDate() const { return optionletStripper_->optionletFixingDates().back(); }

VolatilityType DatedStrippedOptionletAdapter::volatilityType() const { return optionletStripper_->volatilityType(); }

Real DatedStrippedOptionletAdapter::displacement() const { return optionletStripper_->displacement(); }
} // namespace QuantExt


/**
 * @file
 * This file is part of SeisSol.
 *
 * @author Carsten Uphoff (c.uphoff AT tum.de, http://www5.in.tum.de/wiki/index.php/Carsten_Uphoff,_M.Sc.)
 * @author Sebastian Wolf (wolf.sebastian AT in.tum.de, https://www5.in.tum.de/wiki/index.php/Sebastian_Wolf,_M.Sc.)
 *
 * @section LICENSE
 * Copyright (c) 2015 - 2020, SeisSol Group
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived from this
 *    software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * @section DESCRIPTION
 **/

#ifndef MODEL_COMMON_HPP_
#define MODEL_COMMON_HPP_

#include <Eigen/Eigen>

#include "utils/logger.h"
#include "Initializer/typedefs.hpp"
#include "generated_code/init.h"
#include "Geometry/MeshTools.h"
#include "Numerical_aux/Transformation.h"

#include "Model/common_datastructures.hpp"

namespace seissol {
  namespace model {
    using Matrix99 = Eigen::Matrix<double, 9, 9>;

    bool testIfAcoustic(real mu);

    template<typename Tmaterial, typename Tmatrix>
    void getTransposedCoefficientMatrix( Tmaterial const& i_material,
                                         unsigned         i_dim,
                                         Tmatrix&         o_M )
    { o_M.setZero(); }

    template<typename Tmaterial, typename T>
    void getTransposedSourceCoefficientTensor(  Tmaterial const& material,
                                                T& E);

    template<typename Tmaterial, typename Tloc, typename Tneigh>
    void getTransposedGodunovState( Tmaterial const&  local,
                                    Tmaterial const&  neighbor,
                                    FaceType          faceType,
                                    Tloc&             QgodLocal,
                                    Tneigh&           QgodNeighbor );

    template<typename T>
    void getTransposedFreeSurfaceGodunovState( bool isAcoustic,
                                               T& QgodLocal,
                                               T& QgodNeighbor,
                                               Eigen::Matrix<double, 9, 9>& R);

    template<typename T>
    void getPlaneWaveOperator( T const& material,
                               double const n[3],
                               std::complex<double> Mdata[NUMBER_OF_QUANTITIES*NUMBER_OF_QUANTITIES] );

    template<typename T, typename S>
    void initializeSpecificLocalData( T const&,
                                      S* LocalData ) {}

    template<typename T, typename S>
    void initializeSpecificNeighborData(  T const&,
                                          S* NeighborData ) {}

    /*
     * Calculates the so called Bond matrix. Anisotropic materials are characterized by
     * 21 different material parameters. Due to the directional dependence of anisotropic
     * materials the parameters are not independet of the choice of the coordinate system.
     * The Bond matrix transforms materials from one orthogonal coordinate system to
     * another one.
     * c.f. 10.1111/j.1365-246X.2007.03381.x
     */
    void getBondMatrix( VrtxCoords const i_normal,
                        VrtxCoords const i_tangent1,
                        VrtxCoords const i_tangent2,
                        real* o_N );

    void getFaceRotationMatrix( VrtxCoords const i_normal,
                                VrtxCoords const i_tangent1,
                                VrtxCoords const i_tangent2,
                                init::T::view::type& o_T,
                                init::Tinv::view::type& o_Tinv );
  }
}

template<typename T>
void seissol::model::getPlaneWaveOperator(  T const& material,
                                            double const n[3],
                                            std::complex<double> Mdata[NUMBER_OF_QUANTITIES*NUMBER_OF_QUANTITIES] )
{
  yateto::DenseTensorView<2,std::complex<double>> M(Mdata, {NUMBER_OF_QUANTITIES, NUMBER_OF_QUANTITIES});
  M.setZero();

  double data[NUMBER_OF_QUANTITIES * NUMBER_OF_QUANTITIES];
  yateto::DenseTensorView<2,double> Coeff(data, {NUMBER_OF_QUANTITIES, NUMBER_OF_QUANTITIES});

  for (unsigned d = 0; d < 3; ++d) {
    Coeff.setZero();
    getTransposedCoefficientMatrix(material, d, Coeff);

    for (unsigned i = 0; i < NUMBER_OF_QUANTITIES; ++i) {
      for (unsigned j = 0; j < NUMBER_OF_QUANTITIES; ++j) {
        M(i,j) += n[d] * Coeff(j,i);
      }
    }
  }
}

template<typename T>
void seissol::model::getTransposedFreeSurfaceGodunovState( bool      isAcoustic,
                                                           T&        QgodLocal,
                                                           T&        QgodNeighbor,
                                                           Eigen::Matrix<double, 9, 9>& R)
{
  for (int i = 0; i < 9; i++) {
    for (int j = 0; j < 9; j++) {
      QgodNeighbor(i,j) = std::numeric_limits<double>::signaling_NaN();
    }
  }

  QgodLocal.setZero();
  if (isAcoustic) {
    // Acoustic material only has one traction (=pressure) and one velocity comp.
    // relevant to the Riemann problem
    QgodLocal(0, 6) = -1 * R(6,0) * 1/R(0,0); // S
    QgodLocal(6, 6) = 1.0;
  } else {
    std::array<int, 3> traction_indices = {0,3,5};
    std::array<int, 3> velocity_indices = {6,7,8};
    using Matrix33 = Eigen::Matrix<double, 3, 3>;
    Matrix33 R11 = R(traction_indices, {0,1,2});
    Matrix33 R21 = R(velocity_indices, {0,1,2});
    Matrix33 S = (-(R21 * R11.inverse())).eval();

    //set lower left block
    int row = 0;
    for (auto &t: traction_indices) {
      int col = 0;
      for (auto &v: velocity_indices) {
        QgodLocal(t, v) = S(row, col);
        col++;
      }
      row++;
    }

    //set lower right block
    for (auto &v : velocity_indices) {
      QgodLocal(v, v) = 1.0;
    }
  }
}

#endif


#include "hoCuNDArray_utils.h"
#include "radial_utilities.h"
#include "hoNDArray_fileio.h"
#include "cuNDArray.h"
#include "imageOperator.h"
#include "identityOperator.h"
#include "hoPartialDerivativeOperator.h"
#include "cuConebeamProjectionOperator.h"
#include "hoCuConebeamProjectionOperator.h"
#include "cuConvolutionOperator.h"
#include "hoCuNDArray_blas.h"
#include "hoCuNDArray_elemwise.h"
#include "hoCuNDArray_blas.h"
#include "cgSolver.h"
#include "CBCT_acquisition.h"
#include "complext.h"
#include "encodingOperatorContainer.h"
#include "vector_td_io.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuGPBBSolver.h"
#include "cuTvOperator.h"
#include "cuTvPicsOperator.h"
#include "cuNCGSolver.h"
#include "hoCuCgDescentSolver.h"
#include "hoNDArray_utils.h"
#include "hoCuPartialDerivativeOperator.h"
#include "cuDCTOperator.h"
#include "cuNDArray_fileio.h"
#include "cuATrousOperator.h"
#include "hdf5_utils.h"
#include <iostream>
#include <algorithm>
#include <sstream>
#include <math_constants.h>
#include <boost/program_options.hpp>
#include <cuTv1dOperator.h>
#include "dicomWriter.h"
//#include "cuDWTOperator.h"
#include "cuATvOperator.h"

using namespace std;
using namespace Gadgetron;

namespace po = boost::program_options;

boost::shared_ptr<cuNDArray<float> > calculate_prior(boost::shared_ptr<CBCT_binning>  binning,boost::shared_ptr<CBCT_acquisition> ps, hoCuNDArray<float>& projections, std::vector<size_t> is_dims, floatd3 imageDimensions){
	std::cout << "Calculating FDK prior" << std::endl;
	boost::shared_ptr<CBCT_binning> binning_pics=binning->get_3d_binning();
	std::vector<size_t> is_dims3d = is_dims;
	is_dims3d.pop_back();
	boost::shared_ptr< hoCuConebeamProjectionOperator >
	Ep( new hoCuConebeamProjectionOperator() );
	Ep->setup(ps,binning_pics,imageDimensions);
	Ep->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());
	Ep->set_domain_dimensions(&is_dims3d);
	Ep->set_use_filtered_backprojection(true);
	boost::shared_ptr<hoCuNDArray<float> > prior3d(new hoCuNDArray<float>(&is_dims3d));
	Ep->mult_MH(&projections,prior3d.get());

	hoCuNDArray<float> tmp_proj(*ps->get_projections());
	Ep->mult_M(prior3d.get(),&tmp_proj);
	float s = dot(ps->get_projections().get(),&tmp_proj)/dot(&tmp_proj,&tmp_proj);
	*prior3d *= s;
	boost::shared_ptr<cuNDArray<float> > prior(new cuNDArray<float>(*expand( prior3d.get(), is_dims.back() )));
	std::cout << "Prior complete" << std::endl;
	return prior;
}

int main(int argc, char** argv)
{
	string acquisition_filename;
	string outputFile;
	uintd3 imageSize;
	floatd3 voxelSize;
	int device;
	unsigned int downsamples;
	unsigned int iterations;
	float rho,dct_weight;
	po::options_description desc("Allowed options");

	desc.add_options()
    		("help", "produce help message")
    		("acquisition,a", po::value<string>(&acquisition_filename)->default_value("acquisition.hdf5"), "Acquisition data")
    		("samples,n",po::value<unsigned int>(),"Number of samples per ray")
    		("output,f", po::value<string>(&outputFile)->default_value("reconstruction.hdf5"), "Output filename")
    		("size,s",po::value<uintd3>(&imageSize)->default_value(uintd3(512,512,1)),"Image size in pixels")
    		("binning,b",po::value<string>(),"Binning file for 4d reconstruction")
    		("SAG","Use exact SAG correction if present")
    		("voxelSize,v",po::value<floatd3>(&voxelSize)->default_value(floatd3(0.488f,0.488f,1.0f)),"Voxel size in mm")
    		("dimensions,d",po::value<floatd3>(),"Image dimensions in mm. Overwrites voxelSize.")
    		("iterations,i",po::value<unsigned int>(&iterations)->default_value(10),"Number of iterations")
    		("TV,T",po::value<float>(),"TV Weight ")
					("ATV",po::value<float>(),"TV Weight ")
					("TV4D",po::value<float>(),"Total variation weight in temporal dimensions")
    		("PICS",po::value<float>(),"TV Weight of the prior image (Prior image compressed sensing)")
    		("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
    		("downsample,D",po::value<unsigned int>(&downsamples)->default_value(0),"Downsample projections this factor")
    		("rho",po::value<float>(&rho)->default_value(0.5f),"Rho-value for line search. Must be between 0 and 1. Smaller value means faster runtime, but less stable algorithm.")
    		("DCT",po::value<float>(&dct_weight)->default_value(0),"DCT regularization")
    		("use_prior","Use an FDK prior")
    		("Wavelet,W",po::value<float>(),"Wavelet weight")
    		("3D","Only use binning for selecting valid projections")
    		;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}

	std::stringstream command_line_string;
	std::cout << "Command line options:" << std::endl;
	for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
		boost::any a = it->second.value();
		command_line_string << it->first << ": ";
		if (a.type() == typeid(std::string)) command_line_string << it->second.as<std::string>();
		else if (a.type() == typeid(int)) command_line_string << it->second.as<int>();
		else if (a.type() == typeid(unsigned int)) command_line_string << it->second.as<unsigned int>();
		else if (a.type() == typeid(float)) command_line_string << it->second.as<float>();
		else if (a.type() == typeid(vector_td<float,3>)) command_line_string << it->second.as<vector_td<float,3> >();
		else if (a.type() == typeid(vector_td<int,3>)) command_line_string << it->second.as<vector_td<int,3> >();
		else if (a.type() == typeid(vector_td<unsigned int,3>)) command_line_string << it->second.as<vector_td<unsigned int,3> >();
		else command_line_string << "Unknown type" << std::endl;
		command_line_string << std::endl;
	}

	std::cout << command_line_string.str();

	cudaSetDevice(device);
	cudaDeviceReset();

	//Really weird stuff. Needed to initialize the device?? Should find real bug.
	cudaDeviceManager::Instance()->lockHandle();
	cudaDeviceManager::Instance()->unlockHandle();

	boost::shared_ptr<CBCT_acquisition> ps(new CBCT_acquisition());
	ps->load(acquisition_filename);
	ps->get_geometry()->print(std::cout);
	ps->downsample(downsamples);

	boost::shared_ptr<CBCT_binning> binning(new CBCT_binning());
	if (vm.count("binning")){
		std::cout << "Loading binning data" << std::endl;
		binning->load(vm["binning"].as<string>());
		if (vm.count("3D"))
			binning = binning->get_3d_binning();
	} else binning->set_as_default_3d_bin(ps->get_projections()->get_size(2));
	binning->print(std::cout);

	floatd3 imageDimensions;
	if (vm.count("dimensions")){
		imageDimensions = vm["dimensions"].as<floatd3>();
		voxelSize = imageDimensions/imageSize;
	}
	else imageDimensions = voxelSize*imageSize;

	std::vector<size_t> is_dims = to_std_vector((uint64d3)imageSize);

	std::cout << "IS dimensions " << is_dims[0] << " " << is_dims[1] << " " << is_dims[2] << std::endl;
	std::cout << "Image size " << imageDimensions << std::endl;

	is_dims.push_back(binning->get_number_of_bins());

	cuNCGSolver<float> solver;
	//gpBbSolver<cuNDArray<float>> solver;

	hoCuNDArray<float>* projections = ps->get_projections().get();
	std::cout << "Projection nrm" << nrm2(projections) << std::endl;
	boost::shared_ptr<cuNDArray<float>> prior;
	if (vm.count("use_prior")){
		prior = calculate_prior(binning,ps,*projections,is_dims,imageDimensions);
		solver.set_x0(prior);
	}

	// auto projections2 = ps->get_projections();
	//auto result = calculate_prior(binning,ps,*projections2,is_dims,imageDimensions);
	// Define encoding matrix
	boost::shared_ptr< cuConebeamProjectionOperator >
	E( new cuConebeamProjectionOperator() );

	E->setup(ps,binning,imageDimensions);
	E->set_domain_dimensions(&is_dims);
	E->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());

	//hoCuGPBBSolver<float> solver;
	//hoCuCgDescentSolver<float> solver;

	solver.set_encoding_operator(E);
	solver.set_domain_dimensions(&is_dims);
	solver.set_max_iterations(iterations);
	solver.set_output_mode(hoCuGPBBSolver<float>::OUTPUT_VERBOSE);
	solver.set_non_negativity_constraint(true);
	solver.set_rho(rho);

	cuNDArray<float> cuproj(*projections);
	if (E->get_use_offset_correction())
		E->offset_correct(&cuproj);

	if (vm.count("TV")){
		std::cout << "Total variation regularization in use" << std::endl;
		boost::shared_ptr<cuTvOperator<float,3> > tv(new cuTvOperator<float,3>);
		tv->set_weight(vm["TV"].as<float>());
		solver.add_nonlinear_operator(tv);
		/*
    boost::shared_ptr<hoCuTvOperator<float,4> > tv2(new hoCuTvOperator<float,4>);
    tv2->set_step(2);
    tv2->set_weight(vm["TV"].as<float>());
    solver.add_nonlinear_operator(tv2);
    boost::shared_ptr<hoCuTvOperator<float,4> > tv3(new hoCuTvOperator<float,4>);
    tv3->set_step(3);
    tv3->set_weight(vm["TV"].as<float>());
    solver.add_nonlinear_operator(tv3);
		 */

	}

	if (vm.count("ATV")){
		std::cout << "Advanced Total variation regularization in use" << std::endl;
		boost::shared_ptr<cuATvOperator<float,3> > tv(new cuATvOperator<float,3>);
		tv->set_weight(vm["ATV"].as<float>());
		solver.add_nonlinear_operator(tv);
		/*
    boost::shared_ptr<hoCuTvOperator<float,4> > tv2(new hoCuTvOperator<float,4>);
    tv2->set_step(2);
    tv2->set_weight(vm["TV"].as<float>());
    solver.add_nonlinear_operator(tv2);
    boost::shared_ptr<hoCuTvOperator<float,4> > tv3(new hoCuTvOperator<float,4>);
    tv3->set_step(3);
    tv3->set_weight(vm["TV"].as<float>());
    solver.add_nonlinear_operator(tv3);
		 */

	}

	if (vm.count("TV4D")) {
		std::cout << "Total variation 4d regularization in use" << std::endl;
		boost::shared_ptr<cuTv1DOperator<float, 4> > tv4d(new cuTv1DOperator<float, 4>);
		tv4d->set_weight(vm["TV4D"].as<float>());
		solver.add_nonlinear_operator(tv4d);
	}

	if (vm.count("Wavelet")){
		//auto wave=  boost::make_shared<cuATrousOperator<float>>();
//		wave->set_domain_dimensions(&is_dims);
		//wave->set_levels({2,2,2,2});
		auto wave=  boost::make_shared<cuDWTOperator<float,3>>();
		wave->set_domain_dimensions(&is_dims);
		wave->set_codomain_dimensions(&is_dims);
		wave->set_levels(3);
		wave->set_weight(vm["Wavelet"].as<float>());
		solver.add_regularization_operator(wave,1);
	}

	if (dct_weight > 0){
		auto dctOp = boost::make_shared<cuDCTOperator<float>>();
		dctOp->set_domain_dimensions(&is_dims);
		dctOp->set_weight(dct_weight);
		solver.add_regularization_operator(dctOp);
	}

  if (vm.count("PICS")){
    std::cout << "PICS in use" << std::endl;
    if (!prior) prior = calculate_prior(binning,ps,*projections,is_dims,imageDimensions);
    boost::shared_ptr<cuTvPicsOperator<float,3> > pics (new cuTvPicsOperator<float,3>);
    pics->set_prior(prior);
    pics->set_weight(vm["PICS"].as<float>());
    solver.add_nonlinear_operator(pics);
    solver.set_x0(prior);
  }

	auto result = solver.solve(&cuproj);

	write_dicom(result.get(),command_line_string.str(),imageDimensions);
	//write_nd_array( result.get(), outputFile.c_str());
	saveNDArray2HDF5(result.get(),outputFile,imageDimensions,vector_td<float,3>(0),command_line_string.str(),iterations);

}


//#include <sba/sba.h>
#include <Eigen/SVD>
#include <Eigen/LU>
#include <iostream>
#include <limits>
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"

namespace ndt_feature_reg
{

template <typename PointSource, typename PointTarget>
PoseEstimator<PointSource,PointTarget>::PoseEstimator(int NRansac,
        double maxidx, double maxidd)
{
    numRansac = NRansac;
    maxInlierXDist2 = maxidx*maxidx;
    maxInlierDDist2 = maxidd*maxidd;
    rot.setIdentity();
    trans.setZero();

    // matcher
    matcher = new cv::BFMatcher(cv::NORM_L2);
    wx = 92;
    wy = 48;
    windowed = false;
    projectMatches = true;

    maxDist = std::numeric_limits<double>::max();
    minDist = -1.;
}

template <typename PointSource, typename PointTarget>
void
PoseEstimator<PointSource,PointTarget>::matchFrames(const NDTFrame<PointSource>& f0, const NDTFrame<PointTarget>& f1, std::vector<cv::DMatch>& fwd_matches)
{
    cv::Mat mask;
    if (windowed)
        mask = cv::windowedMatchingMask(f0.kpts, f1.kpts, wx, wy);

    //require at least 3 kpts each
    if(f0.kpts.size() > 3 && f1.kpts.size() > 3)
    {
	matcher->match(f0.dtors, f1.dtors, fwd_matches, mask);
    }
}

template <typename PointSource, typename PointTarget>
size_t PoseEstimator<PointSource,PointTarget>::estimate(const NDTFrame<PointSource> &f0, const NDTFrame<PointTarget> &f1)
{
    // set up match lists
    matches.clear();
    inliers.clear();

//     std::cout<<"N KPTS: "<<f0.kpts.size()<<" "<<f1.kpts.size()<<std::endl;
//     std::cout<<"descriptors: "<<f0.dtors.size().width<<"x"<<f0.dtors.size().height<<" "
//			       <<f1.dtors.size().width<<"x"<<f1.dtors.size().height<<std::endl;
    // do forward and reverse matches
    std::vector<cv::DMatch> fwd_matches, rev_matches;
    matchFrames(f0, f1, fwd_matches);
    matchFrames(f1, f0, rev_matches);
//     printf("**** Forward matches: %d, reverse matches: %d ****\n", (int)fwd_matches.size(), (int)rev_matches.size());

    // combine unique matches into one list
    for (int i = 0; i < (int)fwd_matches.size(); ++i)
    {
        if (fwd_matches[i].trainIdx >= 0)
            matches.push_back( cv::DMatch(i, fwd_matches[i].trainIdx, fwd_matches[i].distance) );
        //matches.push_back( cv::DMatch(fwd_matches[i].queryIdx, fwd_matches[i].trainIdx, fwd_matches[i].distance) );
    }
    for (int i = 0; i < (int)rev_matches.size(); ++i)
    {
        if (rev_matches[i].trainIdx >= 0 && i != fwd_matches[rev_matches[i].trainIdx].trainIdx)
            matches.push_back( cv::DMatch(rev_matches[i].trainIdx, i, rev_matches[i].distance) );
        //matches.push_back( cv::DMatch(rev_matches[i].trainIdx, rev_matches[i].queryIdx, rev_matches[i].distance) );
    }
//     printf("**** Total unique matches: %d ****\n", (int)matches.size());

    // do it
    return estimate(f0, f1, matches);
}

template <typename PointSource, typename PointTarget>
size_t PoseEstimator<PointSource,PointTarget>::estimate(const NDTFrame<PointSource>& f0, const NDTFrame<PointTarget>& f1,
        const std::vector<cv::DMatch> &matches)
{
    // convert keypoints in match to 3d points
    std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > p0; // homogeneous coordinates
    std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > p1;

    int nmatch = matches.size();
    //srand(getDoubleTime());

    // set up data structures for fast processing
    // indices to good matches
    std::vector<int> m0, m1;
    for (int i=0; i<nmatch; i++)
    {
        m0.push_back(matches[i].queryIdx);
        m1.push_back(matches[i].trainIdx);
        //std::cout<<m0[i]<<" "<<m1[i]<<std::endl;
    }

    nmatch = m0.size();

    if (nmatch < 3) return 0;   // can't do it...

    int bestinl = 0;

    // RANSAC loop
//#pragma omp parallel for shared( bestinl )
    for (int i=0; i<numRansac; i++)
    {
        //std::cout << "ransac loop : " << i << std::endl;
        // find a candidate
        int a=rand()%nmatch;
        int b = a;
        while (a==b)
            b=rand()%nmatch;
        int c = a;
        while (a==c || b==c)
            c=rand()%nmatch;

        int i0a = m0[a];
        int i0b = m0[b];
        int i0c = m0[c];
        int i1a = m1[a];
        int i1b = m1[b];
        int i1c = m1[c];

        if (i0a == i0b || i0a == i0c || i0b == i0c ||
                i1a == i1b || i1a == i1c || i1b == i1c)
            continue;

        //std::cout<<a<<" "<<b<<" "<<c<<std::endl;
        //std::cout<<i0a<<" "<<i0b<<" "<<i0c<<std::endl;
        //std::cout<<i1a<<" "<<i1b<<" "<<i1c<<std::endl;

        // get centroids
        Eigen::Vector3d p0a = f0.pts[i0a].head(3);
        Eigen::Vector3d p0b = f0.pts[i0b].head(3);
        Eigen::Vector3d p0c = f0.pts[i0c].head(3);
        Eigen::Vector3d p1a = f1.pts[i1a].head(3);
        Eigen::Vector3d p1b = f1.pts[i1b].head(3);
        Eigen::Vector3d p1c = f1.pts[i1c].head(3);

        Eigen::Vector3d c0 = (p0a+p0b+p0c)*(1.0/3.0);
        Eigen::Vector3d c1 = (p1a+p1b+p1c)*(1.0/3.0);

        //std::cout<<c0.transpose()<<std::endl;
        //std::cout<<c1.transpose()<<std::endl;
        // subtract out
        p0a -= c0;
        p0b -= c0;
        p0c -= c0;
        p1a -= c1;
        p1b -= c1;
        p1c -= c1;

        Eigen::Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
                            p1c*p0c.transpose();

        // do the SVD thang
        Eigen::JacobiSVD<Eigen::Matrix3d> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
        Eigen::Matrix3d V = svd.matrixV();
        Eigen::Matrix3d R = V * svd.matrixU().transpose();
        double det = R.determinant();
        //ntot++;
        if (det < 0.0)
        {
            //nneg++;
            V.col(2) = V.col(2)*-1.0;
            R = V * svd.matrixU().transpose();
        }
        Eigen::Vector3d tr = c0-R*c1;    // translation

        // transformation matrix, 3x4
        Eigen::Matrix<double,3,4> tfm;
        //        tfm.block<3,3>(0,0) = R.transpose();
        //        tfm.col(3) = -R.transpose()*tr;
        tfm.block<3,3>(0,0) = R;
        tfm.col(3) = tr;

#if 0
        // find inliers, based on image reprojection
        int inl = 0;
        for (int i=0; i<nmatch; i++)
        {
            Vector3d pt = tfm*f1.pts[m1[i]];
            Vector3d ipt = f0.cam2pix(pt);
            const cv::KeyPoint &kp = f0.kpts[m0[i]];
            double dx = kp.pt.x - ipt.x();
            double dy = kp.pt.y - ipt.y();
            double dd = f0.disps[m0[i]] - ipt.z();
            if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
                    dd*dd < maxInlierDDist2)
            {
                inl+=(int)sqrt(ipt.z()); // clever way to weight closer points
//		 inl+=(int)sqrt(ipt.z()/matches[i].distance);
//		 cout << "matches[i].distance : " << matches[i].distance << endl;
//		 inl++;
            }
        }
#endif
        int inl = 0;
        for (int i=0; i<nmatch; i++)
        {
            Eigen::Vector3d pt1 = tfm*f1.pts[m1[i]];
            Eigen::Vector3d pt0 = f0.pts[m0[i]].head(3);

//	       double z = fabs(pt1.z() - pt0.z())*0.5;
            double z = pt1.z();
            double dx = pt1.x() - pt0.x();
            double dy = pt1.y() - pt0.y();
            double dd = pt1.z() - pt0.z();

            if (projectMatches)
            {
                // The central idea here is to divide by the distance (this is essentially what cam2pix does).
                dx = dx / z;
                dy = dy / z;
            }
            if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
                    dd*dd < maxInlierDDist2)
            {
//----		    inl+=(int)sqrt(pt0.z()); // clever way to weight closer points
//		 inl+=(int)sqrt(ipt.z()/matches[i].distance);
//		 cout << "matches[i].distance : " << matches[i].distance << endl;
                inl++;
            }
        }

//#pragma omp critical
        if (inl > bestinl)
        {
            bestinl = inl;
            rot = R;
            trans = tr;
//	       std::cout << "bestinl : " << bestinl << std::endl;
        }
    }

    //printf("Best inliers: %d\n", bestinl);
    //printf("Total ransac: %d  Neg det: %d\n", ntot, nneg);

    // reduce matches to inliers
    std::vector<cv::DMatch> inls;    // temporary for current inliers
    inliers.clear();
    Eigen::Matrix<double,3,4> tfm;
    tfm.block<3,3>(0,0) = rot;
    tfm.col(3) = trans;

    //std::cout<<"f0: "<<f0.pts.size()<<" "<<f0.kpts.size()<<" "<<f0.pc_kpts.size()<<std::endl;
    //std::cout<<"f1: "<<f1.pts.size()<<" "<<f1.kpts.size()<<" "<<f1.pc_kpts.size()<<std::endl;

    nmatch = matches.size();
    for (int i=0; i<nmatch; i++)
    {
        Eigen::Vector3d pt1 = tfm*f1.pts[matches[i].trainIdx];
        //Eigen::Vector3d pt1_unchanged = f1.pts[matches[i].trainIdx].head(3);
        //Vector3d pt1 = pt1_unchanged;
#if 0
        Vector3d ipt = f0.cam2pix(pt);
        const cv::KeyPoint &kp = f0.kpts[matches[i].queryIdx];
        double dx = kp.pt.x - ipt.x();
        double dy = kp.pt.y - ipt.y();
        double dd = f0.disps[matches[i].queryIdx] - ipt.z();
#endif
        Eigen::Vector3d pt0 = f0.pts[matches[i].queryIdx].head(3);

        //double z = fabs(pt1.z() - pt0.z())*0.5;
        double z = pt1.z();
        double dx = pt1.x() - pt0.x();
        double dy = pt1.y() - pt0.y();
        double dd = pt1.z() - pt0.z();

        if (projectMatches)
        {
            // The central idea here is to divide by the distance (this is essentially what cam2pix does).
            dx = dx / z;
            dy = dy / z;
        }

        if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
                dd*dd < maxInlierDDist2)
        {
            if (z < maxDist && z > minDist)

//	       if (fabs(f0.kpts[matches[i].queryIdx].pt.y - f1.kpts[matches[i].trainIdx].pt.y) > 300)
            {
//		   std::cout << " ---------- " << dx << "," << dy << "," << dd << ",\npt0 " << pt0.transpose() << "\npt1 " << pt1.transpose() << f0.kpts[matches[i].queryIdx].pt << "," <<
//			 f1.kpts[matches[i].trainIdx].pt << "\n unchanged pt1 " << pt1_unchanged.transpose() << std::endl;
                inliers.push_back(matches[i]);
            }
        }
    }

#if 0
    // Test with the SBA...
    {
        // system
        SysSBA sba;
        sba.verbose = 0;

#if 0
        // set up nodes
        // should have a frame => node function
        Vector4d v0 = Vector4d(0,0,0,1);
        Quaterniond q0 = Quaternion<double>(Vector4d(0,0,0,1));
        sba.addNode(v0, q0, f0.cam, true);

        Quaterniond qr1(rot);   // from rotation matrix
        Vector4d temptrans = Vector4d(trans(0), trans(1), trans(2), 1.0);

        //        sba.addNode(temptrans, qr1.normalized(), f1.cam, false);
        qr1.normalize();
        sba.addNode(temptrans, qr1, f1.cam, false);

        int in = 3;
        if (in > (int)inls.size())
            in = inls.size();

        // set up projections
        for (int i=0; i<(int)inls.size(); i++)
        {
            // add point
            int i0 = inls[i].queryIdx;
            int i1 = inls[i].trainIdx;
            Vector4d pt = f0.pts[i0];
            sba.addPoint(pt);

            // projected point, ul,vl,ur
            Vector3d ipt;
            ipt(0) = f0.kpts[i0].pt.x;
            ipt(1) = f0.kpts[i0].pt.y;
            ipt(2) = ipt(0)-f0.disps[i0];
            sba.addStereoProj(0, i, ipt);

            // projected point, ul,vl,ur
            ipt(0) = f1.kpts[i1].pt.x;
            ipt(1) = f1.kpts[i1].pt.y;
            ipt(2) = ipt(0)-f1.disps[i1];
            sba.addStereoProj(1, i, ipt);
        }

        sba.huber = 2.0;
        sba.doSBA(5,10e-4,SBA_DENSE_CHOLESKY);
        int nbad = sba.removeBad(2.0); // 2.0
        cout << endl << "Removed " << nbad << " projections > 2 pixels error" << endl;
        sba.doSBA(5,10e-5,SBA_DENSE_CHOLESKY);

//        cout << endl << sba.nodes[1].trans.transpose().head(3) << endl;

        // get the updated transform
        trans = sba.nodes[1].trans.head(3);
        Quaterniond q1;
        q1 = sba.nodes[1].qrot;
        quat = q1;
        rot = q1.toRotationMatrix();

        // set up inliers
        inliers.clear();
        for (int i=0; i<(int)inls.size(); i++)
        {
            ProjMap &prjs = sba.tracks[i].projections;
            if (prjs[0].isValid && prjs[1].isValid) // valid track
                inliers.push_back(inls[i]);
        }

        printf("Inliers: %d   After polish: %d\n", (int)inls.size(), (int)inliers.size());
#endif
    }
#endif

//     std::cout << std::endl << trans.transpose().head(3) << std::endl << std::endl;
//     std::cout << rot << std::endl;

    return inliers.size();
}
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    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.
 */

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>

#include <Eigen/Core>

#include <tudat/astro/basic_astro/physicalConstants.h>
#include <tudat/basics/testMacros.h>
#include <tudat/math/basic/mathematicalConstants.h>
#include "tudat/astro/basic_astro/unitConversions.h"
#include <tudat/astro/basic_astro/orbitalElementConversions.h>

#include <tudat/io/basicInputOutput.h>
#include <tudat/io/applicationOutput.h>

#include "tudat/astro/ephemerides/approximatePlanetPositions.h"
#include "tudat/astro/trajectory_design/trajectory.h"
#include "tudat/astro/trajectory_design/exportTrajectory.h"
#include "tudat/astro/trajectory_design/planetTrajectory.h"

int main( )
{
    using namespace tudat;
    using namespace tudat::input_output;
    using namespace tudat::input_output::parsed_data_vector_utilities;
    using namespace tudat::transfer_trajectories;

    //////////////////////////////////////////////////////////////////////////
    ////////////////////////// CASSSINI //////////////////////////////////////
    //////////////////////////////////////////////////////////////////////////

    // Specify required parameters
    // Specify the number of legs and type of legs.
    int numberOfLegs = 6;
    std::vector< TransferLegType > legTypeVector;
    legTypeVector.resize( numberOfLegs );
    legTypeVector[ 0 ] = mga_Departure;
    legTypeVector[ 1 ] = mga_Swingby;
    legTypeVector[ 2 ] = mga_Swingby;
    legTypeVector[ 3 ] = mga_Swingby;
    legTypeVector[ 4 ] = mga_Swingby;
    legTypeVector[ 5 ] = capture;

    // Create the ephemeris vector.
    std::vector< ephemerides::EphemerisPointer >
            ephemerisVector( numberOfLegs );
    ephemerisVector[ 0 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::earthMoonBarycenter );
    ephemerisVector[ 1 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::venus );
    ephemerisVector[ 2 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::venus );
    ephemerisVector[ 3 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::earthMoonBarycenter );
    ephemerisVector[ 4 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::jupiter );
    ephemerisVector[ 5 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::saturn );

    // Create gravitational parameter vector
    Eigen::VectorXd gravitationalParameterVector( numberOfLegs );
    gravitationalParameterVector << 3.9860119e14, 3.24860e14, 3.24860e14, 3.9860119e14, 1.267e17, 3.79e16;

    // Create variable vector.
    Eigen::VectorXd variableVector( numberOfLegs + 1 );
    variableVector << -789.8117, 158.302027105278, 449.385873819743, 54.7489684339665,
            1024.36205846918, 4552.30796805542, 1/*dummy*/;
    variableVector *= physical_constants::JULIAN_DAY;

    // Create departure and capture variables.
    Eigen::VectorXd semiMajorAxes( 2 ), eccentricities( 2 );
    semiMajorAxes << std::numeric_limits< double >::infinity( ), 1.0895e8 / 0.02;
    eccentricities << 0.0, 0.98;

    // Sun gravitational parameter
    const double sunGravitationalParameter = 1.32712428e20;

    // Create minimum pericenter radii vector
    Eigen::VectorXd minimumPericenterRadii( numberOfLegs );
    minimumPericenterRadii << 6778000.0, 6351800.0, 6351800.0, 6778000.0, 600000000.0, 600000000.0;

    // Create the trajectory problem.
    Trajectory Cassini1( numberOfLegs, legTypeVector, ephemerisVector,
                         gravitationalParameterVector, variableVector, sunGravitationalParameter,
                         minimumPericenterRadii, semiMajorAxes, eccentricities );

    // Vectors for the specific maneuvers and the total delta v
    std::vector< Eigen::Vector3d > positionVector;
    std::vector< double > timeVector;
    std::vector< double > deltaVVector;
    double resultingDeltaV;

    // Calculate the orbits
    Cassini1.calculateTrajectory( resultingDeltaV );
    Cassini1.maneuvers( positionVector, timeVector, deltaVVector );

    std::cout << " Cassini Mission: " << std::endl;
    std::cout << " Total Delta V needed: " << resultingDeltaV <<std::endl;
    std::cout << " Time of Earth departure: " << timeVector[ 0 ]
              << ". Delta V needed at Earth: " << deltaVVector[ 0 ] << std::endl;
    std::cout << " Time of Venus visit: " << timeVector[ 1 ]
              << ". Delta V needed at Venus: " << deltaVVector[ 1 ] << std::endl;
    std::cout << " Time of second Venus visit: " << timeVector[ 2 ]
              << ". Delta V needed at Venus: " << deltaVVector[ 2 ] << std::endl;
    std::cout << " Time of Earth visit: " << timeVector[ 3 ]
              << ". Delta V needed at Earth: " << deltaVVector[ 3 ] << std::endl;
    std::cout << " Time of Jupiter visit: " << timeVector[ 4 ]
              << ". Delta V needed at Jupiter: " << deltaVVector[ 4 ] << std::endl;
    std::cout << " Time of Saturn capture: " << timeVector[ 5 ]
              << ". Delta V needed at Saturn: " << deltaVVector[ 5 ] << std::endl;

    std::cout << std::endl << std::endl;

    //////////////////////////////////////////////////////////////////////////
    ////////////////////////// MESSENGER /////////////////////////////////////
    //////////////////////////////////////////////////////////////////////////

    // Specify required parameters
    // Specify the number of legs and type of legs.
    numberOfLegs = 5;
    legTypeVector.resize( numberOfLegs );
    legTypeVector[ 0 ] = mga1DsmVelocity_Departure;
    legTypeVector[ 1 ] = mga1DsmVelocity_Swingby;
    legTypeVector[ 2 ] = mga1DsmVelocity_Swingby;
    legTypeVector[ 3 ] = mga1DsmVelocity_Swingby;
    legTypeVector[ 4 ] = capture;

    // Create the ephemeris vector.
    ephemerisVector.resize( numberOfLegs );
    ephemerisVector[ 0 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::earthMoonBarycenter );
    ephemerisVector[ 1 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::earthMoonBarycenter );
    ephemerisVector[ 2 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::venus );
    ephemerisVector[ 3 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::venus );
    ephemerisVector[ 4 ] = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::mercury );

    // Create gravitational parameter vector
    gravitationalParameterVector.resize( numberOfLegs );
    gravitationalParameterVector << 3.9860119e14, 3.9860119e14, 3.24860e14, 3.24860e14, 2.2321e13;

    // Create variable vector.
    variableVector.resize( numberOfLegs /*time of flight*/ + 1 /*start epoch*/ +
                           4 * ( numberOfLegs - 1 ) /*additional variables for model, except the final capture leg*/ );

    // Add the time of flight and start epoch, which are in JD.
    variableVector << 1171.64503236 * physical_constants::JULIAN_DAY,
            399.999999715 * physical_constants::JULIAN_DAY,
            178.372255301 * physical_constants::JULIAN_DAY,
            299.223139512 * physical_constants::JULIAN_DAY,
            180.510754824 * physical_constants::JULIAN_DAY,
            1, // The capture time is irrelevant for the final leg.
            // Add the additional variables.
            0.234594654679, 1408.99421278, 0.37992647165 * 2 * 3.14159265358979,
            std::acos(  2 * 0.498004040298 - 1. ) - 3.14159265358979 / 2, // 1st leg.
            0.0964769387134, 1.35077257078, 1.80629232251 * 6.378e6, 0.0, // 2nd leg.
            0.829948744508, 1.09554368115, 3.04129845698 * 6.052e6, 0.0, // 3rd leg.
            0.317174785637, 1.34317576594, 1.10000000891 * 6.052e6, 0.0; // 4th leg.

    // Create minimum pericenter radii vector
    minimumPericenterRadii.resize( numberOfLegs );
    minimumPericenterRadii << TUDAT_NAN, TUDAT_NAN, TUDAT_NAN, TUDAT_NAN, TUDAT_NAN;

    // Create departure and capture variables.
    semiMajorAxes << std::numeric_limits< double >::infinity( ),
            std::numeric_limits< double >::infinity( );
    eccentricities << 0.0, 0.0;

    // Create the trajectory problem.
    Trajectory Messenger( numberOfLegs, legTypeVector, ephemerisVector,
                          gravitationalParameterVector, variableVector, sunGravitationalParameter,
                          minimumPericenterRadii, semiMajorAxes, eccentricities );

    // Vectors for the specific maneuvers and the total delta v
    std::vector< Eigen::Vector3d > positionVectorMessenger;
    std::vector< double > timeVectorMessenger;
    std::vector< double > deltaVVectorMessenger;
    double resultingDeltaVMessenger;

    // Calculate the orbits
    Messenger.calculateTrajectory( resultingDeltaVMessenger );
    Messenger.maneuvers( positionVectorMessenger, timeVectorMessenger, deltaVVectorMessenger );

    std::cout << " Messenger Mission: " << std::endl;
    std::cout << " Total Delta V: " << resultingDeltaVMessenger <<std::endl;
    std::cout << " Time of Earth departure: " << timeVectorMessenger[ 0 ]
              << ". Delta V needed at Earth: " << deltaVVectorMessenger[ 0 ] << std::endl;
    std::cout << " Time of 1st DSM: " << timeVectorMessenger[ 1 ]
              << ". Delta V needed for 1st DSM: " << deltaVVectorMessenger[ 1 ] << std::endl;
    std::cout << " Time of second Earth visit: " << timeVectorMessenger[ 2 ]
              << ". Delta V needed at Earth: " << deltaVVectorMessenger[ 2 ] << std::endl;
    std::cout << " Time of 2nd DSM: " << timeVectorMessenger[ 3 ]
              << ". Delta V needed for 2nd DSM: " << deltaVVectorMessenger[ 3 ] << std::endl;
    std::cout << " Time of Venus visit: " << timeVectorMessenger[ 4 ]
              << ". Delta V needed at Venus: " << deltaVVectorMessenger[ 4 ] << std::endl;
    std::cout << " Time of 3d DSM: " << timeVectorMessenger[ 5 ]
              << ". Delta V needed for 3d DSM: " << deltaVVectorMessenger[ 5 ] << std::endl;
    std::cout << " Time of second Venus visit: " << timeVectorMessenger[ 6 ]
              << ". Delta V needed at Venus: " << deltaVVectorMessenger[ 6 ] << std::endl;
    std::cout << " Time of 4th DSM: " << timeVectorMessenger[ 7 ]
              << ". Delta V needed for 4th DSM: " << deltaVVectorMessenger[ 7 ] << std::endl;
    std::cout << " Time of Mercury capture: " << timeVectorMessenger[ 8 ]
              << ". Delta V needed at Mercury: " << deltaVVectorMessenger[ 8 ] << std::endl;

    // Define vectors to calculate intermediate points
    std::vector< Eigen::Vector3d > interPositionVectorMessenger;
    std::vector< double > interTimeVectorMessenger;

    // Calculate intermediate points and write to file
    std::string outputFileTraj = tudat_applications::getOutputPath( ) + "/messengerTrajectory.dat";
    Messenger.intermediatePoints( 1000.0 , interPositionVectorMessenger, interTimeVectorMessenger );
    writeTrajectoryToFile( interPositionVectorMessenger, interTimeVectorMessenger, outputFileTraj );

    // Define vectors to calculate intermediate points
    std::vector< Eigen::Vector3d > manPositionVectorMessenger;
    std::vector< double > manTimeVectorMessenger;
    std::vector< double > manDeltaVVectorMessenger;

    // Calculate maneuvers and write to file
    std::string outputFileMan = tudat_applications::getOutputPath( ) + "/messengerManeuvers.dat";
    Messenger.maneuvers( manPositionVectorMessenger, manTimeVectorMessenger, manDeltaVVectorMessenger );
    writeTrajectoryToFile( manPositionVectorMessenger, manTimeVectorMessenger, outputFileMan );

    // Calculate trajectories of the planets and output to file
    std::vector< Eigen::Vector3d > positionVectorEarth;
    std::vector< double > timeVectorEarth;
    std::vector< Eigen::Vector3d > positionVectorVenus;
    std::vector< double > timeVectorVenus;
    std::vector< Eigen::Vector3d > positionVectorMercury;
    std::vector< double > timeVectorMercury;

    // Earth
    returnSingleRevolutionPlanetTrajectory(
                std::make_shared< ephemerides::ApproximatePlanetPositions >(
                    ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::earthMoonBarycenter ),
                sunGravitationalParameter,
                1171.64503236,
                1000.0,
                positionVectorEarth,
                timeVectorEarth );

    // Venus
    returnSingleRevolutionPlanetTrajectory(
                std::make_shared< ephemerides::ApproximatePlanetPositions >(
                    ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::venus ),
                sunGravitationalParameter,
                1171.64503236,
                1000.0,
                positionVectorVenus,
                timeVectorVenus );

    // Mercury
    returnSingleRevolutionPlanetTrajectory(
                std::make_shared< ephemerides::ApproximatePlanetPositions >(
                    ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::mercury ),
                sunGravitationalParameter,
                1171.64503236,
                1000.0,
                positionVectorMercury,
                timeVectorMercury );

    std::string outputFilePlanetE = tudat_applications::getOutputPath(  ) + "earthTrajectory.dat";
    writeTrajectoryToFile( positionVectorEarth, timeVectorEarth, outputFilePlanetE );

    std::string outputFilePlanetV = tudat_applications::getOutputPath(  ) + "venusTrajectory.dat";
    writeTrajectoryToFile( positionVectorVenus, timeVectorVenus, outputFilePlanetV );

    std::string outputFilePlanetM = tudat_applications::getOutputPath(  ) + "mercuryTrajectory.dat";
    writeTrajectoryToFile( positionVectorMercury, timeVectorMercury, outputFilePlanetM );

    // Final statement.
    // The exit code EXIT_SUCCESS indicates that the program was successfully executed.
    return EXIT_SUCCESS;
}


#ifndef STAN_MATH_REV_MAT_FUN_DOT_PRODUCT_HPP
#define STAN_MATH_REV_MAT_FUN_DOT_PRODUCT_HPP

#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/typedefs.hpp>
#include <stan/math/prim/mat/err/check_vector.hpp>
#include <stan/math/prim/arr/err/check_matching_sizes.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/rev/core.hpp>
#include <stan/math/rev/mat/fun/typedefs.hpp>
#include <stan/math/rev/scal/fun/value_of.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits.hpp>
#include <vector>

namespace stan {
namespace math {

namespace {
template <typename T>
struct dot_product_store_type;

template <>
struct dot_product_store_type<var> {
  typedef vari** type;
};

template <>
struct dot_product_store_type<double> {
  typedef double* type;
};

template <typename T1, typename T2>
class dot_product_vari : public vari {
 protected:
  typename dot_product_store_type<T1>::type v1_;
  typename dot_product_store_type<T2>::type v2_;
  size_t length_;

  inline static double var_dot(vari** v1, vari** v2, size_t length) {
    Eigen::VectorXd vd1(length), vd2(length);
    for (size_t i = 0; i < length; i++) {
      vd1[i] = v1[i]->val_;
      vd2[i] = v2[i]->val_;
    }
    return vd1.dot(vd2);
  }

  inline static double var_dot(const T1* v1, const T2* v2, size_t length) {
    Eigen::VectorXd vd1(length), vd2(length);
    for (size_t i = 0; i < length; i++) {
      vd1[i] = value_of(v1[i]);
      vd2[i] = value_of(v2[i]);
    }
    return vd1.dot(vd2);
  }

  template <typename Derived1, typename Derived2>
  inline static double var_dot(const Eigen::DenseBase<Derived1>& v1,
                               const Eigen::DenseBase<Derived2>& v2) {
    Eigen::VectorXd vd1(v1.size()), vd2(v1.size());
    for (int i = 0; i < v1.size(); i++) {
      vd1[i] = value_of(v1[i]);
      vd2[i] = value_of(v2[i]);
    }
    return vd1.dot(vd2);
  }
  inline void chain(vari** v1, vari** v2) {
    for (size_t i = 0; i < length_; i++) {
      v1[i]->adj_ += adj_ * v2_[i]->val_;
      v2[i]->adj_ += adj_ * v1_[i]->val_;
    }
  }
  inline void chain(double* v1, vari** v2) {
    for (size_t i = 0; i < length_; i++) {
      v2[i]->adj_ += adj_ * v1_[i];
    }
  }
  inline void chain(vari** v1, double* v2) {
    for (size_t i = 0; i < length_; i++) {
      v1[i]->adj_ += adj_ * v2_[i];
    }
  }
  inline void initialize(vari**& mem_v, const var* inv, vari** shared = NULL) {
    if (shared == NULL) {
      mem_v = reinterpret_cast<vari**>(
          ChainableStack::memalloc_.alloc(length_ * sizeof(vari*)));
      for (size_t i = 0; i < length_; i++)
        mem_v[i] = inv[i].vi_;
    } else {
      mem_v = shared;
    }
  }
  template <typename Derived>
  inline void initialize(vari**& mem_v, const Eigen::DenseBase<Derived>& inv,
                         vari** shared = NULL) {
    if (shared == NULL) {
      mem_v = reinterpret_cast<vari**>(
          ChainableStack::memalloc_.alloc(length_ * sizeof(vari*)));
      for (size_t i = 0; i < length_; i++)
        mem_v[i] = inv(i).vi_;
    } else {
      mem_v = shared;
    }
  }

  inline void initialize(double*& mem_d, const double* ind,
                         double* shared = NULL) {
    if (shared == NULL) {
      mem_d = reinterpret_cast<double*>(
          ChainableStack::memalloc_.alloc(length_ * sizeof(double)));
      for (size_t i = 0; i < length_; i++)
        mem_d[i] = ind[i];
    } else {
      mem_d = shared;
    }
  }
  template <typename Derived>
  inline void initialize(double*& mem_d, const Eigen::DenseBase<Derived>& ind,
                         double* shared = NULL) {
    if (shared == NULL) {
      mem_d = reinterpret_cast<double*>(
          ChainableStack::memalloc_.alloc(length_ * sizeof(double)));
      for (size_t i = 0; i < length_; i++)
        mem_d[i] = ind(i);
    } else {
      mem_d = shared;
    }
  }

 public:
  dot_product_vari(typename dot_product_store_type<T1>::type v1,
                   typename dot_product_store_type<T2>::type v2, size_t length)
      : vari(var_dot(v1, v2, length)), v1_(v1), v2_(v2), length_(length) {}

  dot_product_vari(const T1* v1, const T2* v2, size_t length,
                   dot_product_vari<T1, T2>* shared_v1 = NULL,
                   dot_product_vari<T1, T2>* shared_v2 = NULL)
      : vari(var_dot(v1, v2, length)), length_(length) {
    if (shared_v1 == NULL) {
      initialize(v1_, v1);
    } else {
      initialize(v1_, v1, shared_v1->v1_);
    }
    if (shared_v2 == NULL) {
      initialize(v2_, v2);
    } else {
      initialize(v2_, v2, shared_v2->v2_);
    }
  }
  template <typename Derived1, typename Derived2>
  dot_product_vari(const Eigen::DenseBase<Derived1>& v1,
                   const Eigen::DenseBase<Derived2>& v2,
                   dot_product_vari<T1, T2>* shared_v1 = NULL,
                   dot_product_vari<T1, T2>* shared_v2 = NULL)
      : vari(var_dot(v1, v2)), length_(v1.size()) {
    if (shared_v1 == NULL) {
      initialize(v1_, v1);
    } else {
      initialize(v1_, v1, shared_v1->v1_);
    }
    if (shared_v2 == NULL) {
      initialize(v2_, v2);
    } else {
      initialize(v2_, v2, shared_v2->v2_);
    }
  }
  template <int R1, int C1, int R2, int C2>
  dot_product_vari(const Eigen::Matrix<T1, R1, C1>& v1,
                   const Eigen::Matrix<T2, R2, C2>& v2,
                   dot_product_vari<T1, T2>* shared_v1 = NULL,
                   dot_product_vari<T1, T2>* shared_v2 = NULL)
      : vari(var_dot(v1, v2)), length_(v1.size()) {
    if (shared_v1 == NULL) {
      initialize(v1_, v1);
    } else {
      initialize(v1_, v1, shared_v1->v1_);
    }
    if (shared_v2 == NULL) {
      initialize(v2_, v2);
    } else {
      initialize(v2_, v2, shared_v2->v2_);
    }
  }
  virtual void chain() { chain(v1_, v2_); }
};
}  // namespace

/**
 * Returns the dot product.
 *
 * @param[in] v1 First column vector.
 * @param[in] v2 Second column vector.
 * @return Dot product of the vectors.
 * @throw std::domain_error if length of v1 is not equal to length of v2.
 */
template <typename T1, int R1, int C1, typename T2, int R2, int C2>
inline typename boost::enable_if_c<
    boost::is_same<T1, var>::value || boost::is_same<T2, var>::value, var>::type
dot_product(const Eigen::Matrix<T1, R1, C1>& v1,
            const Eigen::Matrix<T2, R2, C2>& v2) {
  check_vector("dot_product", "v1", v1);
  check_vector("dot_product", "v2", v2);
  check_matching_sizes("dot_product", "v1", v1, "v2", v2);
  return var(new dot_product_vari<T1, T2>(v1, v2));
}
/**
 * Returns the dot product.
 *
 * @param[in] v1 First array.
 * @param[in] v2 Second array.
 * @param[in] length Length of both arrays.
 * @return Dot product of the arrays.
 */
template <typename T1, typename T2>
inline typename boost::enable_if_c<
    boost::is_same<T1, var>::value || boost::is_same<T2, var>::value, var>::type
dot_product(const T1* v1, const T2* v2, size_t length) {
  return var(new dot_product_vari<T1, T2>(v1, v2, length));
}

/**
 * Returns the dot product.
 *
 * @param[in] v1 First vector.
 * @param[in] v2 Second vector.
 * @return Dot product of the vectors.
 * @throw std::domain_error if sizes of v1 and v2 do not match.
 */
template <typename T1, typename T2>
inline typename boost::enable_if_c<
    boost::is_same<T1, var>::value || boost::is_same<T2, var>::value, var>::type
dot_product(const std::vector<T1>& v1, const std::vector<T2>& v2) {
  check_matching_sizes("dot_product", "v1", v1, "v2", v2);
  return var(new dot_product_vari<T1, T2>(&v1[0], &v2[0], v1.size()));
}

}  // namespace math
}  // namespace stan
#endif


/*******************************************************************************
 *
 * Resolution of a system of linear constraints over the domain of intervals
 * is based on W. Harvey & P. J. Stuckey's paper: Improving linear constraint
 * propagation by changing constraint representation, in Constraints,
 * 8(2):173–207, 2003.
 *
 * Author: Arnaud J. Venet (arnaud.j.venet@nasa.gov)
 *
 * Contributors: Alexandre C. D. Wimmers (alexandre.c.wimmers@nasa.gov)
 *               Jorge Navas (jorge.navas@sri.com)
 * Notices:
 *
 * Copyright (c) 2011 United States Government as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All Rights Reserved.
 *
 * Disclaimers:
 *
 * No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF
 * ANY KIND, EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED
 * TO, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS,
 * ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE,
 * OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL BE
 * ERROR FREE, OR ANY WARRANTY THAT DOCUMENTATION, IF PROVIDED, WILL CONFORM TO
 * THE SUBJECT SOFTWARE. THIS AGREEMENT DOES NOT, IN ANY MANNER, CONSTITUTE AN
 * ENDORSEMENT BY GOVERNMENT AGENCY OR ANY PRIOR RECIPIENT OF ANY RESULTS,
 * RESULTING DESIGNS, HARDWARE, SOFTWARE PRODUCTS OR ANY OTHER APPLICATIONS
 * RESULTING FROM USE OF THE SUBJECT SOFTWARE.  FURTHER, GOVERNMENT AGENCY
 * DISCLAIMS ALL WARRANTIES AND LIABILITIES REGARDING THIRD-PARTY SOFTWARE,
 * IF PRESENT IN THE ORIGINAL SOFTWARE, AND DISTRIBUTES IT "AS IS."
 *
 * Waiver and Indemnity:  RECIPIENT AGREES TO WAIVE ANY AND ALL CLAIMS AGAINST
 * THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS, AS WELL
 * AS ANY PRIOR RECIPIENT.  IF RECIPIENT'S USE OF THE SUBJECT SOFTWARE RESULTS
 * IN ANY LIABILITIES, DEMANDS, DAMAGES, EXPENSES OR LOSSES ARISING FROM SUCH
 * USE, INCLUDING ANY DAMAGES FROM PRODUCTS BASED ON, OR RESULTING FROM,
 * RECIPIENT'S USE OF THE SUBJECT SOFTWARE, RECIPIENT SHALL INDEMNIFY AND HOLD
 * HARMLESS THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS,
 * AS WELL AS ANY PRIOR RECIPIENT, TO THE EXTENT PERMITTED BY LAW.
 * RECIPIENT'S SOLE REMEDY FOR ANY SUCH MATTER SHALL BE THE IMMEDIATE,
 * UNILATERAL TERMINATION OF THIS AGREEMENT.
 *
 ******************************************************************************/

#pragma once

#include <vector>
#include <set>
#include <map>
#include <boost/optional.hpp>
#include <crab/common/types.hpp>
#include <crab/common/debug.hpp>
#include <crab/common/stats.hpp>
#include <crab/common/bignums.hpp>
#include <crab/common/wrapint.hpp>
#include <crab/domains/linear_constraints.hpp>

namespace ikos {

  // Interval traits:
  // The solver is parametric on the kind of Interval
  namespace linear_interval_solver_impl {
    // gamma(i) \ gamma(j)
    template<typename Interval>
    inline Interval trim_interval(Interval i, Interval j);

    template<typename Interval, typename Number>
    inline Interval mk_interval(Number n, typename crab::wrapint::bitwidth_t bitwidth) {
      // default implementation ignores bitwidth
      return Interval(n);
    }

    template<typename Interval>
    Interval lower_half_line(Interval i, bool is_signed);

    template<typename Interval>
    Interval upper_half_line(Interval i, bool is_signed);
  }

  template< typename Number, typename VariableName, typename IntervalCollection >
  class linear_interval_solver {

  public:
    typedef typename IntervalCollection::value_type Interval;
    typedef variable< Number, VariableName > variable_t;
    typedef linear_expression< Number, VariableName > linear_expression_t;
    typedef linear_constraint< Number, VariableName > linear_constraint_t;
    typedef linear_constraint_system< Number, VariableName > linear_constraint_system_t;
    typedef typename variable_t::bitwidth_t bitwidth_t;

  private:
    typedef std::vector< linear_constraint_t > cst_table_t;
    typedef std::set< unsigned int > uint_set_t;
    typedef std::map< variable_t, uint_set_t > trigger_table_t;
    typedef typename linear_constraint_t::variable_set_t variable_set_t;

  private:
    class bottom_found { };

  private:
    std::size_t _max_cycles;
    std::size_t _max_op;
    bool _is_contradiction;
    bool _is_large_system;
    cst_table_t _cst_table;
    trigger_table_t _trigger_table;
    variable_set_t _refined_variables;
    std::size_t _op_count;

  private:
    static const std::size_t _large_system_cst_threshold = 3;
    // cost of one propagation cycle for a dense 3x3 system of constraints
    static const std::size_t _large_system_op_threshold = 27;

  private:
    void refine(variable_t v, Interval i, IntervalCollection& env) {
      crab::ScopedCrabStats __st__("Linear Interval Solver.Solving refinement");
      CRAB_LOG("integer-solver",
	       crab::outs() << "\tRefine " << v << " with " << i << "\n";);
      Interval old_i = env[v];
      Interval new_i = old_i & i;
      CRAB_LOG("integer-solver",
	       crab::outs() << "\tOld=" << old_i << " New=" << new_i << "\n";);
      if (new_i.is_bottom()) {
	throw bottom_found();
      }
      if (!(old_i == new_i)) {
	env.set(v, new_i);
	this->_refined_variables += v;
	++(this->_op_count);
      }
    }

    Interval compute_residual(const linear_constraint_t &cst, variable_t pivot,
                              IntervalCollection& env) {
      crab::ScopedCrabStats __st__("Linear Interval Solver.Solving computing residual");
      namespace interval_traits = linear_interval_solver_impl;
      bitwidth_t w = pivot.get_bitwidth();
      Interval residual= interval_traits::mk_interval<Interval>(cst.constant(), w);
      for (typename linear_constraint_t::iterator it = cst.begin(); it != cst.end(); ++it) {
	variable_t v = it->second;
	if (!(v == pivot)) {
	  residual = residual - (interval_traits::mk_interval<Interval>(it->first, w) * env[v]);
	  ++(this->_op_count);
	  if (residual.is_top()) break;
	}
      }
      return residual;
    }

    void propagate(const linear_constraint_t &cst, IntervalCollection& env) {
      crab::ScopedCrabStats __st__("Linear Interval Solver.Solving propagation");
      namespace interval_traits = linear_interval_solver_impl;

      CRAB_LOG("integer-solver",
	       linear_constraint_t tmp(cst);
	       crab::outs() << "Integer solver processing " << tmp << "\n";);

      for (typename linear_constraint_t::iterator it = cst.begin(), et = cst.end();
	   it != et; ++it) {
	Number c = it->first;
	variable_t pivot = it->second;
	Interval res = compute_residual(cst, pivot, env);
	Interval rhs = Interval::top();
	if (!res.is_top()) {
	  Interval ic = interval_traits::mk_interval<Interval>(c, pivot.get_bitwidth());
	  rhs = res / ic;
	}

	if (cst.is_equality()) {
	  refine(pivot, rhs, env);
	} else if (cst.is_inequality()) {
	  if (c > 0) {
	    refine(pivot, interval_traits::lower_half_line(rhs, cst.is_signed()), env);
	  } else {
	    refine(pivot, interval_traits::upper_half_line(rhs, cst.is_signed()), env);
	  }
	} else if (cst.is_strict_inequality()) {
	  // do nothing
	} else {
	  // cst is a disequation
	  Interval old_i = env[pivot];
	  Interval new_i = interval_traits::trim_interval(old_i, rhs);
	  if (new_i.is_bottom()) {
	    throw bottom_found();
	  }
	  if (!(old_i == new_i)) {
	    env.set(pivot, new_i);
	    this->_refined_variables += pivot;
	  }
	  ++(this->_op_count);
	}
      }
    }

    void solve_large_system(IntervalCollection& env) {
      this->_op_count = 0;
      this->_refined_variables.clear();
      for (typename cst_table_t::iterator it = this->_cst_table.begin();
           it != this->_cst_table.end(); ++it) {
	this->propagate(*it, env);
      }
      do {
	variable_set_t vars_to_process(this->_refined_variables);
	this->_refined_variables.clear();
	for (typename variable_set_t::iterator it = vars_to_process.begin();
               it != vars_to_process.end(); ++it) {
	  uint_set_t& csts = this->_trigger_table[*it];
	  for (typename uint_set_t::iterator cst_it = csts.begin();
                 cst_it != csts.end(); ++cst_it) {
	    this->propagate(this->_cst_table.at(*cst_it), env);
	  }
	}
      }
      while (!this->_refined_variables.empty() &&
             this->_op_count <= this->_max_op);
    }

    void solve_small_system(IntervalCollection& env) {
      std::size_t cycle = 0;
      do {
	++cycle;
	this->_refined_variables.clear();
	for (typename cst_table_t::iterator it = this->_cst_table.begin();
               it != this->_cst_table.end(); ++it) {
	  this->propagate(*it, env);
	}
      }
      while (!this->_refined_variables.empty() &&  cycle <= this->_max_cycles);
    }

  public:

    linear_interval_solver(const linear_constraint_system_t &csts, std::size_t max_cycles)
      : _max_cycles(max_cycles),
        _is_contradiction(false),
        _is_large_system(false),
        _op_count(0) {

      crab::ScopedCrabStats __st_a__("Linear Interval Solver");
      crab::ScopedCrabStats __st_b__("Linear Interval Solver.Preprocessing");
      std::size_t op_per_cycle = 0;
      for (typename linear_constraint_system_t::iterator it = csts.begin();
           it != csts.end(); ++it) {
	const linear_constraint_t &cst = *it;
	if (cst.is_contradiction()) {
	  this->_is_contradiction = true;
	  return;
	} else if (cst.is_tautology()) {
	  continue;
	} else {
	  std::size_t cst_size = cst.size();
	  if (cst.is_strict_inequality()) {
	    // convert e < c into {e <= c, e != c}
	    linear_constraint_t c1(cst.expression(), linear_constraint_t::kind_t::INEQUALITY);
	    linear_constraint_t c2(cst.expression(), linear_constraint_t::kind_t::DISEQUATION);
	    this->_cst_table.push_back(c1);
	    this->_cst_table.push_back(c2);
	    cst_size = c1.size() + c2.size();
	  } else {
	    this->_cst_table.push_back(cst);
	  }
	  // cost of one reduction step on the constraint in terms
	  // of accesses to the interval collection
	  op_per_cycle += cst_size * cst_size;
	}
      }

      this->_is_large_system = (this->_cst_table.size() >
                                _large_system_cst_threshold) ||
          (op_per_cycle > _large_system_op_threshold);

      if (!this->_is_contradiction && this->_is_large_system) {
	this->_max_op = op_per_cycle * max_cycles;
	for (unsigned int i = 0; i < this->_cst_table.size(); ++i) {
	  const linear_constraint_t& cst = this->_cst_table.at(i);
	  variable_set_t vars = cst.variables();
	  for (typename variable_set_t::iterator it = vars.begin(); it != vars.end(); ++it) {
	    this->_trigger_table[*it].insert(i);
	  }
	}
      }
    }

    void run(IntervalCollection& env) {
      crab::ScopedCrabStats __st_a__("Linear Interval Solver");
      crab::ScopedCrabStats __st_b__("Linear Interval Solver.Solving");
      if (this->_is_contradiction) {
        env.set_to_bottom();
      } else {
        try {
          if (this->_is_large_system) {
	    this->solve_large_system(env);
          } else {
            this->solve_small_system(env);
          }
        }
        catch (bottom_found& e) {
          env.set_to_bottom();
        }
      }
    }

  }; // class linear_interval_solver

} // namespace ikos


#include "Point3f.h"
#include <cmath>
#include <sstream>
#include <ostream>
#include <cmath>
#include "Constants.h"
#include <boost/lexical_cast.hpp>

namespace ccmc
{
	/**
	 * @param out
	 * @param point
	 * @return
	 */
	std::ostream& operator<<(std::ostream& out, const Point3f& point)
	{
		out << point.toString();
		return out;
	}

	/**
	 * TODO: finish documentation
	 * @param component1
	 * @param component2
	 * @param component3
	 */
	Point3f::Point3f(const float& component1, const float& component2, const float& component3)
	{
		this->component1 = component1;
		this->component2 = component2;
		this->component3 = component3;
		coordinates = Point3f::CARTESIAN;
	}

	/**
	 * TODO: finish documentation
	 * @param component1
	 * @param component2
	 * @param component3
	 */
	Point3f::Point3f(const float& component1, const float& component2, const float& component3, Coordinates c)
	{
		this->component1 = component1;
		this->component2 = component2;
		this->component3 = component3;
		coordinates = c;
	}

	Point3f::Point3f(const Point3f& p)
	{
		this->component1 = p.component1;
		this->component2 = p.component2;
		this->component3 = p.component3;
		coordinates = p.coordinates;
	}

	/**
	 * TODO: finish documentation
	 */
	Point3f::Point3f()
	{
		this->component1 = 0.0;
		this->component2 = 0.0;
		this->component3 = 0.0;
	}

	/**
	 * TODO: finish documentation
	 */
	Point3f::Point3f(Coordinates c)
	{
		this->component1 = 0.0;
		this->component2 = 0.0;
		this->component3 = 0.0;
		coordinates = c;
	}

	/**
	 * TODO: finish documentation
	 */
	Point3f::~Point3f()
	{

	}

	/**
	 * TODO: finish documentation
	 */
	Point3f Point3f::operator+(const Point3f& p) const
	{
		Point3f point;
		point.component1 = component1 + p.component1;
		point.component2 = component2 + p.component2;
		point.component3 = component3 + p.component3;
		point.coordinates = p.coordinates;

		return point;
	}
	/**
	 * Minus operator where sender is on the right
	 * TODO: test this
	 */
	Point3f Point3f::operator-(const Point3f& p) const
	{
		Point3f point;
		point.component1 = component1 - p.component1;
		point.component2 = component2 - p.component2;
		point.component3 = component3 - p.component3;
		point.coordinates = p.coordinates;

		return point;
	}

	/**
	 * TODO: finish documentation
	 */
	Point3f Point3f::operator*(float value) const
	{
		Point3f point;
		point.component1 = component1 * value;
		point.component2 = component2 * value;
		point.component3 = component3 * value;
		point.coordinates = coordinates;
		return point;
	}

	/**
	 * TODO: finish documentation
	 */
	Point3f Point3f::operator*(double value) const
	{
		Point3f point;
		point.component1 = component1 * value;
		point.component2 = component2 * value;
		point.component3 = component3 * value;
		point.coordinates = coordinates;
		return point;
	}

	/** compute the distance between this point, and Point p **/
	/**
	 * TODO: finish documentation
	 */
	float Point3f::distance(const Point3f& p) const
	{
		float dist;

		if (coordinates == Point3f::SPHERICAL)
		{
			//r p t
			float sinTheta1 = std::sin(ccmc::constants::DegreesToRadians*(90.f-component2));
			float cartesianComponent1 = component1 * sinTheta1 * std::cos(ccmc::constants::DegreesToRadians * component3);
			float cartesianComponent2 = component1 * sinTheta1 * std::sin(ccmc::constants::DegreesToRadians * component3);
			float cartesianComponent3 = component1 * std::cos(ccmc::constants::DegreesToRadians * (90.f - component2));

			float sinTheta2 = std::sin(ccmc::constants::DegreesToRadians*(90.f-p.component2));
			float pCartesianComponent1 = p.component1 * sinTheta1 * std::cos(ccmc::constants::DegreesToRadians * p.component3);
			float pCartesianComponent2 = p.component1 * sinTheta1 * std::sin(ccmc::constants::DegreesToRadians * p.component3);
			float pCartesianComponent3 = p.component1 * std::cos(ccmc::constants::DegreesToRadians * (90.f - p.component2));

			float diff1 = cartesianComponent1 - pCartesianComponent1;
			float diff2 = cartesianComponent2 - pCartesianComponent2;
			float diff3 = cartesianComponent3 - pCartesianComponent3;

			dist = std::sqrt(diff1 * diff1 + diff2 * diff2 + diff3 * diff3);
		} else
		{
			dist = std::sqrt((component1 - p.component1) * (component1 - p.component1) + (component2 - p.component2)
					* (component2 - p.component2) + (component3 - p.component3) * (component3 - p.component3));
		}
		return dist;
	}

	/**
	 * TODO: finish documentation
	 */
	void Point3f::normalize()
	{

		float mag = magnitude();
		if (std::abs(mag - 0.0) > .0000001)
		{
			this->component1 /= mag;
			this->component2 /= mag;
			this->component3 /= mag;
		}
	}

	/**
	 * Computes the magnitude of the cartesian vector
	 */
	float Point3f::magnitude()
	{
		return sqrt(component1 * component1 + component2 * component2 + component3 * component3);
	}

	/**
	 * TODO: finish documentation
	 */
	std::string Point3f::toString() const
	{
/*		std::string beg = "(";
		std::string mid = ",";
		std::string end = ")";
		std::ostringstream oss1;
		oss1 << component1;
		std::string scomponent1 = oss1.str();
		std::ostringstream oss2;
		oss2 << component2;
		std::string scomponent2 = oss2.str();
		std::ostringstream oss3;
		oss3 << component3;
		std::string scomponent3 = oss3.str();
		std::string str = beg + scomponent1 + mid + scomponent2 + mid + scomponent3 + end;
		return str;
		*/
		std::string temp_string = "";
		temp_string = "(" + boost::lexical_cast<std::string>(component1) + ",";
		temp_string += boost::lexical_cast<std::string>(component2) + ",";
		temp_string += boost::lexical_cast<std::string>(component3) + ")";
		return temp_string;
	}

	/**
	 * TODO: finish documentation
	 */
	void Point3f::setCoordinates(Point3f::Coordinates c)
	{
		coordinates = c;
	}

	/**
	 * TODO: finish documentation
	 */
	Point3f::Coordinates Point3f::getCoordinates()
	{
		return coordinates;
	}

	Point3f Point3f::getCartesian()
	{
		Point3f cartesian;
		float sinTheta1 = std::sin(ccmc::constants::DegreesToRadians*(90.f-component2));
		cartesian.component1 = component1 * sinTheta1 * std::cos(ccmc::constants::DegreesToRadians * component3);
		cartesian.component2 = component1 * sinTheta1 * std::sin(ccmc::constants::DegreesToRadians * component3);
		cartesian.component3 = component1 * std::cos(ccmc::constants::DegreesToRadians * (90.f - component2));
		return cartesian;

	}

}