/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*!
 Copyright (C) 2005, 2006, 2007, 2009 StatPro Italia srl

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

// the only header you need to use QuantLib
#include <ql/quantlib.hpp>

#ifdef BOOST_MSVC
/* Uncomment the following lines to unmask floating-point
   exceptions. Warning: unpredictable results can arise...

   See http://www.wilmott.com/messageview.cfm?catid=10&threadid=9481
   Is there anyone with a definitive word about this?
*/
// #include <float.h>
// namespace { unsigned int u = _controlfp(_EM_INEXACT, _MCW_EM); }
#endif

#include <boost/timer.hpp>
#include <iostream>
#include <iomanip>

using namespace QuantLib;

#if defined(QL_ENABLE_SESSIONS)
namespace QuantLib {

    Integer sessionId() { return 0; }

}
#endif

int main(int, char* []) {

    try {

        boost::timer timer;
        std::cout << std::endl;

        // set up dates
        Calendar calendar = TARGET();
        Date todaysDate(15, May, 1998);
        Date settlementDate(17, May, 1998);
        Settings::instance().evaluationDate() = todaysDate;

        // our options
        Option::Type type(Option::Put);
        Real underlying = 36;
        Real strike = 40;
        Spread dividendYield = 0.00;
        Rate riskFreeRate = 0.06;
        Volatility volatility = 0.20;
        Date maturity(17, May, 1999);
        DayCounter dayCounter = Actual365Fixed();

        std::cout << "Option type = "  << type << std::endl;
        std::cout << "Maturity = "        << maturity << std::endl;
        std::cout << "Underlying price = "        << underlying << std::endl;
        std::cout << "Strike = "                  << strike << std::endl;
        std::cout << "Risk-free interest rate = " << io::rate(riskFreeRate)
                  << std::endl;
        std::cout << "Dividend yield = " << io::rate(dividendYield)
                  << std::endl;
        std::cout << "Volatility = " << io::volatility(volatility)
                  << std::endl;
        std::cout << std::endl;
        std::string method;
        std::cout << std::endl ;

        // write column headings
        Size widths[] = { 35, 14, 14, 14 };
        std::cout << std::setw(widths[0]) << std::left << "Method"
                  << std::setw(widths[1]) << std::left << "European"
                  << std::setw(widths[2]) << std::left << "Bermudan"
                  << std::setw(widths[3]) << std::left << "American"
                  << std::endl;

        std::vector<Date> exerciseDates;
        for (Integer i=1; i<=4; i++)
            exerciseDates.push_back(settlementDate + 3*i*Months);

        boost::shared_ptr<Exercise> europeanExercise(
                                         new EuropeanExercise(maturity));

        boost::shared_ptr<Exercise> bermudanExercise(
                                         new BermudanExercise(exerciseDates));

        boost::shared_ptr<Exercise> americanExercise(
                                         new AmericanExercise(settlementDate,
                                                              maturity));

        Handle<Quote> underlyingH(
            boost::shared_ptr<Quote>(new SimpleQuote(underlying)));

        // bootstrap the yield/dividend/vol curves
        Handle<YieldTermStructure> flatTermStructure(
            boost::shared_ptr<YieldTermStructure>(
                new FlatForward(settlementDate, riskFreeRate, dayCounter)));
        Handle<YieldTermStructure> flatDividendTS(
            boost::shared_ptr<YieldTermStructure>(
                new FlatForward(settlementDate, dividendYield, dayCounter)));
        Handle<BlackVolTermStructure> flatVolTS(
            boost::shared_ptr<BlackVolTermStructure>(
                new BlackConstantVol(settlementDate, calendar, volatility,
                                     dayCounter)));
        boost::shared_ptr<StrikedTypePayoff> payoff(
                                        new PlainVanillaPayoff(type, strike));
        boost::shared_ptr<BlackScholesMertonProcess> bsmProcess(
                 new BlackScholesMertonProcess(underlyingH, flatDividendTS,
                                               flatTermStructure, flatVolTS));

        // options
        VanillaOption europeanOption(payoff, europeanExercise);
        VanillaOption bermudanOption(payoff, bermudanExercise);
        VanillaOption americanOption(payoff, americanExercise);

        // Analytic formulas:

        // Black-Scholes for European
        method = "Black-Scholes";
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                                     new AnalyticEuropeanEngine(bsmProcess)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << "N/A"
                  << std::setw(widths[3]) << std::left << "N/A"
                  << std::endl;

        // semi-analytic Heston for European
        method = "Heston semi-analytic";
        boost::shared_ptr<HestonProcess> hestonProcess(
            new HestonProcess(flatTermStructure, flatDividendTS,
                              underlyingH, volatility*volatility,
                              1.0, volatility*volatility, 0.001, 0.0));
        boost::shared_ptr<HestonModel> hestonModel(
                                              new HestonModel(hestonProcess));
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                                     new AnalyticHestonEngine(hestonModel)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << "N/A"
                  << std::setw(widths[3]) << std::left << "N/A"
                  << std::endl;

        // semi-analytic Bates for European
        method = "Bates semi-analytic";
        boost::shared_ptr<BatesProcess> batesProcess(
            new BatesProcess(flatTermStructure, flatDividendTS,
                             underlyingH, volatility*volatility,
                             1.0, volatility*volatility, 0.001, 0.0,
                             1e-14, 1e-14, 1e-14));
        boost::shared_ptr<BatesModel> batesModel(new BatesModel(batesProcess));
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                                                new BatesEngine(batesModel)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << "N/A"
                  << std::setw(widths[3]) << std::left << "N/A"
                  << std::endl;

        // Barone-Adesi and Whaley approximation for American
        method = "Barone-Adesi/Whaley";
        americanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                       new BaroneAdesiWhaleyApproximationEngine(bsmProcess)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << "N/A"
                  << std::setw(widths[2]) << std::left << "N/A"
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;

        // Bjerksund and Stensland approximation for American
        method = "Bjerksund/Stensland";
        americanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                      new BjerksundStenslandApproximationEngine(bsmProcess)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << "N/A"
                  << std::setw(widths[2]) << std::left << "N/A"
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;

        // Integral
        method = "Integral";
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                                             new IntegralEngine(bsmProcess)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << "N/A"
                  << std::setw(widths[3]) << std::left << "N/A"
                  << std::endl;

        // Finite differences
        Size timeSteps = 801;
        method = "Finite differences";
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                 new FDEuropeanEngine<CrankNicolson>(bsmProcess,
                                                     timeSteps,timeSteps-1)));
        bermudanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                 new FDBermudanEngine<CrankNicolson>(bsmProcess,
                                                     timeSteps,timeSteps-1)));
        americanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                 new FDAmericanEngine<CrankNicolson>(bsmProcess,
                                                     timeSteps,timeSteps-1)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << bermudanOption.NPV()
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;

        // Binomial method: Jarrow-Rudd
        method = "Binomial Jarrow-Rudd";
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                new BinomialVanillaEngine<JarrowRudd>(bsmProcess,timeSteps)));
        bermudanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                new BinomialVanillaEngine<JarrowRudd>(bsmProcess,timeSteps)));
        americanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                new BinomialVanillaEngine<JarrowRudd>(bsmProcess,timeSteps)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << bermudanOption.NPV()
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;
        method = "Binomial Cox-Ross-Rubinstein";
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                      new BinomialVanillaEngine<CoxRossRubinstein>(bsmProcess,
                                                                   timeSteps)));
        bermudanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                      new BinomialVanillaEngine<CoxRossRubinstein>(bsmProcess,
                                                                   timeSteps)));
        americanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                      new BinomialVanillaEngine<CoxRossRubinstein>(bsmProcess,
                                                                   timeSteps)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << bermudanOption.NPV()
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;

        // Binomial method: Additive equiprobabilities
        method = "Additive equiprobabilities";
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                new BinomialVanillaEngine<AdditiveEQPBinomialTree>(bsmProcess,
                                                                   timeSteps)));
        bermudanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                new BinomialVanillaEngine<AdditiveEQPBinomialTree>(bsmProcess,
                                                                   timeSteps)));
        americanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                new BinomialVanillaEngine<AdditiveEQPBinomialTree>(bsmProcess,
                                                                   timeSteps)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << bermudanOption.NPV()
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;

        // Binomial method: Binomial Trigeorgis
        method = "Binomial Trigeorgis";
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                new BinomialVanillaEngine<Trigeorgis>(bsmProcess,timeSteps)));
        bermudanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                new BinomialVanillaEngine<Trigeorgis>(bsmProcess,timeSteps)));
        americanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                new BinomialVanillaEngine<Trigeorgis>(bsmProcess,timeSteps)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << bermudanOption.NPV()
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;

        // Binomial method: Binomial Tian
        method = "Binomial Tian";
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                      new BinomialVanillaEngine<Tian>(bsmProcess,timeSteps)));
        bermudanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                      new BinomialVanillaEngine<Tian>(bsmProcess,timeSteps)));
        americanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                      new BinomialVanillaEngine<Tian>(bsmProcess,timeSteps)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << bermudanOption.NPV()
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;

        // Binomial method: Binomial Leisen-Reimer
        method = "Binomial Leisen-Reimer";
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
              new BinomialVanillaEngine<LeisenReimer>(bsmProcess,timeSteps)));
        bermudanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
              new BinomialVanillaEngine<LeisenReimer>(bsmProcess,timeSteps)));
        americanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
              new BinomialVanillaEngine<LeisenReimer>(bsmProcess,timeSteps)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << bermudanOption.NPV()
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;

        // Binomial method: Binomial Joshi
        method = "Binomial Joshi";
        europeanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                    new BinomialVanillaEngine<Joshi4>(bsmProcess,timeSteps)));
        bermudanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                    new BinomialVanillaEngine<Joshi4>(bsmProcess,timeSteps)));
        americanOption.setPricingEngine(boost::shared_ptr<PricingEngine>(
                    new BinomialVanillaEngine<Joshi4>(bsmProcess,timeSteps)));
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << bermudanOption.NPV()
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;

        // Monte Carlo Method: MC (crude)
        timeSteps = 1;
        method = "MC (crude)";
        Size mcSeed = 42;
        boost::shared_ptr<PricingEngine> mcengine1;
        mcengine1 = MakeMCEuropeanEngine<PseudoRandom>(bsmProcess)
            .withSteps(timeSteps)
            .withAbsoluteTolerance(0.02)
            .withSeed(mcSeed);
        europeanOption.setPricingEngine(mcengine1);
        // Real errorEstimate = europeanOption.errorEstimate();
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << "N/A"
                  << std::setw(widths[3]) << std::left << "N/A"
                  << std::endl;

        // Monte Carlo Method: QMC (Sobol)
        method = "QMC (Sobol)";
        Size nSamples = 32768;  // 2^15

        boost::shared_ptr<PricingEngine> mcengine2;
        mcengine2 = MakeMCEuropeanEngine<LowDiscrepancy>(bsmProcess)
            .withSteps(timeSteps)
            .withSamples(nSamples);
        europeanOption.setPricingEngine(mcengine2);
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << europeanOption.NPV()
                  << std::setw(widths[2]) << std::left << "N/A"
                  << std::setw(widths[3]) << std::left << "N/A"
                  << std::endl;

        // Monte Carlo Method: MC (Longstaff Schwartz)
        method = "MC (Longstaff Schwartz)";
        boost::shared_ptr<PricingEngine> mcengine3;
        mcengine3 = MakeMCAmericanEngine<PseudoRandom>(bsmProcess)
            .withSteps(100)
            .withAntitheticVariate()
            .withCalibrationSamples(4096)
            .withAbsoluteTolerance(0.02)
            .withSeed(mcSeed);
        americanOption.setPricingEngine(mcengine3);
        std::cout << std::setw(widths[0]) << std::left << method
                  << std::fixed
                  << std::setw(widths[1]) << std::left << "N/A"
                  << std::setw(widths[2]) << std::left << "N/A"
                  << std::setw(widths[3]) << std::left << americanOption.NPV()
                  << std::endl;

        // End test
        double seconds = timer.elapsed();
        Integer hours = int(seconds/3600);
        seconds -= hours * 3600;
        Integer minutes = int(seconds/60);
        seconds -= minutes * 60;
        std::cout << " \nRun completed in ";
        if (hours > 0)
            std::cout << hours << " h ";
        if (hours > 0 || minutes > 0)
            std::cout << minutes << " m ";
        std::cout << std::fixed << std::setprecision(0)
                  << seconds << " s\n" << std::endl;
        return 0;

    } catch (std::exception& e) {
        std::cerr << e.what() << std::endl;
        return 1;
    } catch (...) {
        std::cerr << "unknown error" << std::endl;
        return 1;
    }
}


/** MIT License

Copyright (c) 2018 Benjamin Bercovici and Jay McMahon

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

/**
 * @file   RigidBodyKinematics.hpp
 * @Author Benjamin Bercovici (bebe0705@colorado.edu)
 * @date   July, 2017
 * @brief  Header of the RigidBodyKinematics libary
 *
 * Rigid Body Kinematics library implementating a handful of useful rigid body routines
 */

#ifndef RIGIDBODYKINEMATICS_HPP
#define RIGIDBODYKINEMATICS_HPP

#include <armadillo>

namespace RBK {

/**
Converts MRP to DCM
@param sigma MRP vector
@return dcm DCM matrix
*/
	arma::mat::fixed<3,3> mrp_to_dcm(const arma::vec::fixed<3> & mrp);

/**
Converts DCM to MRP.
@param dcm DCM
@param short_rot True if short rotation is desired (default), false otherwise
@return mrp MRP set
*/
	arma::vec::fixed<3> dcm_to_mrp(const arma::mat::fixed<3,3> & dcm, const bool short_rot = true);

/**
Converts Quaternions to MRP.
@param Q Unit Quaternion
@param short_rot True if short rotation is desired (default), false otherwise
@return mrp MRP set
*/
	arma::vec::fixed<3> quat_to_mrp(const arma::vec::fixed<4> & Q , const bool short_rot = true);

/**
Converts a set of 321 Euler angles to DCM
@param euler_angles 321 sequence of Euler angles (rad)
@return dcm DCM
*/
	arma::mat::fixed<3,3> euler321_to_dcm(const arma::vec::fixed<3> & euler_angles);

/**
Converts a set of (longitude,latitude) angles to DCM
@param (longitude,latitude) angles (rad)
@return dcm DCM
*/
	arma::mat::fixed<3,3> longitude_latitude_to_dcm(const arma::vec::fixed<3> & euler_angles);

/**
Converts a set of 321 Euler angles to mrp
@param euler_angles 321 sequence of Euler angles (rad)
@return mrp MRP set
*/
	arma::vec::fixed<3> euler321_to_mrp(const arma::vec::fixed<3> & euler_angles);

/**
Computes the time derivative of the attitude set
assuming torque free rotational dynamics and MRP as attitude coordinate
@param[in] t Current time
@param[in] attitude_set mrp + angular velocities
@param[in] inertia inertia matrix
@param[in] L external torque (defaults to (0,0,0))
@return time derivative of the input attitude set
*/
	arma::vec::fixed<6> dXattitudedt(double t, const arma::vec::fixed<6> & attitude_set, const arma::mat::fixed<3,3> & inertia,
		const arma::vec::fixed<3> & L = arma::zeros<arma::vec>(3)) ;

/**
Computes the time derivative of the angular velocity set
@param[in] t Current time
@param[in] attitude_set mrp + angular velocities
@param[in] inertia inertia matrix
@param[in] L external torque (defaults to (0,0,0))
@return time derivative of the input angular velocity
*/
	arma::vec::fixed<3> domegadt(double t,
		const arma::vec::fixed<6> & attitude_set,
		const arma::mat::fixed<3,3> & inertia,
		const arma::vec::fixed<3> & L = arma::zeros<arma::vec>(3)) ;

/**
Computes the time derivative of a mrp set given
a corresponding angular velocity
@param t Current time
@param attitude_set mrp + angular velocities
@return time derivative of the input mrp
*/
	arma::vec::fixed<3> dmrpdt(double t, const arma::vec::fixed<6> & attitude_set );

/**
Returns the shadow set of the input mrp if crossing
surface is reached
@param mrp MRP set
@param force_switch if true, will force the switching of the MRP to its shadow without checking its norm
@return mrp or its shadow set

*/
	arma::vec::fixed<3> shadow_mrp(const arma::vec::fixed<3> & mrp, bool force_switch = false) ;

/**
Converts MRP to quaternions
@param sigma MRP vector
@return quat Unit quaternion
*/
	arma::vec::fixed<4> mrp_to_quat(const arma::vec::fixed<3> & mrp);

/**
Converts a set of 321 Euler angles to DCM
@param euler_angles 321 sequence of Euler angles (deg)
@return dcm DCM
*/
	arma::mat::fixed<3,3> euler321d_to_dcm(const arma::vec::fixed<3> & euler_angles);

/**
Converts a set of 321 Euler angles to mrp
@param euler_angles 321 sequence of Euler angles (deg)
@return mrp MRP set
*/
	arma::vec::fixed<3> euler321d_to_mrp(const arma::vec::fixed<3> & euler_angles);

/**
Converts a set of 313 Euler angles to DCM
@param euler_angles 313 sequence of Euler angles (deg)
@return dcm DCM
*/
	arma::mat::fixed<3,3> euler313d_to_dcm(const arma::vec::fixed<3> & euler_angles);

/**
Converts a set of 313 Euler angles to mrp
@param euler_angles 313 sequence of Euler angles (deg)
@return mrp MRP set
*/
	arma::vec::fixed<3> euler313d_to_mrp(const arma::vec::fixed<3> & euler_angles);

/**
Converts a set of 321 Euler angles expressed in degrees to DCM
@param euler_angles 321 sequence of Euler angles (deg)
@return dcm DCM
*/
	arma::mat::fixed<3,3> euler321d_to_dcm(const arma::vec::fixed<3> & euler_angles);

/**
Converts a set of 321 Euler angles expressed in degrees to mrp
@param euler_angles 321 sequence of Euler angles (deg)
@return mrp MRP set
*/
	arma::vec::fixed<3> euler321d_to_mrp(const arma::vec::fixed<3> & euler_angles);

/**
Converts a set of 313 Euler angles to mrp
@param euler_angles 313 sequence of Euler angles (rad)
@return mrp MRP set
*/
	arma::vec::fixed<3> euler313_to_mrp(const arma::vec::fixed<3> & euler_angles);

/**
Converts a set of 313 Euler angles to DCM
@param euler_angles 313 sequence of Euler angles (rad)
@return dcm DCM
*/
	arma::mat::fixed<3,3> euler313_to_dcm(const arma::vec::fixed<3> & euler_angles);

/**
Returns the matrix tilde[x] of the linear operator v|---> cross(x,v)
@param vec 3-by-1 vector
@return M Skew-symmetric matrix of the said linear operator
*/
	arma::mat::fixed<3,3> tilde(const arma::vec::fixed<3> & vec);

/**
Matrix of the elemental rotation about the first axis of the current frame
@param angle Rotation angle (rad)
@return M Elemental rotation matrix
*/
	arma::mat::fixed<3,3> M1(const double angle);

/**
Matrix of the elemental rotation about the second axis of the current frame
@param angle Rotation angle (rad)
@return M Elemental rotation matrix
*/
	arma::mat::fixed<3,3> M2(const double angle);

/**
Matrix of the elemental rotation about the third axis of the current frame
@param angle Rotation angle (rad)
@return M Elemental rotation matrix
*/
	arma::mat::fixed<3,3> M3(const double angle);

/**
Converts a DCM to the corresponding set of Euler angles
@param m DCM
@return angles Sequence of 321 Euler angles angles [yaw,pitch,roll]
*/
	arma::vec::fixed<3> dcm_to_euler321(const arma::mat::fixed<3,3> & dcm);

/**
Converts a DCM to the corresponding set of Euler angles
@param m DCM
@return angles Sequence of 313 Euler angles angles [right ascension,inclination,longitude]
*/
	arma::vec::fixed<3> dcm_to_euler313(const arma::mat::fixed<3,3> & dcm);

/**
Converts a MRP to the corresponding set of Euler angles
@param sigma MRP vector
@return angles Sequence of 313 Euler angles angles [right ascension,inclination,longitude]
*/
	arma::vec::fixed<3> mrp_to_euler313(const arma::vec::fixed<3> & mrp);

/**
Converts a MRP to the corresponding set of Euler angles
@param sigma MRP vector
@return angles Sequence of 321 Euler angles angles [right ascension,inclination,longitude]
*/
	arma::vec::fixed<3> mrp_to_euler321(const arma::vec::fixed<3> & mrp);

/**
Converts a DCM to the corresponding set of Euler angles in degrees
@param m DCM
@return angles Sequence of 321 Euler angles angles [yaw,pitch,roll]
*/
	arma::vec::fixed<3> dcm_to_euler321d(const arma::mat::fixed<3,3> & dcm);

/**
Converts a DCM to the corresponding set of Euler angles in degrees
@param m DCM
@return angles Sequence of 313 Euler angles angles [right ascension,inclination,longitude]
*/
	arma::vec::fixed<3> dcm_to_euler313d(const arma::mat::fixed<3,3> & dcm);

/**
Converts a MRP to the corresponding set of Euler angles in degrees
@param sigma MRP vector
@return angles Sequence of 313 Euler angles angles [right ascension,inclination,longitude]
*/
	arma::vec::fixed<3> mrp_to_euler313d(const arma::vec::fixed<3> & mrp);

/**
Converts a MRP to the corresponding set of Euler angles in degrees
@param sigma MRP vector
@return angles Sequence of 321 Euler angles angles [right ascension,inclination,longitude]
*/
	arma::vec::fixed<3> mrp_to_euler321d(const arma::vec::fixed<3> & mrp);

/**
Converts a DCM to a quaternion corresponding to the short-path rotation
@param dcm DCM
@return Q Unit quaternion
*/
	arma::vec::fixed<4> dcm_to_quat(const arma::mat::fixed<3,3> & dcm) ;

/**
Converts a dcm to the principal rotation vector
@param dcm DCM
@return prv Principal rotation vector
*/
	arma::vec::fixed<3> dcm_to_prv(const arma::mat::fixed<3,3> & dcm) ;

/**
Converts a PRV to the corresponding DCM
@param prv Principal rotation vector
@return DCM
*/

	arma::mat::fixed<3,3> prv_to_dcm(const arma::vec::fixed<3> & prv);

/**
Converts a PRV to a well-behaved MRP set
@param prv Principal rotation vector
@return MRP set
*/

	arma::vec::fixed<3> prv_to_mrp(const arma::vec::fixed<3> & prv);

/**
Returns the B matrix in the evaluation of the MRP's time derivative (sigma_dot = 1/4 * Bmat(sigma) * omega)
@param mrp MRP set
@return instantiated B mtrix
*/
	arma::mat::fixed<3,3> Bmat(const arma::vec::fixed<3> & mrp);

/**
Returns the partial derivative of mrp_dot with respect to the mrp
@param attitude_set attitude set comprised of the mrp set and its associated angular velocity
@return partial derivative of mrp_dot with respect to the mrp
*/
	arma::mat::fixed<3,3> partial_mrp_dot_partial_mrp(const arma::vec::fixed<6> & attitude_set);

}

#endif


#ifndef MOCHIMOCHI_AROW_HPP_
#define MOCHIMOCHI_AROW_HPP_

#include <Eigen/Dense>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <fstream>
#include "../../functions/enumerate.hpp"

class AROW {
private :
  const std::size_t kDim;
  const double kR;

private :
  Eigen::VectorXd _covariances;
  Eigen::VectorXd _means;

public :
  AROW(const std::size_t dim, const double r)
    : kDim(dim),
      kR(r),
      _covariances(Eigen::VectorXd::Ones(kDim)),
      _means(Eigen::VectorXd::Zero(kDim)) {

    static_assert(std::numeric_limits<decltype(dim)>::max() > 0, "Dimension Error. (Dimension > 0)");
    static_assert(std::numeric_limits<decltype(r)>::max() > 0, "Hyper Parameter Error. (r > 0)");
    assert(dim > 0);
    assert(r > 0);

  }

  virtual ~AROW() { }

private :

  double suffer_loss(const double margin, const int label) const {
    return margin * label;
  }

  double compute_margin(const Eigen::VectorXd& x) const {
    return _means.dot(x);
  }

  double compute_confidence(const Eigen::VectorXd& feature) const {
    auto confidence = 0.0;
    functions::enumerate(feature.data(), feature.data() + feature.size(), 0,
                         [&](const int index, const double value) {
                           confidence += _covariances[index] * value * value;
                         });
    return confidence;
  }

public :

  bool update(const Eigen::VectorXd& feature, const int label) {
    const auto margin = compute_margin(feature);

    if (suffer_loss(margin, label) >= 1.0) { return false; }

    const auto confidence = compute_confidence(feature);
    const auto beta = 1.0 / (confidence + kR);
    const auto alpha = std::max(0.0, 1.0 - label * margin) * beta;

    functions::enumerate(feature.data(), feature.data() + feature.size(), 0,
                         [&](const int index, const double value) {
                           const auto v = _covariances[index] * value;
                           _means[index] += alpha * label * v;
                           _covariances[index] -= beta * v * v;
                         });
    return true;
  }

  int predict(const Eigen::VectorXd& x) const {
    return compute_margin(x) > 0.0 ? 1 : -1;
  }

  Eigen::VectorXd get_means(void) const {
    return _means;
  }

  void save(const std::string& filename) {
    std::ofstream ofs(filename);
    assert(ofs);
    boost::archive::text_oarchive oa(ofs);
    oa << *this;
    ofs.close();
  }

  void load(const std::string& filename) {
    std::ifstream ifs(filename);
    assert(ifs);
    boost::archive::text_iarchive ia(ifs);
    ia >> *this;
    ifs.close();
  }

private :
  friend class boost::serialization::access;
  BOOST_SERIALIZATION_SPLIT_MEMBER();
  template <class Archive>
  void save(Archive& ar, const unsigned int version) const {
    std::vector<double> covariances_vector(_covariances.data(), _covariances.data() + _covariances.size());
    std::vector<double> means_vector(_means.data(), _means.data() + _means.size());
    ar & boost::serialization::make_nvp("covariances", covariances_vector);
    ar & boost::serialization::make_nvp("means", means_vector);
    ar & boost::serialization::make_nvp("dimension", const_cast<std::size_t&>(kDim));
    ar & boost::serialization::make_nvp("r", const_cast<double&>(kR));
  }

  template <class Archive>
  void load(Archive& ar, const unsigned int version) {
    std::vector<double> covariances_vector;
    std::vector<double> means_vector;
    ar & boost::serialization::make_nvp("covariances", covariances_vector);
    ar & boost::serialization::make_nvp("means", means_vector);
    ar & boost::serialization::make_nvp("dimension", const_cast<std::size_t&>(kDim));
    ar & boost::serialization::make_nvp("r", const_cast<double&>(kR));
    _covariances = Eigen::Map<Eigen::VectorXd>(&covariances_vector[0], covariances_vector.size());
    _means = Eigen::Map<Eigen::VectorXd>(&means_vector[0], means_vector.size());
  }
};

#endif //MOCHIMOCHI_AROW_HPP_


#include <sstream>
#include <fstream>
#include <iostream>
#include <random>
#include <memory>
#include <array>

#include <Eigen/Core>
#include <gflags/gflags.h>
#include <ceres/ceres.h>

template<typename T>
class Ellipse
{
public:
    Ellipse() : Ellipse(0, 0, 1, 1)
    {
    }

    explicit Ellipse(T h, T k, T a, T b)
    {
        m_data[0] = h;
        m_data[1] = k;
        m_data[2] = a;
        m_data[3] = b;
    }

    explicit Ellipse(const T* data)
    {
        std::copy(data, data + 4, m_data.begin());
    }

    explicit Ellipse(const Ellipse& other) : Ellipse(other.params())
    {
    }

    Ellipse& operator= (const Ellipse &other)
    {
        std::copy(other.m_data.begin(), other.m_data.end(), m_data.begin());
        return *this;
    }

    const T* params() const
    {
        return m_data.data();
    }

    T* params()
    {
        return m_data.data();
    }

    T h() const
    {
        return m_data[0];
    }

    T k() const
    {
        return m_data[1];
    }

    T a() const
    {
        return m_data[2];
    }

    T b() const
    {
        return m_data[3];
    }

    void set_bounds(ceres::Problem& problem)
    {
        problem.SetParameterLowerBound(params(), 2, 1e-5);
        problem.SetParameterLowerBound(params(), 3, 1e-5);
    }

    std::string to_string()
    {
        std::stringstream buff;
        buff << "(h=" << h() << ", k=" << k() << ", a=" << a() << ", b=" << b() << ")";
        return buff.str();
    }

private:
    std::array<T, 4> m_data;
};

/** Sample functor which simply computes the residual.
 *  Used for numeric differentiation.
 */
struct NumericEllipseCostFunctor
{
    NumericEllipseCostFunctor(const Eigen::Vector2d &observed_point) : observed_point(observed_point) {}
    bool operator()(const double *const parameters, double *residuals) const
    {
        Ellipse<double> ellipse(parameters);

        // compute the cost
        const double dx = observed_point.x() - ellipse.h();
        const double dy = observed_point.y() - ellipse.k();
        const double a2 = ellipse.a() * ellipse.a();
        const double b2 = ellipse.b() * ellipse.b();
        residuals[0] = (dx * dx) / a2 + (dy * dy) / b2 - 1;
        return true;
    }

    Eigen::Vector2d observed_point;
};

/** Slightly more advanced functor which is templated, allowing
 *  Ceres to automatically compute the Jacobian using
 *  templates.
 */
struct AutoEllipseCostFunctor
{
    AutoEllipseCostFunctor(const Eigen::Vector2d &observed_point) : observed_point(observed_point) {}

    /** Ceres will create a version of this with a special
     *  autodiff type for determining the Jacobian
     *  and another with doubles for residual computation
     */
    template <typename T>
    bool operator()(const T *const parameters, T *residuals) const
    {
        Ellipse<T> ellipse(parameters);

        T dx = T(observed_point.x()) - ellipse.h();
        T dy = T(observed_point.y()) - ellipse.k();
        T a2 = ellipse.a() * ellipse.a();
        T b2 = ellipse.b() * ellipse.b();
        residuals[0] = (dx * dx) / a2 + (dy * dy) / b2 - T(1.0);
        return true;
    }

    Eigen::Vector2d observed_point;
};

/** If the analytic gradient is simple to compute or if perfomance is a concern,
 *  it can be best to compute the Jacobians by hand, as shown here. The template
 *  arguments indicate to Ceres the number of residuals, and the number of
 *  parameters. This can also be determined dynamically using the base
 *  class `CostFunction`.
 */
struct AnalyticEllipseCostFunction : public ceres::SizedCostFunction<1, 4>
{
    AnalyticEllipseCostFunction(const Eigen::Vector2d &observed_point) : observed_point(observed_point) {}
    virtual ~AnalyticEllipseCostFunction() {}

    /** This function performs double duty: it both computes the residuals and,
     *  at other times, will also compute the Jacobians. This is communicated
     *  via potential `nullptr` values in `jacobians`. While somewhat awkward,
     *  this allows for re-use of sub-expressions for increased efficiency.
     *  The sizes of the arrays are indicated via the template argument
     *  above.
     *
     *  \param parameters an array of parameter arrays
     *  \param residuals an array of residuals values
     *  \param jacobians an array of Jacobian matrices. Each matrix is in row-major order.
     *  \return whether the evaluation was successful
     */
    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
    {
        Ellipse<double> ellipse(parameters[0]);

        // We can re-use all of these later
        const double dx = observed_point.x() - ellipse.h();
        const double dy = observed_point.y() - ellipse.k();
        const double dx2 = dx * dx;
        const double dy2 = dy * dy;
        const double a2 = ellipse.a() * ellipse.a();
        const double b2 = ellipse.b() * ellipse.b();
        residuals[0] = dx2 / a2 + dy2 / b2 - 1;

        // will be null if only evaluating residuals
        if (jacobians != nullptr)
        {
            // if some parameters are being held constant,
            // then individual Jacobian matrices will also be null
            // to avoid unneeded computation
            if (jacobians[0] != nullptr)
            {
                using jacobian_t = Eigen::Matrix<double, 1, 4, Eigen::RowMajor>;
                Eigen::Map<jacobian_t> jac(jacobians[0]);
                jac(0, 0) = (-2 * dx) / a2;
                jac(0, 1) = (-2 * dy) / b2;
                jac(0, 2) = (-2 * dx2) / (a2 * ellipse.a());
                jac(0, 3) = (-2 * dy2) / (b2 * ellipse.b());
            }
        }

        return true;
    }

    Eigen::Vector2d observed_point;
};

/** We can inject our own code into the optimization process to do
 *  custom logging and the like. This class writes intermediate values
 *  to a CSV file.
 */
class CSVCallback : public ceres::IterationCallback
{
public:
    explicit CSVCallback(const std::string &path, const double *params, int num_observations)
        : m_params(params), m_num_observations(num_observations), m_output(path)
    {
        m_output << "Cost,h,k,a,b" << std::endl;
    }

    ~CSVCallback() {}

    ceres::CallbackReturnType operator()(const ceres::IterationSummary &summary)
    {
        Ellipse<double> ellipse(m_params);
        m_output << summary.cost / m_num_observations << "," << ellipse.h() << "," << ellipse.k() << "," << ellipse.a() << "," << ellipse.b() << std::endl;
        return ceres::CallbackReturnType::SOLVER_CONTINUE;
    }

private:
    const double *m_params;
    const int m_num_observations;
    std::ofstream m_output;
};

/** Creates a dataset consisting of noisy samples from an arc of an
 *  axis-aligned ellipse.
 *
 *  \param num_observations the number of observations to sample
 *  \param params the ellipse parameters
 *  \param start_angle the starting angle of the arc in radians
 *  \param end_angle the ending angle of the arc in radians
 *  \param noise_sigma the sigma of the Gaussian used for noise
 *  \return a matrix of points
 */
Eigen::Matrix2Xd create_dataset(int num_observations,
                                const Ellipse<double> &ellipse,
                                double start_angle = -0.5,
                                double end_angle = 2.0,
                                double noise_sigma = 0.05)
{
    Eigen::RowVectorXd angles = Eigen::RowVectorXd::LinSpaced(num_observations, start_angle, end_angle);
    Eigen::Matrix2Xd data(2, num_observations);
    data.row(0) = (ellipse.a() * angles.array().cos()) + ellipse.h();
    data.row(1) = (ellipse.b() * angles.array().sin()) + ellipse.k();
    std::random_device rd{};
    std::mt19937 gen{rd()};
    std::normal_distribution<> d{0, noise_sigma};
    for (auto i = 0; i < data.cols(); ++i)
    {
        data(0, i) += d(gen);
        data(1, i) += d(gen);
    }

    return data;
}

// Setup the problem using numeric differentiation.
void setup_numeric(ceres::Problem &problem, const Eigen::Matrix2Xd &dataset, double *params)
{
    std::cout << "Numeric differentiation: " << std::endl;
    using cost_t = ceres::NumericDiffCostFunction<NumericEllipseCostFunctor,
                                                  ceres::CENTRAL, // method to use
                                                  1,              // # residuals
                                                  4>;             // # params
    for (auto i = 0; i < dataset.cols(); ++i)
    {
        ceres::CostFunction *cost_function =
            new cost_t(new NumericEllipseCostFunctor(dataset.col(i)));
        problem.AddResidualBlock(cost_function, nullptr, params);
    }
}

// Setup the problem using automatic differentiation.
void setup_autodiff(ceres::Problem &problem, const Eigen::Matrix2Xd &dataset, double *params)
{
    std::cout << "Automatic differentiation: " << std::endl;
    using cost_t = ceres::AutoDiffCostFunction<AutoEllipseCostFunctor,
                                               1,   // # residuals
                                               4>;  // # params
    for (auto i = 0; i < dataset.cols(); ++i)
    {
        ceres::CostFunction *cost_function =
            new cost_t(new AutoEllipseCostFunctor(dataset.col(i)));
        problem.AddResidualBlock(cost_function, nullptr, params);
    }
}

// Setup the problem using analytic differentiation.
void setup_analytic(ceres::Problem &problem, const Eigen::Matrix2Xd &dataset, double *params)
{
    std::cout << "Analytic differentiation: " << std::endl;
    for (auto i = 0; i < dataset.cols(); ++i)
    {
        ceres::CostFunction *cost_function = new AnalyticEllipseCostFunction(dataset.col(i));
        problem.AddResidualBlock(cost_function, nullptr, params);
    }
}

// Perform a gradient check
int check_gradients(const Eigen::Matrix2Xd &dataset, double *params, double tolerance)
{
    // First we create an instance of the cost function we want to check
    Eigen::Vector2d observed_point = dataset.col(0);
    auto cost_function = std::make_shared<AnalyticEllipseCostFunction>(observed_point);

    const double *parameters[] = {params};

    // We can use this object to customise the checking process
    ceres::NumericDiffOptions numeric_diff_options;
    ceres::GradientChecker gradient_checker(cost_function.get(), nullptr, numeric_diff_options);

    // We perform a probe. If unsuccessful, we can view the erroneous
    // gradients by writing the error log to the console
    ceres::GradientChecker::ProbeResults results;
    if (!gradient_checker.Probe(parameters, tolerance, &results))
    {
        std::cerr << "An error has occurred:\n"
                  << results.error_log;
        return EXIT_FAILURE;
    }

    std::cout << "Gradients correct!" << std::endl;
    return EXIT_SUCCESS;
}

DEFINE_string(mode, "numeric", "Mode for the program (one of 'numeric', 'autodiff', 'analytic', 'check_grad')");
DEFINE_int32(num_observations, 100, "Number of observations");
DEFINE_bool(verbose, false, "Output a verbose summary of the optimization");
DEFINE_bool(dump_data, false, "Whether to dump the data to a csv");

int main(int argc, char **argv)
{
    gflags::SetUsageMessage("Ceres Example");
    gflags::ParseCommandLineFlags(&argc, &argv, true);

    Ellipse<double> initial(0.1, 0.3, 0.9, 1.2);
    Ellipse<double> target(-0.3, 0.5, 4.3, 2.1);
    Ellipse<double> ellipse;
    Eigen::Matrix2Xd dataset = create_dataset(FLAGS_num_observations, target);

    if (FLAGS_dump_data)
    {
        std::ofstream output(FLAGS_mode + "_data.csv");
        output << "x,y" << std::endl;
        for (auto i = 0; i < dataset.cols(); ++i)
        {
            output << dataset(0, i) << "," << dataset(1, i) << std::endl;
        }
    }

    ceres::Problem problem;
    ellipse = initial;

    if ("autodiff" == FLAGS_mode)
    {
        setup_autodiff(problem, dataset, ellipse.params());
    }
    else if ("numeric" == FLAGS_mode)
    {
        setup_numeric(problem, dataset, ellipse.params());
    }
    else if ("analytic" == FLAGS_mode)
    {
        setup_analytic(problem, dataset, ellipse.params());
    }
    else if ("check_grad" == FLAGS_mode)
    {
        return check_gradients(dataset, ellipse.params(), 1e-9);
    }
    else
    {
        std::cout << "Unrecognized mode: " << FLAGS_mode << std::endl;
        return 1;
    }

    // we can set upper and lower bounds for all parameters
    ellipse.set_bounds(problem);

    // The solver has a wide variety customization options
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;
    options.num_threads = 8;

    // Here we add our own custom callback for logging to a file
    options.update_state_every_iteration = true;
    std::shared_ptr<CSVCallback> callback = std::make_shared<CSVCallback>(FLAGS_mode + "_fit.csv", ellipse.params(), FLAGS_num_observations);
    options.callbacks.push_back(callback.get());

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    if (FLAGS_verbose)
    {
        std::cout << summary.FullReport() << std::endl;
    }
    else
    {
        std::cout << summary.BriefReport() << std::endl;
    }

    std::cout << "Initial: " << initial.to_string() << std::endl
              << "Final: " << ellipse.to_string() << std::endl
              << "Target: " << target.to_string() << std::endl;

    return 0;
}


#ifndef GUARD_CIRCUIT_SIMULATOR_HPP
#define GUARD_CIRCUIT_SIMULATOR_HPP

#include <sstream>
#include <iostream>
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>

template <typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
struct Functor
{
	// Information that tells the caller the numeric type (eg. double) and size (input / output dim)
	typedef _Scalar Scalar;
	enum
	{ // Required by numerical differentiation module
		InputsAtCompileTime = NX,
		ValuesAtCompileTime = NY
	};
	// Tell the caller the matrix sizes associated with the input, output, and jacobian
	typedef Eigen::Matrix<Scalar, InputsAtCompileTime, 1> InputType;
	typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, 1> ValueType;
	typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;

	// Local copy of the number of inputs
	int m_inputs, m_values;

	// Two constructors:
	Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
	Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

	// Get methods for users to determine function input and output dimensions
	int inputs() const { return m_inputs; }
	int values() const { return m_values; }
};
struct ConductanceFunc : Functor<double>
{
	// Simple constructor
	double time = 0;
	Circuit::Schematic *schem;
	Circuit::ParamTable *param;
	double timestep;
	int NUM_NODES = 0;
	ConductanceFunc(Circuit::Schematic *schem, Circuit::ParamTable *param, double time, double timestep, int NUM_NODES) : Functor<double>(schem->nonLinearComps.size(), schem->nonLinearComps.size())
	{
		this->schem = schem;
		this->param = param;
		this->timestep = timestep;
		this->time = time;
		this->NUM_NODES = NUM_NODES;
	}

	int operator()(const Eigen::VectorXd &vDiff, Eigen::VectorXd &fvec) const
	{
		Eigen::VectorXd voltage(NUM_NODES);
		Eigen::VectorXd current(NUM_NODES);
		Eigen::MatrixXd conductance(NUM_NODES, NUM_NODES);
		for (int i = 0; i < vDiff.size(); i++)
		{
			schem->nonLinearComps[i]->setConductance(param, timestep, vDiff(i));
		}
		Circuit::Math::getConductanceTRAN(schem, conductance, param, time, timestep);
		Circuit::Math::getCurrentTRAN(schem, current, conductance, param, time, timestep);
		Circuit::Math::solveMatrix(conductance, voltage, current);

		for (int i = 0; i < vDiff.size(); i++)
		{
			double vPos = (schem->nonLinearComps[i]->getPosNode()->getId() != -1) ? voltage(schem->nonLinearComps[i]->getPosNode()->getId()) : 0;
			double vNeg = (schem->nonLinearComps[i]->getNegNode()->getId() != -1) ? voltage(schem->nonLinearComps[i]->getNegNode()->getId()) : 0;
			fvec(i) = vPos - vNeg - vDiff(i);
		}

		return 0;
	}
	int getVdif(const Eigen::VectorXd &vDiff, Eigen::VectorXd &fvec) const
	{

		Eigen::VectorXd voltage(NUM_NODES);
		Eigen::VectorXd current(NUM_NODES);
		Eigen::MatrixXd conductance(NUM_NODES, NUM_NODES);

		for (int i = 0; i < vDiff.size(); i++)
		{
			schem->nonLinearComps[i]->setConductance(param, timestep, vDiff(i));
		}
		Circuit::Math::getConductanceTRAN(schem, conductance, param, time, timestep);
		Circuit::Math::getCurrentTRAN(schem, current, conductance, param, time, timestep);
		Circuit::Math::solveMatrix(conductance, voltage, current);

		for (int i = 0; i < vDiff.size(); i++)
		{
			double vPos = (schem->nonLinearComps[i]->getPosNode()->getId() != -1) ? voltage(schem->nonLinearComps[i]->getPosNode()->getId()) : 0;
			double vNeg = (schem->nonLinearComps[i]->getNegNode()->getId() != -1) ? voltage(schem->nonLinearComps[i]->getNegNode()->getId()) : 0;
			fvec(i) = vPos - vNeg;
		}

		return 0;
	}
	void getVoltageVector(const Eigen::VectorXd &vDiff, Eigen::VectorXd &fvec)
	{
		Eigen::VectorXd current(NUM_NODES);
		Eigen::MatrixXd conductance(NUM_NODES, NUM_NODES);

		for (int i = 0; i < vDiff.size(); i++)
		{
			schem->nonLinearComps[i]->setConductance(param, timestep, vDiff(i));
		}

		Circuit::Math::getConductanceTRAN(schem, conductance, param, time, timestep);
		Circuit::Math::getCurrentTRAN(schem, current, conductance, param, time, timestep);
		Circuit::Math::solveMatrix(conductance, fvec, current);
	}
};

class Circuit::Simulator
{
private:
	Schematic *schem;
	double tranStopTime;
	double tranSaveStart;
	double tranStepTime;
	std::stringstream spiceStream;
	std::stringstream csvStream;

	void spicePrintTitle()
	{
		spiceStream << "Time";
		for (auto node_pair : schem->nodes)
		{
			spiceStream << "\tV(" << node_pair.first << ")";
		}
		for (auto comp_pair : schem->comps)
		{
			spiceStream << "\tI(" << comp_pair.first << ")";
		}
		spiceStream << "\n";
	}
	void csvPrintTitle()
	{
		csvStream << "Time";
		for (auto node_pair : schem->nodes)
		{
			csvStream << ",V(" << node_pair.first << ")";
		}
		for (auto comp_pair : schem->comps)
		{
			csvStream << ",I(" << comp_pair.first << ")";
		}
		csvStream << "\n";
	}

	void printStep(int n)
	{
		ParamTable *param = schem->tables[n];
		if (param->lookup.size() == 0)
		{
			return;
		}
		for (auto x : param->lookup)
		{
			spiceStream << "Step Information:";
			csvStream << "Step Information:";
			for (std::pair<std::string, double> var : param->lookup)
			{
				csvStream << " " << var.first << "=" << var.second;
				spiceStream << " " << var.first << "=" << var.second;
			}
			csvStream << " Run: " << n + 1 << "/" << schem->tables.size() << std::endl;
			spiceStream << " Run: " << n + 1 << "/" << schem->tables.size() << std::endl;
		}
	}
	void spicePrint(ParamTable *param, double time, double timestep)
	{
		spiceStream << time;
		for (auto node_pair : schem->nodes)
		{
			spiceStream << "\t" << node_pair.second->voltage;
		}
		for (auto comp_pair : schem->comps)
		{
			spiceStream << "\t" << comp_pair.second->getCurrent(param, time, timestep);
		}
		spiceStream << "\n";
	}
	void csvPrint(ParamTable *param, double time, double timestep)
	{
		csvStream << time;
		for (auto node_pair : schem->nodes)
		{
			csvStream << "," << node_pair.second->voltage;
		}
		for (auto comp_pair : schem->comps)
		{
			csvStream << "," << comp_pair.second->getCurrent(param, time, timestep);
		}
		csvStream << "\n";
	}

public:
	enum SimulationType
	{
		OP,
		TRAN,
		DC,
		SMALL_SIGNAL
	};

	const SimulationType type;

	enum OutputFormat
	{
		CSV,
		SPACE // actually tab separated
	};

	using enumPair = std::pair<SimulationType, std::string>;

	std::map<SimulationType, std::string> simulationTypeMap = {
		enumPair(OP, "OP"),
		enumPair(TRAN, "TRAN"),
		enumPair(DC, "DC"),
		enumPair(SMALL_SIGNAL, "SMALL_SIGNAL"),
	};

	Simulator(Schematic *schem, SimulationType type) : schem(schem), type(type) {}
	Simulator(Schematic *schem, SimulationType type, double tranStopTime, double tranSaveStart = 0, double tranStepTime = 0) : Simulator(schem, type)
	{
		if (tranStepTime == 0)
		{
			tranStepTime = tranStopTime / 1000.0; // default of a thousand cycles
		}
		this->tranStopTime = tranStopTime;
		this->tranSaveStart = tranSaveStart;
		this->tranStepTime = tranStepTime;
	}

	void run(std::ostream &dst, OutputFormat format)
	{
		const unsigned int NUM_NODES = schem->nodes.size() - 1;
		const unsigned int NUM_V_GUESS = schem->nonLinearComps.size();

		Eigen::VectorXd voltage(NUM_NODES);
		Eigen::VectorXd vGuess(NUM_V_GUESS);
		Eigen::VectorXd current(NUM_NODES);
		Eigen::MatrixXd conductance(NUM_NODES, NUM_NODES);

		if (format == SPACE)
		{
			spiceStream.str("");
			spicePrintTitle();
		}
		else if (format == CSV)
		{
			csvStream.str("");
			csvPrintTitle();
		}
		ParamTable *param;
		for (size_t i = 0; i < schem->tables.size(); i++)
		{
			param = schem->tables[i];
			printStep(i);

			for_each(schem->nodes.begin(), schem->nodes.end(), [&](const auto node_pair) {
				if (node_pair.second->getId() != -1)
				{
					node_pair.second->voltage = 0.0;
				}
			});
			if (type == OP)
			{
				Circuit::Math::getConductanceOP(schem, conductance, param);
				Circuit::Math::getCurrentOP(schem, current, conductance, param);
				Circuit::Math::solveMatrix(conductance, voltage, current);

				dst << "\t-----Operating Point-----\t\n";
				if (param->lookup.size() > 0)
				{
					dst << "Step Information: ";
					for (std::pair<std::string, double> var : param->lookup)
					{
						dst << " " << var.first << "=" << var.second;
					}
					dst << " Run: " << i + 1 << "/" << schem->tables.size() << std::endl;
				}
				dst << std::endl;
				for_each(schem->nodes.begin(), schem->nodes.end(), [&](const auto node_pair) {
					if (node_pair.second->getId() != -1)
					{
						node_pair.second->voltage = voltage[node_pair.second->getId()];
						dst << "V(" << node_pair.first << ")\t\t" << node_pair.second->voltage << "\t\tnode_voltage\n";
					}
				});

				for_each(schem->comps.begin(), schem->comps.end(), [&](const auto comp_pair) {
					dst << "I(" << comp_pair.first << ")\t\t" << comp_pair.second->getCurrent(param, 0, -1) << "\t\tdevice_current\n";
				});
			}
			else if (type == TRAN)
			{
				if (!schem->nonLinear)
				{
					Eigen::SparseMatrix<double> sparse;

					for (double t = 0; t <= tranStopTime; t += tranStepTime)
					{
						Math::progressBar(t / tranStopTime, i, schem->tables.size());
						Math::getConductanceTRAN(schem, conductance, param, t, tranStepTime);
						Math::getCurrentTRAN(schem, current, conductance, param, t, tranStepTime);

						try
						{
							Circuit::Math::solveMatrix(conductance, voltage, current);
						}
						catch (const std::exception &e)
						{
							std::cerr << "error solving skipping timestep" << std::endl;
							continue;
						}

						for_each(schem->nodes.begin(), schem->nodes.end(), [&](const auto node_pair) {
							if (node_pair.second->getId() != -1)
							{
								node_pair.second->voltage = voltage[node_pair.second->getId()];
							}
						});
						if (t >= tranSaveStart)
						{
							if (format == SPACE)
							{
								spicePrint(param, t, tranStepTime);
							}
							else if (format == CSV)
							{
								csvPrint(param, t, tranStepTime);
							}
						}
					}
				}
				else
				{
					for (double t = 0; t <= tranStopTime; t += tranStepTime)
					{
						//Math::progressBar(t / tranStopTime, i, schem->tables.size());
						Math::init_vector(vGuess);
						ConductanceFunc functor(schem, param, t, tranStepTime, NUM_NODES);
						Eigen::NumericalDiff<ConductanceFunc> numDiff(functor);

						if (schem->itType == Schematic::IterationType::Levenberg)
						{
							Eigen::LevenbergMarquardt<Eigen::NumericalDiff<ConductanceFunc>, double> lm(numDiff);
							lm.parameters.maxfev = 1000;
							lm.parameters.xtol = 1.0e-10;
							lm.minimize(vGuess);
						}
						else if (schem->itType == Schematic::IterationType::Newton)
						{
							for (size_t i = 0; i < 1000; i++)
							{
								Eigen::MatrixXd jaq(NUM_V_GUESS, NUM_V_GUESS);
								numDiff.df(vGuess, jaq);
								Eigen::VectorXd vErrVec(NUM_V_GUESS);
								functor(vGuess, vErrVec);
								Eigen::MatrixXd inverseJaq = jaq.transpose().inverse();
								for (size_t x = 0; x < NUM_V_GUESS; x++)
								{
									for (size_t y = 0; y < NUM_V_GUESS; y++)
									{
										if (std::isnan(inverseJaq(x, y)))
										{
											inverseJaq(x, y) = 1e-200;
										}
										if (!std::isfinite(inverseJaq(x, y)))
										{
											inverseJaq(x, y) = 1e200;
										}
									}
								}
								std::cerr<<t<<","<<i<<","<<vGuess[0]<<","<<vGuess[1]<<","<<vErrVec.norm()<<std::endl;
								vGuess = vGuess - 0.005 * (inverseJaq * vErrVec);
							}
						}
						else
						{
							std::cerr << "unknown iteration type" << std::endl;
							std::terminate();
						}

						Eigen::VectorXd vErrVec(NUM_V_GUESS);
						functor.getVoltageVector(vGuess, voltage);
						for_each(schem->nodes.begin(), schem->nodes.end(), [&](const auto node_pair) {
							if (node_pair.second->getId() != -1)
							{
								node_pair.second->voltage = voltage[node_pair.second->getId()];
							}
						});
						if (t >= tranSaveStart)
						{
							if (format == SPACE)
							{
								spicePrint(param, t, tranStepTime);
							}
							else if (format == CSV)
							{
								csvPrint(param, t, tranStepTime);
							}
						}
					}
				}
				std::cerr << std::endl;
			}
			if (format == SPACE && type != OP)
			{
				dst << spiceStream.str();
				spiceStream.str("");
			}
			else if (format == CSV && type != OP)
			{
				dst << csvStream.str();
				csvStream.str("");
			}
		}
	}
};

#endif


/*======================================================================
Copyright 2019 Google LLC

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    https://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
======================================================================*/

#include <cmath>

#include <Eigen/Dense>
#include "PCV_Types.h"

#include "traj_circle.h"

using namespace std;
using namespace Eigen;

static double hz_;
static double r_,wr_;
static double a_,wa_;
static double ramp_,t_;

void
init_traj_circle(double r,
                 double wr,
                 double a,
                 double wa,
                 double hz,
                 double ramp)
{
  r_  = r;
  wr_ = wr;
  a_  = a;
  wa_ = wa;
  hz_ = hz;
  ramp_ = ramp;

  t_=0.0;
}

void
traj_circle(Vector3d &x, Vector3d &xd, Vector3d &xdd)
{
  double wr,wa;

   if( t_<ramp_ )
   { wr = t_/ramp_ * wr_;
     wa = t_/ramp_ * wa_;
   }
   else
   { wr = wr_;
     wa = wa_;
   }

    x[0] =  r_*      cos( wr*t_ ) - r_ ;
   xd[0] = -r_*wr*   sin( wr*t_ ) ;
  xdd[0] = -r_*wr*wr*cos( wr*t_ ) ;

    x[1] =  r_*      sin( wr*t_ ) ;
   xd[1] =  r_*wr*   cos( wr*t_ ) ;
  xdd[1] = -r_*wr*wr*sin( wr*t_ ) ;

    x[2] =  a_*   (1-cos( wa*t_ ));
   xd[2] =  a_*wa*   sin( wa*t_ ) ;
  xdd[2] =  a_*wa*wa*cos( wa*t_ ) ;

  t_ += 1.0/hz_;

}


#include <pybind11/eigen.h>
#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "glog/logging.h"

namespace py = pybind11;

std::tuple<Eigen::Vector3d, double> GetRotationVector(Eigen::Ref<Eigen::Vector3d> v1, Eigen::Ref<Eigen::Vector3d> v2) {
    auto rotationVector = Eigen::AngleAxisd(Eigen::Quaterniond::FromTwoVectors(v1, v2));
    return std::make_tuple(rotationVector.axis(), rotationVector.angle());
}

// The input 3D points are stored as columns.
Eigen::Affine3d Find3DAffineTransform(Eigen::Matrix3Xd in, Eigen::Matrix3Xd out) {
    // Default output
    Eigen::Affine3d A;
    A.linear() = Eigen::Matrix3d::Identity(3, 3);
    A.translation() = Eigen::Vector3d::Zero();

    if (in.cols() != out.cols()) throw "Find3DAffineTransform(): input data mis-match";

    // First find the scale, by finding the ratio of sums of some distances,
    // then bring the datasets to the same scale.
    double dist_in = 0, dist_out = 0;
    for (int col = 0; col < in.cols() - 1; col++) {
        dist_in += (in.col(col + 1) - in.col(col)).norm();
        dist_out += (out.col(col + 1) - out.col(col)).norm();
    }
    if (dist_in <= 0 || dist_out <= 0) return A;
    double scale = dist_out / dist_in;
    out /= scale;

    // Find the centroids then shift to the origin
    Eigen::Vector3d in_ctr = Eigen::Vector3d::Zero();
    Eigen::Vector3d out_ctr = Eigen::Vector3d::Zero();
    for (int col = 0; col < in.cols(); col++) {
        in_ctr += in.col(col);
        out_ctr += out.col(col);
    }
    in_ctr /= in.cols();
    out_ctr /= out.cols();
    for (int col = 0; col < in.cols(); col++) {
        in.col(col) -= in_ctr;
        out.col(col) -= out_ctr;
    }

    // SVD
    Eigen::MatrixXd Cov = in * out.transpose();
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(Cov, Eigen::ComputeThinU | Eigen::ComputeThinV);

    // Find the rotation
    double d = (svd.matrixV() * svd.matrixU().transpose()).determinant();
    if (d > 0)
        d = 1.0;
    else
        d = -1.0;
    Eigen::Matrix3d I = Eigen::Matrix3d::Identity(3, 3);
    I(2, 2) = d;
    Eigen::Matrix3d R = svd.matrixV() * I * svd.matrixU().transpose();

    // The final transform
    A.linear() = scale * R;
    A.translation() = scale * (out_ctr - R * in_ctr);

    return A;
}

Eigen::AngleAxisd EstimatePalmAngleFromBase(const std::vector<Eigen::Vector3d>& landmark_list,
                                            const Eigen::MatrixXd& base) {
    // Calculate a maybe stable vectors that penetrating palm perpendicularly.
    Eigen::Vector3d rotationVectorSum;
    auto PALM_PLAIN_INDICES = {5, 9, 13, 17};

    for (size_t i = 0; i < PALM_PLAIN_INDICES.size() - 1; i++) {
        auto a = landmark_list[i] - landmark_list[0];
        auto b = landmark_list[i + 1] - landmark_list[0];
        rotationVectorSum += Eigen::AngleAxisd(Eigen::Quaterniond::FromTwoVectors(a, b)).axis();
    }
    auto meanPalmVector = rotationVectorSum / (PALM_PLAIN_INDICES.size() - 1);

    // Calculate unit direction vectors perpendicular to palm vector.
    Eigen::Vector3d fingerVector = ((landmark_list[9] - landmark_list[0]) + (landmark_list[13] - landmark_list[0]))/2.0;
    fingerVector /= fingerVector.norm();
    Eigen::Vector3d thumbVector = fingerVector.cross(meanPalmVector);
    thumbVector /= thumbVector.norm();

    // Calculate rotation vector that rotate axis.
    Eigen::MatrixXd vectors(3,3);
    vectors.col(0) = thumbVector;
    vectors.col(1) = fingerVector;
    vectors.col(2) = meanPalmVector;

    // std::cout << vectors << std::endl;
    const auto& A = Find3DAffineTransform(base, vectors);
    auto rotation = Eigen::AngleAxisd(A.linear());
    return std::move(rotation);
}

std::tuple<Eigen::Vector3d, double> EstimatePalmRotation(
    const std::vector<Eigen::Vector3d>& landmark_list,
    const std::string& direction) {
    Eigen::Matrix3d baseMatrix;
    // This base is
    // (1) Fingers ar pointing to camera.
    // (2)
    if (direction == "Right") {
        baseMatrix << 1, 0, 0,
                    0, -1, 0,
                    0, 0, -1;
    } else if (direction == "Left") {
        baseMatrix << 1, 0, 0,
                    0, -1, 0,
                    0, 0, 1;
    }
    auto rotation = EstimatePalmAngleFromBase(landmark_list, baseMatrix);
    return std::make_tuple(rotation.axis(), rotation.angle());
}

std::vector<std::tuple<Eigen::Vector3d, double>> GetRelativeAnglesFromXYPlane(
    const std::vector<Eigen::Vector3d>& landmarkList, const std::vector<int>& ids
) {
    std::vector<Eigen::Vector3d> positions;
    for (auto id : ids) {
        positions.push_back(landmarkList[id]);
    }

    // Get the direction of the finger.
    Eigen::Vector3d base = positions.back() - positions.front();
    base[2] = 0;

    std::vector<Eigen::Vector3d> finger_diffs = { base };
    for (size_t i = 0; i < positions.size() - 1; i++)
    {
        finger_diffs.push_back(positions[i + 1] - positions[i]);
    }

    std::vector<std::tuple<Eigen::Vector3d, double>> rotations;
    for (size_t i = 0; i < finger_diffs.size() - 1; i++)
    {
        rotations.push_back(GetRotationVector(finger_diffs[i + 1], finger_diffs[i]));
    }
    return std::move(rotations);
}

std::map<std::string, std::vector<std::tuple<Eigen::Vector3d, double>>> GetFingers(
    const std::vector<Eigen::Vector3d>& landmark_list, const std::map<std::string, std::vector<int>>& fingerIndicesMap) {
    std::map<std::string, std::vector<std::tuple<Eigen::Vector3d, double>>> fingerNameToRotations;

    // Normalize Vector by hand pose.
    Eigen::Matrix3d baseMatrix;
    baseMatrix << 1, 0, 0,
                  0, 1, 0,
                  0, 0, 1;
    auto rotation = EstimatePalmAngleFromBase(landmark_list, baseMatrix);
    std::vector<Eigen::Vector3d> directionNormalizedLandmarks;
    for (const auto& point : landmark_list) {
        directionNormalizedLandmarks.push_back(rotation.inverse() * point);
    }

    // Get the rotations for each fingers.
    for (const auto& tuple : fingerIndicesMap) {
        fingerNameToRotations[std::get<0>(tuple)] = GetRelativeAnglesFromXYPlane(
            directionNormalizedLandmarks, std::get<1>(tuple)
        );
    }

    return fingerNameToRotations;
}

PYBIND11_MODULE(landmark_utils, m) {
    // m.def("get_shortest_rotvec_between_two_vector", &GetRotationVector);
    m.def("get_shortest_rotvec_between_two_vector", &GetRotationVector, py::return_value_policy::move);
    m.def("get_fingers", &GetFingers, py::return_value_policy::move);
    m.def("estimate_palm_rotation", &EstimatePalmRotation, py::return_value_policy::move);
}