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

/*
 Copyright (C) 2008 Roland Lichters

 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.
*/

#include "cdo.hpp"
#include "utilities.hpp"
#include <ql/experimental/credit/cdo.hpp>
#include <ql/experimental/credit/pool.hpp>
#include <ql/experimental/credit/integralcdoengine.hpp>
#include <ql/experimental/credit/midpointcdoengine.hpp>
#include <ql/experimental/credit/randomdefaultlatentmodel.hpp>
#include <ql/experimental/credit/inhomogeneouspooldef.hpp>
#include <ql/experimental/credit/homogeneouspooldef.hpp>
#include <ql/experimental/credit/gaussianlhplossmodel.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/termstructures/credit/flathazardrate.hpp>
#include <ql/time/calendars/target.hpp>
#include <ql/time/daycounters/actual360.hpp>
#include <ql/time/daycounters/actualactual.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/currencies/europe.hpp>
#include <ql/functional.hpp>
#include <boost/preprocessor/iteration/local.hpp>
#include <iomanip>
#include <iostream>

using namespace QuantLib;
using namespace std;
using namespace boost::unit_test_framework;

#ifndef QL_PATCH_SOLARIS

namespace cdo_test {

    Real hwAttachment[] = { 0.00, 0.03, 0.06, 0.10 };
    Real hwDetachment[] = { 0.03, 0.06, 0.10, 1.00 };

    struct hwDatum {
        Real correlation;
        Integer nm;
        Integer nz;
        Real trancheSpread[4];
    };

    // HW Table 7
    // corr, Nm, Nz, 0-3, 3-6, 6-10, 10-100
    hwDatum hwData7[] = {
        { 0.1, -1, -1, { 2279, 450,  89,  1 } },
        { 0.3, -1, -1, { 1487, 472, 203,  7 } },
        // Opening the T, T&G tests too. The convolution is analytical
        //   now so it runs it a time comparable to the gaussian tests and
        //   has enough precission to pass the tests.
        // Below the T models are integrated with a quadrature, even if this
        //   is incorrect the test pass good enough, the quadrature gets to
        //   be worst as the kernel deviates from a normal, this is low
        //   orders of the T; here 5 is enough, 3 would not be.
        { 0.3, -1,  5, { 1766, 420, 161,  6 } },
        { 0.3,  5, -1, { 1444, 408, 171, 10 } },
        { 0.3,  5,  5, { 1713, 359, 136,  9 } }
    };

    void check(int i, int j, const string& desc, Real found, Real expected,
               Real bpTolerance, Real relativeTolerance)
    {
        /* Uncomment to display the full show if your debugging:
        std::cout<< "Case: "<< i << " " << j << " " << found << " :: "
            << expected  <<  " ("<< desc << ") " << std::endl;
        */
        Real absDiff = found - expected;
        Real relDiff = absDiff / expected;
        BOOST_CHECK_MESSAGE (fabs(relDiff) < relativeTolerance ||
                             fabs(absDiff) < bpTolerance,
                             "case " << i << " " << j << " ("<< desc << "): "
                             << found << " vs. " << expected);
    }

}

#endif

void CdoTest::testHW(unsigned dataSet) {
    #ifndef QL_PATCH_SOLARIS

    BOOST_TEST_MESSAGE ("Testing CDO premiums against Hull-White values"
                        " for data set " << dataSet << "...");

    using namespace cdo_test;

    SavedSettings backup;

    Size poolSize = 100;
    Real lambda = 0.01;

    // nBuckets and period determine the computation time
    Size nBuckets = 200;
    // Period period = 1*Months;
    // for MC engines
    Size numSims = 5000;

    Real rate = 0.05;
    DayCounter daycount = Actual360();
    Compounding cmp = Continuous; // Simple;

    Real recovery = 0.4;
    vector<Real> nominals(poolSize, 100.0);
    Real premium = 0.02;
    Period maxTerm (5, Years);
    Schedule schedule = MakeSchedule().from(Date (1, September, 2006))
                                      .to(Date (1, September, 2011))
                                      .withTenor(Period (3, Months))
                                      .withCalendar(TARGET());

    Date asofDate = Date(31, August, 2006);

    Settings::instance().evaluationDate() = asofDate;

    ext::shared_ptr<YieldTermStructure> yieldPtr(
                                              new FlatForward (asofDate, rate,
                                                               daycount, cmp));
    Handle<YieldTermStructure> yieldHandle (yieldPtr);

    Handle<Quote> hazardRate(ext::shared_ptr<Quote>(new SimpleQuote(lambda)));
    vector<Handle<DefaultProbabilityTermStructure> > basket;
    ext::shared_ptr<DefaultProbabilityTermStructure> ptr (
               new FlatHazardRate (asofDate,
                                   hazardRate,
                                   ActualActual()));
    ext::shared_ptr<Pool> pool (new Pool());
    vector<string> names;
    // probability key items
    vector<Issuer> issuers;
    vector<pair<DefaultProbKey,
           Handle<DefaultProbabilityTermStructure> > > probabilities;
    probabilities.push_back(std::make_pair(
        NorthAmericaCorpDefaultKey(EURCurrency(),
                                   SeniorSec,
                                   Period(0,Weeks),
                                   10.),
       Handle<DefaultProbabilityTermStructure>(ptr)));

    for (Size i=0; i<poolSize; ++i) {
        ostringstream o;
        o << "issuer-" << i;
        names.push_back(o.str());
        basket.push_back(Handle<DefaultProbabilityTermStructure>(ptr));
        issuers.push_back(Issuer(probabilities));
        pool->add(names.back(), issuers.back(), NorthAmericaCorpDefaultKey(
                EURCurrency(), QuantLib::SeniorSec, Period(), 1.));
    }

    ext::shared_ptr<SimpleQuote> correlation (new SimpleQuote(0.0));
    Handle<Quote> hCorrelation (correlation);
    QL_REQUIRE (LENGTH(hwAttachment) == LENGTH(hwDetachment),
                "data length does not match");

    ext::shared_ptr<PricingEngine> midPCDOEngine( new MidPointCDOEngine(
        yieldHandle));
    ext::shared_ptr<PricingEngine> integralCDOEngine( new IntegralCDOEngine(
        yieldHandle));

    const Size i = dataSet;
    correlation->setValue (hwData7[i].correlation);
    QL_REQUIRE (LENGTH(hwAttachment) == LENGTH(hwData7[i].trancheSpread),
                "data length does not match");
    std::vector<ext::shared_ptr<DefaultLossModel> > basketModels;
    std::vector<std::string> modelNames;
    std::vector<Real> relativeToleranceMidp, relativeTolerancePeriod,
        absoluteTolerance;

    if (hwData7[i].nm == -1 && hwData7[i].nz == -1){
        ext::shared_ptr<GaussianConstantLossLM> gaussKtLossLM(new
            GaussianConstantLossLM(hCorrelation,
            std::vector<Real>(poolSize, recovery),
            LatentModelIntegrationType::GaussianQuadrature, poolSize,
            GaussianCopulaPolicy::initTraits()));

        // 1.-Inhomogeneous gaussian
        modelNames.push_back("Inhomogeneous gaussian");
        basketModels.push_back(ext::shared_ptr<DefaultLossModel>( new
            IHGaussPoolLossModel(gaussKtLossLM, nBuckets, 5., -5, 15)));
        absoluteTolerance.push_back(1.);
        relativeToleranceMidp.push_back(0.04);
        relativeTolerancePeriod.push_back(0.04);
        // 2.-homogeneous gaussian
        modelNames.push_back("Homogeneous gaussian");
        basketModels.push_back(ext::shared_ptr<DefaultLossModel>( new
            HomogGaussPoolLossModel(gaussKtLossLM, nBuckets, 5., -5, 15)));
        absoluteTolerance.push_back(1.);
        relativeToleranceMidp.push_back(0.04);
        relativeTolerancePeriod.push_back(0.04);
        // 3.-random default gaussian
        modelNames.push_back("Random default gaussian");
        basketModels.push_back(ext::shared_ptr<DefaultLossModel>(new
            RandomDefaultLM<GaussianCopulaPolicy>(gaussKtLossLM, numSims)));
        absoluteTolerance.push_back(1.);
        relativeToleranceMidp.push_back(0.07);
        relativeTolerancePeriod.push_back(0.07);
        // SECOND MC
        // gaussian LHP
        modelNames.push_back("Gaussian LHP");
        basketModels.push_back(ext::shared_ptr<DefaultLossModel>(new
            GaussianLHPLossModel(hCorrelation,
                std::vector<Real>(poolSize, recovery))));
        absoluteTolerance.push_back(10.);
        relativeToleranceMidp.push_back(0.5);
        relativeTolerancePeriod.push_back(0.5);
        // Binomial...
        // Saddle point...
        // Recursive ...
    }
    else if (hwData7[i].nm > 0 && hwData7[i].nz > 0) {
        TCopulaPolicy::initTraits initTG;
        initTG.tOrders.push_back(hwData7[i].nm);
        initTG.tOrders.push_back(hwData7[i].nz);
        ext::shared_ptr<TConstantLossLM> TKtLossLM(new TConstantLossLM(
            hCorrelation, std::vector<Real>(poolSize, recovery),
            LatentModelIntegrationType::GaussianQuadrature,
            poolSize,
            initTG));
        // 1.-inhomogeneous studentT
        modelNames.push_back("Inhomogeneous student");
        basketModels.push_back(ext::shared_ptr<DefaultLossModel>( new
            IHStudentPoolLossModel(TKtLossLM, nBuckets, 5., -5., 15)));
        absoluteTolerance.push_back(1.);
        relativeToleranceMidp.push_back(0.04);
        relativeTolerancePeriod.push_back(0.04);
        // 2.-homogeneous student T
        modelNames.push_back("Homogeneous student");
        basketModels.push_back(ext::shared_ptr<DefaultLossModel>( new
            HomogTPoolLossModel(TKtLossLM, nBuckets, 5., -5., 15)));
        absoluteTolerance.push_back(1.);
        relativeToleranceMidp.push_back(0.04);
        relativeTolerancePeriod.push_back(0.04);
        // 3.-random default student T
        modelNames.push_back("Random default studentT");
        basketModels.push_back(ext::shared_ptr<DefaultLossModel>(new
            RandomDefaultLM<TCopulaPolicy>(TKtLossLM, numSims)));
        absoluteTolerance.push_back(1.);
        relativeToleranceMidp.push_back(0.07);
        relativeTolerancePeriod.push_back(0.07);
        // SECOND MC
        // Binomial...
        // Saddle point...
        // Recursive ...
    }
    else if (hwData7[i].nm > 0 && hwData7[i].nz == -1) {
        TCopulaPolicy::initTraits initTG;
        initTG.tOrders.push_back(hwData7[i].nm);
        initTG.tOrders.push_back(45);
        /* T_{55} is pretty close to a gaussian. Probably theres no need to
        be this conservative as the polynomial convolution gets shorter and
        faster as the order decreases.
        */
        ext::shared_ptr<TConstantLossLM> TKtLossLM(new TConstantLossLM(
            hCorrelation,
            std::vector<Real>(poolSize, recovery),
            LatentModelIntegrationType::GaussianQuadrature,
            poolSize,
            initTG));
        // 1.-inhomogeneous
        modelNames.push_back("Inhomogeneous student-gaussian");
        basketModels.push_back(ext::shared_ptr<DefaultLossModel>( new
            IHStudentPoolLossModel(TKtLossLM, nBuckets, 5., -5., 15)));
        absoluteTolerance.push_back(1.);
        relativeToleranceMidp.push_back(0.04);
        relativeTolerancePeriod.push_back(0.04);
        // 2.-homogeneous
        modelNames.push_back("Homogeneous student-gaussian");
        basketModels.push_back(ext::shared_ptr<DefaultLossModel>( new
            HomogTPoolLossModel(TKtLossLM, nBuckets, 5., -5., 15)));
        absoluteTolerance.push_back(1.);
        relativeToleranceMidp.push_back(0.04);
        relativeTolerancePeriod.push_back(0.04);
        // 3.-random default
        modelNames.push_back("Random default student-gaussian");
        basketModels.push_back(ext::shared_ptr<DefaultLossModel>(new
            RandomDefaultLM<TCopulaPolicy>(TKtLossLM, numSims)));
        absoluteTolerance.push_back(1.);
        relativeToleranceMidp.push_back(0.07);
        relativeTolerancePeriod.push_back(0.07);
        // SECOND MC
        // Binomial...
        // Saddle point...
        // Recursive ...
    }
    else if (hwData7[i].nm == -1 && hwData7[i].nz > 0) {
        TCopulaPolicy::initTraits initTG;
        initTG.tOrders.push_back(45);// pretty close to gaussian
        initTG.tOrders.push_back(hwData7[i].nz);
        ext::shared_ptr<TConstantLossLM> TKtLossLM(new TConstantLossLM(
            hCorrelation,
            std::vector<Real>(poolSize, recovery),
            LatentModelIntegrationType::GaussianQuadrature,
            poolSize,
            initTG));
        // 1.-inhomogeneous gaussian
        modelNames.push_back("Inhomogeneous gaussian-student");
        basketModels.push_back(ext::shared_ptr<DefaultLossModel>( new
            IHStudentPoolLossModel(TKtLossLM, nBuckets, 5., -5., 15)));
        absoluteTolerance.push_back(1.);
        relativeToleranceMidp.push_back(0.04);
        relativeTolerancePeriod.push_back(0.04);
        // 2.-homogeneous gaussian
        modelNames.push_back("Homogeneous gaussian-student");
        basketModels.push_back(ext::shared_ptr<DefaultLossModel>( new
            HomogTPoolLossModel(TKtLossLM, nBuckets, 5., -5., 15)));
        absoluteTolerance.push_back(1.);
        relativeToleranceMidp.push_back(0.04);
        relativeTolerancePeriod.push_back(0.04);
        // 3.-random default gaussian
        modelNames.push_back("Random default gaussian-student");
        basketModels.push_back(ext::shared_ptr<DefaultLossModel>(new
            RandomDefaultLM<TCopulaPolicy>(TKtLossLM, numSims)));
        absoluteTolerance.push_back(1.);
        relativeToleranceMidp.push_back(0.07);
        relativeTolerancePeriod.push_back(0.07);
        // SECOND MC
        // Binomial...
        // Saddle point...
        // Recursive ...
    }
    else {
        return;
    }

    for (Size j = 0; j < LENGTH(hwAttachment); j ++) {
        ext::shared_ptr<Basket> basketPtr (
            new Basket(asofDate, names, nominals, pool,
                hwAttachment[j], hwDetachment[j]));
        ostringstream trancheId;
        trancheId << "[" << hwAttachment[j] << " , " << hwDetachment[j]
            << "]";
        SyntheticCDO cdoe(basketPtr, Protection::Seller,
                          schedule, 0.0, premium, daycount, Following);

        for(Size im=0; im<basketModels.size(); im++) {

            basketPtr->setLossModel(basketModels[im]);

            cdoe.setPricingEngine(midPCDOEngine);
            check(i, j, modelNames[im]
                +std::string(" with midp integration on ")+trancheId.str(),
                cdoe.fairPremium() * 1e4, hwData7[i].trancheSpread[j],
                absoluteTolerance[im], relativeToleranceMidp[im]);

            cdoe.setPricingEngine(integralCDOEngine);
            check(i, j, modelNames[im]
                +std::string(" with step integration on ")+trancheId.str(),
                cdoe.fairPremium() * 1e4, hwData7[i].trancheSpread[j],
                absoluteTolerance[im], relativeTolerancePeriod[im]);
        }
    }
    #endif
}

test_suite* CdoTest::suite(SpeedLevel speed) {
    test_suite* suite = BOOST_TEST_SUITE("CDO tests");
    #ifndef QL_PATCH_SOLARIS
    if (speed == Slow) {
        #define BOOST_PP_LOCAL_MACRO(n) \
            suite->add(QUANTLIB_TEST_CASE(ext::bind(&CdoTest::testHW, n)));

        #define BOOST_PP_LOCAL_LIMITS (0, 4)
        #include BOOST_PP_LOCAL_ITERATE()
    }
    #endif
    return suite;
}


/**
 * this file will contain the content for the node that will run the AA241x mission.
 *
 * This should handle publishing the simulated sensor information (TODO: figure
 * out what was decided for the sensor type...)
 *
 * This should also handle any book-keeping information that may be needed for
 * scoring or analyzing the mission performance at the end of the flight.
 */

#include <fstream>
#include <iostream>
#include <vector>
#include <string>
#include <cmath>
#include <random>
#include <Eigen/Dense>  // eigen functions

#include <ros/ros.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <sensor_msgs/NavSatFix.h>
#include <mavros_msgs/State.h>

#include <aa241x_mission/MissionState.h>
#include <aa241x_mission/SensorMeasurement.h>
#include <aa241x_mission/PersonEstimate.h>
#include <aa241x_mission/CoordinateConversion.h>
#include <aa241x_mission/RequestLandingPosition.h>

#include "geodetic_trans.hpp"

class MissionNode {

public:

	// TODO: constructor
	// TODO: decide how the settings will be passed -> I think I want them as
	// inputs to the constructor instead of having the constructor pull the
	// private NH data
	MissionNode(int mission_index, std::string mission_file);

	// set some optional parameters
	void setLandingGPS(double landing_lat, double landing_lon);

	// TODO: any services to broadcast (NOTE: need to figure out what services might be neded)

	bool serviceGPStoLakeLagENU(aa241x_mission::CoordinateConversion::Request &req,
		aa241x_mission::CoordinateConversion::Response &res);

	/**
	 * service to request the landing position (position of the "truckbed") in
	 * the Lake Lag ENU frame
	 * @param  req service request (empty)
	 * @param  res service response
	 * @return     true if successfully able to provide data
	 */
	bool serviceRequestLandingPosition(aa241x_mission::RequestLandingPosition::Request &req,
		aa241x_mission::RequestLandingPosition::Response &res);

	// the main function to run the node
	int run();

private:

	// node handler
	ros::NodeHandle _nh;

	// mission settings
	int _mission_index = 0;
	std::string _mission_file;

	float _max_alt = 120;	// maximum allowed altitude [m]

	// sensor setting
	float _sensor_min_h = 30.0;			// min height AGL for the sensor [m]
	float _sensor_max_h = 100.0f;		// max height AGL for the sensor [m]
	float _sensor_d_mult = 5.0f/7.0f;	// multiplier for the equation (*h)
	float _sensor_d_offset = 28.57;		// [m]
	float _sensor_stddev_a = 2;				// min std dev (at height of 50m) [m]
	float _sensor_stddev_b = 1.0f/50.0f;	// scale factor on h [m]

	// lake specific parameters
	double _lake_ctr_lat = 37.4224444;		// [deg]
	double _lake_ctr_lon = -122.1760917;	// [deg]
	float _lake_ctr_alt = 40.0;				// AMSL [m]
	float _lake_ctr_alt_wgs84 = _lake_ctr_alt - 32.060;	// need WGS84 ellipsoid height for GPS data in ROS [m]
	float _lake_radius = 160.0f;			// in bound radius from the center [m]

	// mission monitoring
	bool _in_mission = false;		// true if mission is running
	bool _oob_failure = false;		// true if failed due to OOB
	bool _entered_area = false;		// true if within the operating bounds
	double _mission_time = 0.0;		// time since mission started in [sec]
	float _mission_score = 0.0;		// the current score

	// scoring monitoring
	float _estimate_found_r = 1.0f;			// the radial distance away to consider the person found
	std::vector<int> _scoring_state;        // scoring state (0: no estimate entered, 1: incorrect estimate, 2: correct estimate)
	int _number_people_found = 0;           // number of people found correctly

	// mission "people"
	std::vector<Eigen::Vector2f> _people;	// the positions of the people in the world

	// random sampling stuff
	std::default_random_engine _generator;

	// offsets to the local NED frame used by PX4
	float _e_offset = NAN;
	float _n_offset = NAN;
	float _u_offset = NAN;
	bool _lake_offset_computed = false;

	// landing coordinates
	double _landing_lat = 0.0;
	double _landing_lon = 0.0;
	float _landing_e = 0.0f;
	float _landing_n = 0.0f;
	bool _landing_set = false;

	// data
	geometry_msgs::PoseStamped _current_local_position;		// most recent local position info
	geometry_msgs::PoseStamped _temp_local;					// needed for offset calc
	bool _have_temp_local = false;
	mavros_msgs::State _current_state;						// most recent state info

	// subscribers
	ros::Subscriber _state_sub;			// pixhawk state
	ros::Subscriber _gps_sub;			// filtered GPS data from the pixhawk
	ros::Subscriber _local_pos_sub;		// pixhawk local position
	ros::Subscriber _person_found_sub;	// location of the found individual

	// publishers
	ros::Publisher _measurement_pub;	// simulated sensor "measurement"
	ros::Publisher _mission_state_pub;	// the current mission state
	ros::Publisher _lake_lag_pose_pub;	// the lake lag position as computed by the GPS data

	// services
	ros::ServiceServer _coord_conversion_srv;
	ros::ServiceServer _landing_loc_srv;

	// callbacks
	void stateCallback(const mavros_msgs::State::ConstPtr& msg);
	void gpsCallback(const sensor_msgs::NavSatFix::ConstPtr& msg);
	void localPosCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
	void personFoundCallback(const aa241x_mission::PersonEstimate::ConstPtr& msg);

	// helpers

	/**
	 * read in the mission file and load the positions of the people
	 */
	void loadMission();

	/**
	 * virtualization of the sensor
	 * makes the "measurement" to the people and publishes the data of those in
	 * view
	 */
	void makeMeasurement();

	/**
	 * publish the current mission state information
	 * this includes the frame offset from the local ENU frame to the lake lag
	 * ENU frame
	 */
	void publishMissionState();

};

MissionNode::MissionNode(int mission_index, std::string mission_file) :
_mission_index(mission_index),
_mission_file(mission_file),
_generator(ros::Time::now().toSec())
{
	// load the mission
	loadMission();

	// subscriptions
	_state_sub = _nh.subscribe<mavros_msgs::State>("mavros/state", 1, &MissionNode::stateCallback, this);
	_gps_sub = _nh.subscribe<sensor_msgs::NavSatFix>("/mavros/global_position/global", 1, &MissionNode::gpsCallback, this);
	_local_pos_sub = _nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 10, &MissionNode::localPosCallback, this);
	_person_found_sub = _nh.subscribe<aa241x_mission::PersonEstimate>("person_found", 10, &MissionNode::personFoundCallback, this);

	// advertise publishers
	_measurement_pub = _nh.advertise<aa241x_mission::SensorMeasurement>("measurement", 10);
	_mission_state_pub = _nh.advertise<aa241x_mission::MissionState>("mission_state", 10);
	_lake_lag_pose_pub = _nh.advertise<geometry_msgs::PoseStamped>("geodetic_based_lake_lag_pose", 10);

	// advertise coordinate conversion and landing location
	_coord_conversion_srv = _nh.advertiseService("gps_to_lake_lag", &MissionNode::serviceGPStoLakeLagENU, this);
	_landing_loc_srv = _nh.advertiseService("lake_lag_landing_loc", &MissionNode::serviceRequestLandingPosition, this);
}

void MissionNode::setLandingGPS(double landing_lat, double landing_lon) {
		_landing_lat = landing_lat;
		_landing_lon = landing_lon;
		_landing_set = true;

		// do the conversion
		float useless;
		geodetic_trans::lla2enu(_lake_ctr_lat, _lake_ctr_lon, _lake_ctr_alt_wgs84,
							landing_lat, landing_lon, 0.0f, &_landing_e, &_landing_n, &useless);
};

void MissionNode::stateCallback(const mavros_msgs::State::ConstPtr& msg) {
	_current_state = *msg;

	// check OFFBOARD based mission condition
	bool new_state = (_current_state.mode == "OFFBOARD");
	if (new_state != _in_mission) {
		publishMissionState();

		// update state related flags to their original state
		_oob_failure = false;
		_entered_area = false;
	}
	_in_mission = new_state;
}

void MissionNode::gpsCallback(const sensor_msgs::NavSatFix::ConstPtr& msg) {
	// need to be listening to the raw GPS data for knowing the reference point

	// compute the lake lake frame position
	float ll_east, ll_north, ll_up;

	double lat = msg->latitude;
	double lon = msg->longitude;
	float alt = msg->altitude;
	geodetic_trans::lla2enu(_lake_ctr_lat, _lake_ctr_lon, _lake_ctr_alt_wgs84,
							lat, lon, alt, &ll_east, &ll_north, &ll_up);

	// if we've already handled the offset computation, or don't have a fix yet
	// then continue
	if (!_lake_offset_computed && !(msg->status.status < 0) && _have_temp_local) {
		// save the current position value as the offset to the pixhawk local frame

		// need to acount for the fact that the drone may have moved from its (0,0,0) position

		_e_offset = ll_east - _temp_local.pose.position.x;
		_n_offset = ll_north - _temp_local.pose.position.y;
		_u_offset = ll_up - _temp_local.pose.position.z;

		// DEBUG
		ROS_INFO("offset computed as: (%0.2f, %0.2f, %0.2f)", _e_offset, _n_offset, _u_offset);

		// publish the mission state with this information
		publishMissionState();

		// make as computed
		_lake_offset_computed = true;
	}

	// also going to publish the lake lag frame position as computed by GPS
	geometry_msgs::PoseStamped lake_lag_pose_msg;
	lake_lag_pose_msg.header.stamp = msg->header.stamp;
	lake_lag_pose_msg.pose.position.x = ll_east;
	lake_lag_pose_msg.pose.position.y = ll_north;
	lake_lag_pose_msg.pose.position.z = ll_up;
	_lake_lag_pose_pub.publish(lake_lag_pose_msg);
}

void MissionNode::localPosCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) {

	// if the offset hasn't been computed, don't publish anything yet
	if (!_lake_offset_computed) {
		_temp_local = *msg;
		_have_temp_local = true;
		return;
	}

	// adjust the position with the offset (NOTE: this keeps the time the same)
	geometry_msgs::PoseStamped local_pos = *msg;
	local_pos.pose.position.x += _e_offset;
	local_pos.pose.position.y += _n_offset;
	local_pos.pose.position.z += _u_offset;

	// set the current position information to be the lake local position
	_current_local_position = local_pos;

	// check to see if within the appropriate bounds
	float x = local_pos.pose.position.x;
	float y = local_pos.pose.position.y;
	float d_from_center = sqrt(x*x + y*y);

	// if haven't entered the "play area" check if we have (and update accordingly)
	if (!_entered_area) {
		// NOTE: have a little bit of margin to account for any GPS noise
		if (d_from_center <= (_lake_radius - 5) && local_pos.pose.position.z >= _sensor_min_h) {
			_entered_area = true;
		}

	} else {	// we are in the "play area" and need to check if they violate the OOB conditions

		// if we are above 30m -> going to assume we are searching and therefore
		// need to check the OOB condition
		//
		// if we are < 30m -> assume going for a landing and allow the OOB since
		// landing area may not be within bounds
		if (local_pos.pose.position.z >= _sensor_min_h && d_from_center > _lake_radius) {
			// mark mission as failed (exit mission state and set score to 0)
			_in_mission = false;
			_mission_score = 0.0f;
			_oob_failure = true;

			// also immediately update the mission state
			publishMissionState();
		}

	}

	// if have exceeded the height threshold, immediately fail the mission
	if (local_pos.pose.position.z > _max_alt) {
		_in_mission = false;
		_mission_score = 0.0f;
		_oob_failure = true;

		// also immediately update the mission state
		publishMissionState();
	}
}

void MissionNode::personFoundCallback(const aa241x_mission::PersonEstimate::ConstPtr& msg) {

	// TODO: confirm with professor Alonso that no limit on the possible # of people found
	/*
	if (_number_people_found == 5) {
		return;
	}
	*/

	// don't score anything if not in the mission
	if (!_in_mission) {
		return;
	}

	double id = msg->id;
	float n = msg->n;
	float e = msg->e;

	// if not a valid ID, then return
	if (id < 0 || id > _people.size()) {
		return;
	}

	// get the student estimate
	Eigen::Vector2f est;
	est << n, e;

	// get the true location of the person
	Eigen::Vector2f loc = _people[id];

	// update the score
	if (_scoring_state[id] == 0) {
		// Increment the scoring state (so it cannot be scored again)
		_scoring_state[id] = 1;     // Assume incorrect estimate

		// If within the radius
		if ((loc - est).norm() <= _estimate_found_r) {
			// update number of people found and the mission score
			_number_people_found += 1;
			_mission_score += 10;

			// mark the estimate as having been correct
			_scoring_state[id] = 2;   // Correct estimate
		}
	}
}

void MissionNode::loadMission() {

	// open the mission file (display an error if the file does not exist)
	std::ifstream infile(_mission_file);
	if (!infile.good()) {
		ROS_ERROR("mission file does not exist!");
	}

	// import the data from the mission file into the people vector
	float n, e;
	while (infile >> n >> e) {
		// each person is represented by a 3 vector (NED)
		Eigen::Vector2f loc;
		loc << n, e;
		_people.push_back(loc);			// add the person location to the list
		_scoring_state.push_back(0);	// add a 0 for this ID in the scoring state vector

		// DEBUG
		ROS_INFO("adding person at: (%0.2f %0.2f)", n, e);
	}
}

void MissionNode::makeMeasurement() {

	// put the current local position (ENU) into an NED Egien vector
	float n = _current_local_position.pose.position.y;
	float e = _current_local_position.pose.position.x;
	Eigen::Vector2f current_pos;
	current_pos << n, e;

	// get the height information into a local variable for readibility
	float h = _current_local_position.pose.position.z;

	// don't publish a measurement if not heigh enough
	if (h < _sensor_min_h) {
		return;
	}

	// calculate FOV of the sensor
	float radius = (_sensor_d_mult * h + _sensor_d_offset) / 2.0f;	// [m]

	// get the sensor distribution based on the equation
	float sensor_std = _sensor_stddev_a + h * _sensor_stddev_b;
	std::normal_distribution<float> pos_distribution(0, sqrt(sensor_std));

	// the measurement message
	aa241x_mission::SensorMeasurement meas;
	meas.header.stamp = ros::Time::now();
	meas.num_measurements = 0;

	// check if there are any people in view
	for (uint8_t i = 0; i < _people.size(); i++) {
		Eigen::Vector2f pos = _people[i];

		// for each person in view, get a position measurement
		if ((current_pos - pos).norm() <= radius) {

			// get the N and E coordinates of the measurement
			n = pos_distribution(_generator) + pos(0);
			e = pos_distribution(_generator) + pos(1);

			// add to the message
			meas.num_measurements++;
			meas.id.push_back(i);
			meas.n.push_back(n);
			meas.e.push_back(e);
		}
	}

	// publish a list of position measurements or an empty measurement
	_measurement_pub.publish(meas);
}

void MissionNode::publishMissionState() {

	// populate the topic data
	aa241x_mission::MissionState mission_state;
	mission_state.header.stamp = ros::Time::now();
	mission_state.mission_time = _mission_time;

	// handle the mission state information -> if not in mission, depends on what happened
	if (!_in_mission) {

		if (_oob_failure) {
			mission_state.mission_state = aa241x_mission::MissionState::MISSION_FAILED_OOB;
		} else if (_entered_area) {
			mission_state.mission_state = aa241x_mission::MissionState::MISSION_FAILED_OTHER;
		} else {
			mission_state.mission_state = aa241x_mission::MissionState::MISSION_NOT_STARTED;
		}

	} else {
		mission_state.mission_state = aa241x_mission::MissionState::MISSION_RUNNING;
	}
	// NOTE: I think we will never have a good trigger for the mission ending, so yea

	// add the offset information
	mission_state.e_offset = _e_offset;
	mission_state.n_offset = _n_offset;
	mission_state.u_offset = _u_offset;

	// the current score
	mission_state.score = _mission_score;

	// publish the information
	_mission_state_pub.publish(mission_state);
}

int MissionNode::run() {

	uint8_t counter = 0;	// needed to rate limit the mission state info
	ros::Rate rate(10);		// run the loop at 1Hz, which allows mission state at 0.5Hz and measurement at 1/3Hz
	double last_measurement_time = ros::Time::now().toSec();	// time of the last measurement

	while (ros::ok()) {

		// make a measurement at 1/3 Hz (and if the mission conditions are met)
		float h = _current_local_position.pose.position.z;
		double current_time = ros::Time::now().toSec();
		if (_in_mission && h >= _sensor_min_h && h <= _sensor_max_h && ((current_time - last_measurement_time) >= (3.0f))) {
			makeMeasurement();

			// update time on measurement
			last_measurement_time = current_time;
		}

		// publish the mission state information
		// rate limit this information to a lower rate (e.g. 0.5 Hz)
		// NOTE: anything that changes the values in the state will cause the
		// state to be published so that critical data is sent immediately
		if (counter % 20 == 0) {
			publishMissionState();
		}

		// ros handling + increasing the counter to rate limit topics
		ros::spinOnce();
		rate.sleep();
		counter++;
	}

	return EXIT_SUCCESS;
}

bool MissionNode::serviceGPStoLakeLagENU(aa241x_mission::CoordinateConversion::Request &req,
		aa241x_mission::CoordinateConversion::Response &res) {

	// given a gps lat, lon, alt point
	// need to convert it to the Lake Lag frame ENU coordinate

	float lat = req.latitude;
	float lon = req.longitude;
	float alt = req.altitude;
	float pos_e, pos_n, pos_u;

	geodetic_trans::lla2enu(_lake_ctr_lat, _lake_ctr_lon, _lake_ctr_alt_wgs84,
							lat, lon, alt, &pos_e, &pos_n, &pos_u);

	res.east = pos_e;
	res.north = pos_n;
	res.up = pos_u;

	return true;
}

bool MissionNode::serviceRequestLandingPosition(aa241x_mission::RequestLandingPosition::Request &req,
		aa241x_mission::RequestLandingPosition::Response &res) {

	// return the saved information for the landing position of the drone
	// NOTE: if GPS coordinates not set in the launch file -> return false

	if (!_landing_set) {
		return false;
	}

	// the 2D coordinates to the landing location
	res.east = _landing_e;
	res.north = _landing_n;

	return true;
}

int main(int argc, char **argv) {

	// initialize th enode
	ros::init(argc, argv, "mission_node");

	// get parameters from the launch file which define some mission
	// settings
	ros::NodeHandle private_nh("~");
	// TODO: determine settings
	int mission_index = 0;
	std::string mission_file;
	double landing_lat, landing_lon;

	private_nh.param("mission_index", mission_index, 0);
	if (!private_nh.getParam("mission_file", mission_file)) {
		ROS_ERROR("failed to get mission file");
	}

	// create the node
	MissionNode node(mission_index, mission_file);

	// handling the optional parameters
	if (private_nh.getParam("landing_lat", landing_lat) && private_nh.getParam("landing_lon", landing_lon)) {
		node.setLandingGPS(landing_lat, landing_lon);
		ROS_INFO("landing position coordinates: (%0.2f, %0.2f)", landing_lat, landing_lon);
	} else {
		ROS_INFO("[AA241x] no landing position set");
	}

	// run the node
	return node.run();
}


// Copyright John Maddock 2006.
// Copyright Paul A. Bristow 2007, 2009
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/math/concepts/real_concept.hpp>
#include <boost/test/test_exec_monitor.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/math/special_functions/trunc.hpp>
#include <boost/math/tools/test.hpp>
#include "functor.hpp"
#include <boost/array.hpp>

#include "handle_test_result.hpp"
#include "table_type.hpp"

#ifndef SC_
#define SC_(x) static_cast<typename table_type<T>::type>(BOOST_JOIN(x, L))
#endif

template <class T>
T binomial_wrapper(T n, T k)
{
   return boost::math::binomial_coefficient<T>(
      boost::math::itrunc(n),
      boost::math::itrunc(k));
}

template <class T>
void test_binomial(T, const char* type_name)
{
   using namespace std;

   typedef T (*func_t)(T, T);
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   func_t f = &binomial_wrapper<T>;
#else
   func_t f = &binomial_wrapper;
#endif

#include "binomial_data.ipp"

   boost::math::tools::test_result<T> result = boost::math::tools::test_hetero<T>(
      binomial_data,
      bind_func<T>(f, 0, 1),
      extract_result<T>(2));

   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n"
      "Test results for small arguments and type " << type_name << std::endl << std::endl;
   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n";
   handle_test_result(result, binomial_data[result.worst()], result.worst(), type_name, "binomial_coefficient", "Binomials: small arguments");
   std::cout << std::endl;

#include "binomial_large_data.ipp"

   result = boost::math::tools::test_hetero<T>(
      binomial_large_data,
      bind_func<T>(f, 0, 1),
      extract_result<T>(2));

   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n"
      "Test results for large arguments and type " << type_name << std::endl << std::endl;
   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n";
   handle_test_result(result, binomial_large_data[result.worst()], result.worst(), type_name, "binomial_coefficient", "Binomials: large arguments");
   std::cout << std::endl;
}


#include <catch2/catch.hpp>
#include <Euclid/Math/Numeric.h>

#include <iostream>
#include <boost/math/constants/constants.hpp>

TEST_CASE("Math, Numeric", "[math][numeric]")
{
    SECTION("equality check")
    {
        float zerof = 0.0f;
        float almost_zerof = std::sin(boost::math::double_constants::pi);
        float onef = 1.0f;
        float almost_onef = 0.0f;
        for (int i = 0; i < 10; ++i) {
            almost_onef += 0.1f;
        }
        double smalld = 1.0e-8;
        double almost_smalld = smalld + 1.0e-16;
        double larged = 1.0e8;
        double almost_larged = larged + 1.0e-8;
        REQUIRE(zerof != almost_zerof);
        REQUIRE(onef != almost_onef);
        REQUIRE(smalld != almost_smalld);
        REQUIRE(larged != almost_larged);

        SECTION("eq_almost is robust in most cases")
        {
            REQUIRE(Euclid::eq_almost(zerof, almost_zerof));
            REQUIRE(Euclid::eq_almost(smalld, almost_smalld));
            REQUIRE(Euclid::eq_almost(onef, almost_onef));
            REQUIRE(Euclid::eq_almost(larged, almost_larged));

            double fail = 1.0e-8;
            double almost_fail = fail + 1.0e-14;
            REQUIRE(!Euclid::eq_almost(fail, almost_fail));
            REQUIRE(Euclid::eq_almost(fail, almost_fail, 1.0e-13));
        }

        SECTION("eq_rel_err is valid when not comparing to small values")
        {
            REQUIRE(!Euclid::eq_rel_err(zerof, almost_zerof));
            REQUIRE(Euclid::eq_rel_err(
                zerof,
                almost_zerof,
                1.0e10f * std::numeric_limits<float>::epsilon()));
            REQUIRE(!Euclid::eq_rel_err(smalld, almost_smalld));
            REQUIRE(Euclid::eq_rel_err(
                smalld,
                almost_smalld,
                1.0e8 * std::numeric_limits<double>::epsilon()));
            REQUIRE(Euclid::eq_rel_err(onef, almost_onef));
            REQUIRE(Euclid::eq_rel_err(larged, almost_larged));
        }

        SECTION("eq_abs_err is only valid when the bound is known")
        {
            REQUIRE(Euclid::eq_abs_err(zerof, almost_zerof));
            REQUIRE(Euclid::eq_abs_err(smalld, almost_smalld));
            REQUIRE(!Euclid::eq_abs_err(onef, almost_onef));
            REQUIRE(Euclid::eq_abs_err(onef, almost_onef, 1.0e-6f));
            REQUIRE(!Euclid::eq_abs_err(larged, almost_larged));
            REQUIRE(Euclid::eq_abs_err(larged, almost_larged, 1.0e-7));
        }

        SECTION("eq_ulp is also robust when not comparing to small values")
        {
            REQUIRE(!Euclid::eq_ulp(zerof, almost_zerof));
            REQUIRE(!Euclid::eq_ulp(smalld, almost_smalld));
            REQUIRE(Euclid::eq_ulp(onef, almost_onef, 1));
            REQUIRE(!Euclid::eq_ulp(onef, almost_onef, 0));
            REQUIRE(Euclid::eq_ulp(larged, almost_larged, 1));
        }
    }

    SECTION("inequality check")
    {
        float small = 0.1f;
        float large = 1000.0f;
        float margin = 1.0e-15f;

        REQUIRE(!(small + margin < small));
        REQUIRE(Euclid::less_safe(small + margin, small, 1.0e-14f));
        REQUIRE(!(large + margin < large));
        REQUIRE(Euclid::less_safe(large + margin, large));
        REQUIRE(!(small > small + margin));
        REQUIRE(Euclid::greater_safe(small, small + margin, 1.0e-14f));
        REQUIRE(!(large > large + margin));
        REQUIRE(Euclid::greater_safe(large, large + margin));
    }
}


//
// Copyright (c) 2016-2020 CNRS INRIA
//

#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/parsers/sample-models.hpp"

#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/utils/timer.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

template<typename JointModel>
void test_joint_methods(const pinocchio::JointModelBase<JointModel> & jmodel)
{
  typedef typename pinocchio::JointModelBase<JointModel>::JointDataDerived JointData;
  typedef typename JointModel::ConfigVector_t ConfigVector_t;
  typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType;

  JointData jdata = jmodel.createData();

  ConfigVector_t ql(ConfigVector_t::Constant(jmodel.nq(),-M_PI));
  ConfigVector_t qu(ConfigVector_t::Constant(jmodel.nq(),M_PI));

  ConfigVector_t q = LieGroupType().randomConfiguration(ql,qu);
  pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Random().matrix());
  pinocchio::Inertia::Matrix6 I_check = I;

  jmodel.calc(jdata,q);
  jmodel.calc_aba(jdata,I,true);

  Eigen::MatrixXd S = jdata.S.matrix();
  Eigen::MatrixXd U_check = I_check*S;
  Eigen::MatrixXd D_check = S.transpose()*U_check;
  Eigen::MatrixXd Dinv_check = D_check.inverse();
  Eigen::MatrixXd UDinv_check = U_check*Dinv_check;
  Eigen::MatrixXd update_check = U_check*Dinv_check*U_check.transpose();
  I_check -= update_check;

  BOOST_CHECK(jdata.U.isApprox(U_check));
  BOOST_CHECK(jdata.Dinv.isApprox(Dinv_check));
  BOOST_CHECK(jdata.UDinv.isApprox(UDinv_check));

  // Checking the inertia was correctly updated
  // We use isApprox as usual, except for the freeflyer,
  // where the correct result is exacly zero and isApprox would fail.
  // Only for this single case, we use the infinity norm of the difference
  if(jmodel.shortname() == "JointModelFreeFlyer")
    BOOST_CHECK((I-I_check).lpNorm<Eigen::Infinity>() < Eigen::NumTraits<double>::dummy_precision());
  else
    BOOST_CHECK(I.isApprox(I_check));
}

struct TestJointMethods{

  template <typename JointModel>
  void operator()(const pinocchio::JointModelBase<JointModel> &) const
  {
    JointModel jmodel;
    jmodel.setIndexes(0,0,0);

    test_joint_methods(jmodel);
  }

  void operator()(const pinocchio::JointModelBase<pinocchio::JointModelComposite> &) const
  {
    pinocchio::JointModelComposite jmodel_composite;
    jmodel_composite.addJoint(pinocchio::JointModelRX());
    jmodel_composite.addJoint(pinocchio::JointModelRY());
    jmodel_composite.setIndexes(0,0,0);

    //TODO: correct LieGroup
    //test_joint_methods(jmodel_composite);

  }

  void operator()(const pinocchio::JointModelBase<pinocchio::JointModelRevoluteUnaligned> &) const
  {
    pinocchio::JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
    jmodel.setIndexes(0,0,0);

    test_joint_methods(jmodel);
  }

  void operator()(const pinocchio::JointModelBase<pinocchio::JointModelPrismaticUnaligned> &) const
  {
    pinocchio::JointModelPrismaticUnaligned jmodel(1.5, 1., 0.);
    jmodel.setIndexes(0,0,0);

    test_joint_methods(jmodel);
  }

};

BOOST_AUTO_TEST_CASE( test_joint_basic )
{
  using namespace pinocchio;

  typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned
  , JointModelSpherical, JointModelSphericalZYX
  , JointModelPX, JointModelPY, JointModelPZ
  , JointModelPrismaticUnaligned
  , JointModelFreeFlyer
  , JointModelPlanar
  , JointModelTranslation
  , JointModelRUBX, JointModelRUBY, JointModelRUBZ
  > Variant;

  boost::mpl::for_each<Variant::types>(TestJointMethods());
}

BOOST_AUTO_TEST_CASE ( test_aba_simple )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model; buildModels::humanoidRandom(model);

  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);

  VectorXd q = VectorXd::Ones(model.nq);
  q.segment<4>(3).normalize();
  VectorXd v = VectorXd::Ones(model.nv);
  VectorXd tau = VectorXd::Zero(model.nv);
  VectorXd a = VectorXd::Ones(model.nv);

  tau = rnea(model, data_ref, q, v, a);
  aba(model, data, q, v, tau);

  for(size_t k = 1; k < (size_t)model.njoints; ++k)
  {
    BOOST_CHECK(data_ref.liMi[k].isApprox(data.liMi[k]));
    BOOST_CHECK(data_ref.v[k].isApprox(data.v[k]));
  }

  BOOST_CHECK(data.ddq.isApprox(a, 1e-12));

}

BOOST_AUTO_TEST_CASE ( test_aba_with_fext )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model; buildModels::humanoidRandom(model);

  pinocchio::Data data(model);

  VectorXd q = VectorXd::Random(model.nq);
  q.segment<4>(3).normalize();
  VectorXd v = VectorXd::Random(model.nv);
  VectorXd a = VectorXd::Random(model.nv);

  PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext(model.joints.size(), Force::Random());

  crba(model, data, q);
  computeJointJacobians(model, data, q);
  nonLinearEffects(model, data, q, v);
  data.M.triangularView<Eigen::StrictlyLower>()
  = data.M.transpose().triangularView<Eigen::StrictlyLower>();

  VectorXd tau = data.M * a + data.nle;
  Data::Matrix6x J = Data::Matrix6x::Zero(6, model.nv);
  for(Model::Index i=1;i<(Model::Index)model.njoints;++i) {
    getJointJacobian(model, data, i, LOCAL, J);
    tau -= J.transpose()*fext[i].toVector();
    J.setZero();
  }
  aba(model, data, q, v, tau, fext);

  BOOST_CHECK(data.ddq.isApprox(a, 1e-12));
}

BOOST_AUTO_TEST_CASE ( test_aba_vs_rnea )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model; buildModels::humanoidRandom(model);

  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);

  VectorXd q = VectorXd::Ones(model.nq);
  VectorXd v = VectorXd::Ones(model.nv);
  VectorXd tau = VectorXd::Zero(model.nv);
  VectorXd a = VectorXd::Ones(model.nv);

  crba(model, data_ref, q);
  nonLinearEffects(model, data_ref, q, v);
  data_ref.M.triangularView<Eigen::StrictlyLower>()
  = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();

  tau = data_ref.M * a + data_ref.nle;
  aba(model, data, q, v, tau);

  VectorXd tau_ref = rnea(model, data_ref, q, v, a);
  BOOST_CHECK(tau_ref.isApprox(tau, 1e-12));

  BOOST_CHECK(data.ddq.isApprox(a, 1e-12));

}

BOOST_AUTO_TEST_CASE ( test_computeMinverse )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  buildModels::humanoidRandom(model);
  model.gravity.setZero();

  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v = VectorXd::Random(model.nv);

  crba(model, data_ref, q);
  data_ref.M.triangularView<Eigen::StrictlyLower>()
  = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
  MatrixXd Minv_ref(data_ref.M.inverse());

  computeMinverse(model, data, q);

  BOOST_CHECK(data.Minv.topRows<6>().isApprox(Minv_ref.topRows<6>()));

  data.Minv.triangularView<Eigen::StrictlyLower>()
  = data.Minv.transpose().triangularView<Eigen::StrictlyLower>();

  BOOST_CHECK(data.Minv.isApprox(Minv_ref));

//  std::cout << "Minv:\n" << data.Minv.block<10,10>(0,0) << std::endl;
//  std::cout << "Minv_ref:\n" << Minv_ref.block<10,10>(0,0) << std::endl;
//
//  std::cout << "Minv:\n" << data.Minv.bottomRows<10>() << std::endl;
//  std::cout << "Minv_ref:\n" << Minv_ref.bottomRows<10>() << std::endl;

}

BOOST_AUTO_TEST_CASE(test_multiple_calls)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data1(model), data2(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);

  computeMinverse(model,data1,q);
  data2 = data1;

  for(int k = 0; k < 20; ++k)
  {
    computeMinverse(model,data1,q);
  }

  BOOST_CHECK(data1.Minv.isApprox(data2.Minv));
}

BOOST_AUTO_TEST_SUITE_END ()


/*

	pclub03.cpp

	pclub01.cppから派生。01は以下のもの。
	@Programming Club, Imaplus, Nov 16, 2019
	Instant Test for kdatasettest00.cpp

	Written by Koji Yamamoto
	Copyright (C) 2019-2020 Koji Yamamoto

	TODO:
	度数分布表をつくる。kstatを見て。
	　別に、連続変数用の機能をつける。
	　　start/end, width, bin を指定する方式。
	　　自動で、スタージェスの公式を使う方式。
	　　階級の端点の表を与える方式。
	ヒストグラムを描く。
	SVGにする。

*/

/* ********** Preprocessor Directives ********** */

#include <k09/kdataset01.cpp>
#include <k09/kstat02.cpp>
#include <k09/koutputfile00.cpp>
#include <iostream>
#include <iomanip>
#include <algorithm>

#include <boost/algorithm/string.hpp>

/* ********** Namespace Declarations/Directives ********** */

using namespace std;

/* ********** Class Declarations ********** */

/* ********** Enum Definitions ********** */

/* ********** Function Declarations ********** */

int main( int, char *[]);

void drawHistogramToSvg(
	const std::string &,
	const std::vector <double> &, const std::vector <double> &,
	const std::vector <int> &,
	bool = false
);

std::vector <double>
getGridPoints( double, double, int = 4, bool = true, bool = true);

/* ********** Class Definitions ********** */

/* ********** Global Variables ********** */

/* ********** Definitions of Static Member Variables ********** */

/* ********** Function Definitions ********** */

int main( int, char *[])
{

	vector <double> dvec;
	vector <double> dvecclean;

	{
		Dataset ds;
		bool b;

		cout << "Reading data...";
		b = ds.readCsvFile( "jhpsmerged_191029_v403.csv");
		if ( b == false){
			return 0;
		}
		cout << "Done." << endl;

		cout << "Fixing variable types...";
		int nnum, nmis;
		ds.fixVariableType( nnum, nmis);
		cout << "Done." << endl;

		cout << "Getting numeric vector before specifying missing...";
		b = ds.getNumericVectorWithoutMissing( dvec, "v403");
		if ( b == false){
			return 0;
		}
		cout << "Done." << endl;

		cout << "Specifying missing cases...";
		ds.specifyValid(
			"v403",
			[]( double v)->bool{ return ( v < 99999.0);}
		);
		cout << "Done." << endl;

		cout << "Getting numeric vector excl. missing...";
		b = ds.getNumericVectorWithoutMissing( dvecclean, "v403");
		if ( b == false){
			return 0;
		}
		cout << "Done." << endl;

	}

	cout << endl;
	cout << "***************************************************" << endl;
	cout << "JHPS 2009 Household Income incl. Tax" << endl;
	cout << "Calculated by mean() and median()" << endl;
	cout << "Mean:   " << setprecision( 15) << mean( dvecclean)   << " (Ten Thousand Yen)" << endl;
	cout << "Median: " << median( dvecclean) << " (Ten Thousand Yen)" << endl;
	cout << "***************************************************" << endl;
	cout << "FYI: Mean from \"dirty\" data: " << setprecision( 15) << mean( dvec) << endl;

	// 度数分布表

	cout << endl;
	cout << "Number of unique values: " << countUniqueValues( dvecclean) << endl;
	cout << "FYI Number of unique values in \"dirty\" vector: " << countUniqueValues( dvec) << endl << endl;

	RecodeTable <double, int> rt;
	rt.setAutoTableFromContVar( dvecclean);

	cout << "RecodeTable:" << endl;
	rt.print( cout, ",");
	cout << endl;

	FreqType <int, int> ft;
	ft.setFreqFromRecodeTable( dvecclean, rt);

	ft.printPadding( cout);

	// ヒストグラムをつくりたい。

	vector <int> codes;
	vector <int> counts;
	vector <double> leftvec;
	vector <double> rightvec;
	ft.getVectors( codes, counts);
	ft.getRangeVectors( leftvec, rightvec);

	drawHistogramToSvg( "pclub03out01.svg", leftvec, rightvec, counts);
	drawHistogramToSvg( "pclub03out02.svg", leftvec, rightvec, counts, true); // アニメバージョン

	return 0;

	// 今のRecodeTableには、左端・右端がない（無限大）という指定ができない。

	// FreqTypeにはすごく小さい機能だけを持たせることにして、
	// 別にFreqTableTypeか何かをつくって、そこに、RecodeTableを持たせたり、
	// それをもとにしたFreqを作らせたりしてもよいかも。

	/*
	度数分布表をつくる。kstatを見て。
	　別に、連続変数用の機能をつける。
	　　start/end, width, bin を指定する方式。
	　　自動で、スタージェスの公式を使う方式？
	　　※Stataでは、min{ sqrt(N), 10*ln(N)/ln(10)}らしいので、それでいく。
	　　階級の端点の表を与える方式。
	*/

/*
	// ちょうどいい間隔と基準点の実験。
	{
		vector <double> gridpoints = getGridPoints( -12.34, 567.8);
		for ( auto d : gridpoints){
			cout << d << endl;
		}
		cout << endl;
	}

	{
		vector <double> gridpoints = getGridPoints( -12.34, 567.8, 4, false, false);
		for ( auto d : gridpoints){
			cout << d << endl;
		}
		cout << endl;
	}

	{
		vector <double> gridpoints = getGridPoints( -1234.5, 567.8);
		for ( auto d : gridpoints){
			cout << d << endl;
		}
		cout << endl;
	}

	{
		vector <double> gridpoints = getGridPoints( -1234.5, 567.8, 5);
		for ( auto d : gridpoints){
			cout << d << endl;
		}
		cout << endl;
	}

	{
		vector <double> gridpoints = getGridPoints( 123.5, 5678.9);
		for ( auto d : gridpoints){
			cout << d << endl;
		}
		cout << endl;
	}

	{
		vector <double> gridpoints = getGridPoints( -80001.0, -299.9);
		for ( auto d : gridpoints){
			cout << d << endl;
		}
		cout << endl;
	}

	{
		vector <double> gridpoints = getGridPoints( -80001.0, -299.9, 5);
		for ( auto d : gridpoints){
			cout << d << endl;
		}
		cout << endl;
	}
*/
}

void drawHistogramToSvg(
	const std::string &fn,
	const std::vector <double> &leftvec, const std::vector <double> &rightvec,
	const std::vector <int> &counts,
	bool animated /*= false*/
)
{

	using namespace std;

	// SVGのviewBoxについて：アスペクト比が違っているとわかりにくい。
	// （強制的に余白がつくられたりするか、強制的に拡大縮小して円が歪んだりする）ので、
	// svgタグのサイズとviewBoxのサイズを合わせたい。

	struct Cambus;
	struct Point;

	struct Point {

		double x, y;

		Point( void)
		 : x( std::numeric_limits<double>::quiet_NaN()),
		   y( std::numeric_limits<double>::quiet_NaN())
		{}

		Point( double x0, double y0) : x( x0), y( y0)
		{}

	};

	struct Cambus {

		// 実際の座標系では、y座標は大きいほど「下」の位置を示す。
		// 論理座標系では、y座標は大きいほど「上」の位置を示す。

		double actuXMin, actuYMin, actuXMax, actuYMax; // 実際の座標系での、枠の範囲
		double actuWidth, actuHeight; // 同上

		double theoXMin, theoYMin, theoXMax, theoYMax; // 論理座標系での、枠の範囲
		double theoWidth, theoHeight; // 同上

		void setTheoretical( double xmin0, double ymin0, double xmax0, double ymax0)
		{
			theoXMin = xmin0; theoYMin = ymin0; theoXMax = xmax0; theoYMax = ymax0;
			theoWidth = xmax0 - xmin0; theoHeight = ymax0 - ymin0;
		}

		void setActual( double xmin0, double ymin0, double xmax0, double ymax0)
		{
			actuXMin = xmin0; actuYMin = ymin0; actuXMax = xmax0; actuYMax = ymax0;
			actuWidth = xmax0 - xmin0; actuHeight = ymax0 - ymin0;
		}

		// 論理座標系表現から実際の座標系表現を作成。
		Point getActualFromTheoretical( const Point &poi0)
		{

			double x0 = poi0.x;
			double y0 = poi0.y;
			Point ret;
			ret.x = ( x0 - theoXMin) / theoWidth  * actuWidth  + actuXMin;
			ret.y = ( theoYMax - y0) / theoHeight * actuHeight + actuYMin;
			return ret;

		}

		// x座標のみを算出→論理座標系表現から実際の座標系表現を作成。
		double getXActualFromTheoretical( double x0)
		{

			double retx = ( x0 - theoXMin) / theoWidth  * actuWidth  + actuXMin;
			return retx;

		}

		// y座標のみを算出→論理座標系表現から実際の座標系表現を作成。
		double getYActualFromTheoretical( double y0)
		{

			double rety = ( theoYMax - y0) / theoHeight * actuHeight + actuYMin;
			return rety;

		}

		// 実際の座標系での中点のxを返す。
		double getActualMidX( void)
		{

			return ( actuXMin + actuWidth / 2);

		}

		// 実際の座標系での中点のyを返す。
		double getActualMidY( void)
		{

			return ( actuYMin + actuHeight / 2);

		}

	};

	vector <string> svglines;
	Cambus cam;

	// SVG領域の大きさと、座標系のある領域の大きさを指定することで、それらしく計算してほしい。

	// ちょうどいい間隔のグリッド線の点と、範囲を得る。

	// x軸
	double xminval = leftvec.front();
	double xmaxval = rightvec.back();
	vector <double> xgridpoints = getGridPoints( xminval, xmaxval);
	for ( auto d : xgridpoints){
		cout << d << endl;
	}
	cout << endl;
	// y軸
	double ymaxval = *( max_element( counts.begin(), counts.end()));
	vector <double> ygridpoints = getGridPoints( 0, ymaxval);
	for ( auto d : ygridpoints){
		cout << d << endl;
	}
	cout << endl;

	// 描画範囲は、Gridpointsのさらに5%外側にする。
	double theoWidthTemp = xgridpoints.back() - xgridpoints.front();
	double theoXMin = xgridpoints.front() - 0.05 * theoWidthTemp;
	double theoXMax = xgridpoints.back() + 0.05 * theoWidthTemp;

	double theoHeightTemp = ygridpoints.back() - ygridpoints.front();
	double theoYMin = ygridpoints.front() - 0.05 * theoHeightTemp;
	double theoYMax = ygridpoints.back() + 0.05 * theoHeightTemp;

	// SVGファイル化の開始

	cam.setActual( 50, 50, 450, 450);
	cam.setTheoretical( theoXMin, theoYMin, theoXMax, theoYMax);

	svglines.push_back( R"(<?xml version="1.0" encoding="UTF-8" ?>)"); // This should be exactly in the first line.
	svglines.push_back( R"(<svg width="500px" height="500px" viewBox="0 0 500 500" xmlns="http://www.w3.org/2000/svg">)");
	svglines.push_back( R"(<rect x="0" y="0" width="500" height="500" fill="whitesmoke" stroke-width="0" />)");

	// 背景の描画開始

	// 背景色だけ塗る。
	svglines.push_back( R"(  <rect x="50" y="50" width="400" height="400" fill="gainsboro" stroke-width="0" />)");

	// x軸の目盛を示すグリッド線
	// <g>で属性一括指定：開始
	{
		stringstream ss;
		ss << "  "
		   << R"(<g)"
		   << " "
		   << R"(stroke=")" << "silver" << R"(")"
		   << " "
		   << R"(stroke-width=")" << 1 << R"(")"
		   << R"(>)";
		svglines.push_back( ss.str());
	}
	for ( auto v : xgridpoints){

		Point theoP1( v, theoYMax); // top
		Point theoP2( v, theoYMin); // bottom
		Point actuP1 = cam.getActualFromTheoretical( theoP1);
		Point actuP2 = cam.getActualFromTheoretical( theoP2);

		stringstream ss;
		ss << "    "
		<< R"(<line)"
		<< " "
		<< R"(x1=")" << actuP1.x << R"(")"
		<< " "
		<< R"(y1=")" << actuP1.y << R"(")"
		<< " "
		<< R"(x2=")" << actuP2.x << R"(")"
		<< " "
		<< R"(y2=")" << actuP2.y << R"(")"
		<< " "
		<< R"(/>)";

		svglines.push_back( ss.str());

	}
	// <g>で属性一括指定：終了
	{
		svglines.push_back( "  </g>");
	}

	// y軸の目盛を示すグリッド線
	// <g>で属性一括指定：開始
	{
		stringstream ss;
		ss << "  "
		   << R"(<g)"
		   << " "
		   << R"(stroke=")" << "silver" << R"(")"
		   << " "
		   << R"(stroke-width=")" << 1 << R"(")"
		   << R"(>)";
		svglines.push_back( ss.str());
	}
	for ( auto v : ygridpoints){

		Point theoP1( theoXMin, v); // left
		Point theoP2( theoXMax, v); // right
		Point actuP1 = cam.getActualFromTheoretical( theoP1);
		Point actuP2 = cam.getActualFromTheoretical( theoP2);

		stringstream ss;
		ss << "    "
		<< R"(<line)"
		<< " "
		<< R"(x1=")" << actuP1.x << R"(")"
		<< " "
		<< R"(y1=")" << actuP1.y << R"(")"
		<< " "
		<< R"(x2=")" << actuP2.x << R"(")"
		<< " "
		<< R"(y2=")" << actuP2.y << R"(")"
		<< " "
		<< R"(/>)";

		svglines.push_back( ss.str());

	}
	// <g>で属性一括指定：終了
	{
		svglines.push_back( "  </g>");
	}

	// 背景の描画終了

	// メインの情報の描画開始

	// 度数を示すバー。
	// 注：これを目盛グリッド線よりもあとに描くべし。グリッド線を「上書き」してほしいから。
	// <g>で属性一括指定：開始
	{
		stringstream ss;
		ss << "  "
		   << R"(<g)"
		   << " "
		   << R"(stroke=")" << "Gray" << R"(")"
		   << " "
		   << R"(fill=")" << "Gray" << R"(")"
		   << R"(>)";
		svglines.push_back( ss.str());
	}
	for ( int i = 0; i < counts.size(); i++){

		Point theoP1( leftvec[ i], counts[ i]); // left-top
		Point theoP2( rightvec[ i], 0); // right-bottom
		Point actuP1 = cam.getActualFromTheoretical( theoP1);
		Point actuP2 = cam.getActualFromTheoretical( theoP2);

		if ( animated == true){

			// 以下はアニメ用
			// SVGアニメをパワポに貼っても動かないらしい。

			stringstream ss;

			ss << "    "
			<< R"(<rect)"
			<< " "
			<< R"(x=")" << actuP1.x << R"(")"
			<< " "
			<< R"(y=")" << actuP1.y << R"(")"
			<< " "
			<< R"(width=")" << ( actuP2.x - actuP1.x) << R"(")"
			<< " "
			<< R"(height=")" << ( actuP2.y - actuP1.y) << R"(")"
			<< " "
			<< R"(>)";
			svglines.push_back( ss.str());

			ss.str( "");
			ss << R"(      <animate attributeName="height" begin="0s" dur="1s" from="0" to=")" << ( actuP2.y - actuP1.y) << R"(" repeatCount="1"/>)";
			svglines.push_back( ss.str());

			ss.str( "");
			ss << R"(      <animate attributeName="y" begin="0s" dur="1s" from=")" << actuP2.y << R"(" to=")" << actuP1.y << R"(" repeatCount="1"/>)";
			svglines.push_back( ss.str());

			svglines.push_back( R"(</rect>)");

		} else {

			stringstream ss;
			ss << "    "
			<< R"(<rect)"
			<< " "
			<< R"(x=")" << actuP1.x << R"(")"
			<< " "
			<< R"(y=")" << actuP1.y << R"(")"
			<< " "
			<< R"(width=")" << ( actuP2.x - actuP1.x) << R"(")"
			<< " "
			<< R"(height=")" << ( actuP2.y - actuP1.y) << R"(")"
			<< " "
			<< R"(/>)";
			svglines.push_back( ss.str());

		}

	}
	// <g>で属性一括指定：終了
	{
		svglines.push_back( "  </g>");
	}

	// メインの情報の描画終了

	// 周辺情報記載の開始

	// TODO: フォントサイズを自動調整→優先順位が低い。フォントサイズ固定でもいい。

	// x軸の目盛のヒゲ
	// <g>で属性一括指定：開始
	{
		stringstream ss;
		ss << "  "
		   << R"(<g)"
		   << " "
		   << R"(stroke=")" << "Black" << R"(")"
		   << " "
		   << R"(stroke-width=")" << 1 << R"(")"
		   << R"(>)";
		svglines.push_back( ss.str());
	}
	for ( auto v : xgridpoints){

		double actuX = cam.getXActualFromTheoretical( v);

		double tickheight = 5; // とりあえずの値。

		stringstream ss;

		ss << "    "
		<< R"(<line)"
		<< " "
		<< R"(x1=")" << actuX << R"(")"
		<< " "
		<< R"(y1=")" << cam.actuYMax << R"(")"
		<< " "
		<< R"(x2=")" << actuX << R"(")"
		<< " "
		<< R"(y2=")" << ( cam.actuYMax + tickheight) << R"(")"
		<< " "
		<< R"(/>)";

		svglines.push_back( ss.str());

	}
	// <g>で属性一括指定：終了
	{
		svglines.push_back( "  </g>");
	}

	// TODO: 軸の単位の記載→優先順位は低い。

	// textタグで、IEやWordはdominant-baselineが効かないらしい。
	// （指定してもdominant-baseline="alphabetic"扱いになる。）

	// x軸の目盛のラベル
	double xlabelfontsize = 14; // とりあえずの値。
	// <g>で属性一括指定：開始
	{
		stringstream ss;
		ss << "  "
		   << R"(<g)"
		   << " "
		   << R"(font-family=")" << "Arial,san-serif" << R"(")"
		   << " "
		   << R"(font-size=")" << xlabelfontsize << R"(")"
		   << " "
		   << R"(text-anchor="middle")"
		   << " "
		   << R"(dominant-baseline="alphabetic")" // こうしないとIEやWordで崩れる。。
		   << R"(>)";
		svglines.push_back( ss.str());
	}
	for ( auto v : xgridpoints){

		Point theoP( v, 0); // 本当はy軸の座標は要らないのだが。。

		Point actuP = cam.getActualFromTheoretical( theoP);

		double ticklabelmargin = 10; // とりあえずの値。

		stringstream ss;

		ss << "    "
		<< R"(<text)"
		<< " "
		<< R"(x=")" << actuP.x << R"(")" // 左右方向に中央揃えをする前提で座標を指定。
		<< " "
		// 描画領域の下端からmarginだけ離す。
		// alphabeticの基線は、このフォントの場合、本当のフォント下端より20%上なので、その分をずらしている。
		<< R"(y=")" << ( std::round( cam.actuYMax + ticklabelmargin + xlabelfontsize * 0.8)) << R"(")"
		<< ">"
		<< v // 桁数はどうなるのか。。
		<< R"(</text>)";

		svglines.push_back( ss.str());

	}
	// <g>で属性一括指定：終了
	{
		svglines.push_back( "  </g>");
	}

	// y軸の目盛のヒゲ
	// <g>で属性一括指定：開始
	{
		stringstream ss;
		ss << "  "
		   << R"(<g)"
		   << " "
		   << R"(stroke=")" << "Black" << R"(")"
		   << " "
		   << R"(stroke-width=")" << 1 << R"(")"
		   << R"(>)";
		svglines.push_back( ss.str());
	}
	for ( auto v : ygridpoints){

		double actuY = cam.getYActualFromTheoretical( v);

/*		Point theoP( 0, v); // 本当はy軸の座標は要らないのだが。。

		Point actuP = cam.getActualFromTheoretical( theoP);
*/
		double tickwidth = 5; // とりあえずの値。

		stringstream ss;

		ss << "    "
		<< R"(<line)"
		<< " "
		<< R"(x1=")" << cam.actuXMin << R"(")"
		<< " "
		<< R"(y1=")" << actuY /*actuP.y*/ << R"(")"
		<< " "
		<< R"(x2=")" << ( cam.actuXMin - tickwidth) << R"(")"
		<< " "
		<< R"(y2=")" << actuY /*actuP.y*/ << R"(")"
		<< " "
		<< R"(/>)";

		svglines.push_back( ss.str());

	}
	// <g>で属性一括指定：終了
	{
		svglines.push_back( "  </g>");
	}

	// y軸の目盛ラベル
	/*
	文字列を回転させる方法を探った→svgtest04.svgとsvgtest05.svg
	　svgtest04.svgで2つの方法を試したが、もっとシンプルにしたかった。
	　svgtest05.svgで、transform属性を使えばよいことがわかった。
	*/
	// y軸の目盛のラベル
	double ylabelfontsize = 14; // とりあえずの値。
	// <g>で属性一括指定：開始
	{
		stringstream ss;
		ss << "  "
		   << R"(<g)"
		   << " "
		   << R"(font-family=")" << "Arial,san-serif" << R"(")"
		   << " "
		   << R"(font-size=")" << ylabelfontsize << R"(")"
		   << " "
		   << R"(text-anchor="middle")" // 文字列の左右方向の中心で位置決めする。
		   << " "
		   << R"(dominant-baseline="alphabetic")"
		   << R"(>)";
		svglines.push_back( ss.str());
	}
	for ( auto v : ygridpoints){

		double actuY = cam.getYActualFromTheoretical( v);

		double ticklabelmargin = 10; // とりあえずの値。

		// alphabetic基線に合わせるために20%ずらしている。
		double xplace = std::round( cam.actuXMin - ticklabelmargin - ylabelfontsize * 0.2);

		stringstream ss;

		ss << "    "
		<< R"(<text)"
		<< " "
		<< R"(x=")" << xplace << R"(")" // 描画領域の左端からmarginだけ離す。
		<< " "
		<< R"(y=")" << actuY << R"(")" // 上下方向に中央揃えをする前提で座標を指定。
		<< " "
		<< R"(transform="rotate(270 )" << xplace << " " << actuY << ")" << R"(")" // 回転の中心が各点で異なるので、一括指定できない。
		<< ">"
		<< v // 桁数はどうなるのか。。
		<< R"(</text>)";

		svglines.push_back( ss.str());

	}
	// <g>で属性一括指定：終了
	{
		svglines.push_back( "  </g>");
	}

	// Title

	// グラフタイトル
	string title = "Frequency from pclub03.cpp"s;
	{
		stringstream ss;
		double fontsize = std::floor( cam.actuWidth * 0.7 / title.size() * 2.0); // 描画領域の幅のうち、7割を占めるぐらいのサイズ
		if ( fontsize >= cam.actuYMin * 0.7){ // 余白の高さの70%より大きいのはダメ
			fontsize = cam.actuYMin * 0.7;
		}
		ss << "  "
		<< R"(<text)"
		<< " "
		<< R"(x=")" << ( std::round( cam.getActualMidX())) << R"(")" // 中央揃えをするので。
		<< " "
		<< R"(y=")" << ( std::round( cam.actuYMin * 0.9)) << R"(")" // 余白のうち10%浮かせる。
		<< " "
		<< R"(font-family=")" << "Arial,san-serif" << R"(")"
		<< " "
		<< R"(font-size=")" << fontsize << R"(")"
		<< " "
		<< R"(text-anchor="middle")"
		<< " "
		<< R"(dominant-baseline="text-after-edge")"
		<< ">"
		<< title
		<< R"(</text>)";

		svglines.push_back( ss.str());
	}

	// x軸タイトルを書く。
	string xaxislabel = "Household Income";
	{

		double fontsize = 20; // とりあえずの値。
		double xaxislabelmargin = 30; // とりあえずの値。

		stringstream ss;

		ss << "  "
		<< R"(<text)"
		<< " "
		<< R"(x=")" << ( std::round( cam.getActualMidX())) << R"(")" // 中央揃えをするので。
		<< " "
		<< R"(y=")" << ( std::round( cam.actuYMax + xaxislabelmargin + fontsize)) << R"(")" // 描画領域の下端からmarginだけ離す。
		<< " "
		<< R"(font-family=")" << "Arial,san-serif" << R"(")"
		<< " "
		<< R"(font-size=")" << fontsize << R"(")"
		<< " "
		<< R"(text-anchor="middle")"
		<< " "
		<< R"(dominant-baseline="text-after-edge")" // これでないとIEやWordで崩れる。
		<< ">"
		<< xaxislabel
		<< R"(</text>)";

		svglines.push_back( ss.str());

	}

	// y軸タイトルを書く。
	string yaxislabel = "#Cases";
	{

		double fontsize = 20; // とりあえずの値。
		double yaxislabelmargin = 30; // とりあえずの値。

		stringstream ss;
		double x = std::round( cam.actuXMin - yaxislabelmargin);
		double y = std::round( cam.getActualMidY());

		ss << "  "
		<< R"(<text)"
		<< " "
		<< R"(x=")" << x << R"(")" // 描画領域の左端からmarginだけ離す。
		<< " "
		<< R"(y=")" << y << R"(")" // 中央揃えをするので。
		<< " "
		<< R"(font-family=")" << "Arial,san-serif" << R"(")"
		<< " "
		<< R"(font-size=")" << fontsize << R"(")"
		<< " "
		<< R"(text-anchor="middle")"
		<< " "
		<< R"(dominant-baseline="text-after-edge")"
		<< " "
		<< R"(transform="rotate(270 )" << x << " " << y << ")" << R"(")"
		<< ">"
		<< yaxislabel
		<< R"(</text>)";

		svglines.push_back( ss.str());

	}

	// 周辺情報記載の終了

	// 枠線を描く。最後にすべき。
	// fillは透過させる。
	svglines.push_back( R"(  <rect x="50" y="50" width="400" height="400" fill-opacity="0" stroke="Black" stroke-width="1" />)");

	svglines.push_back( R"(</svg>)");

	string detector = "<!-- "
	                  u8"\u6587\u5B57\u30B3\u30FC\u30C9\u8B58\u5225\u7528" // 「文字コード識別用」というUTF-8文字列
	                  " -->";
	svglines.push_back( detector);

	koutputfile outsvg( fn);
	outsvg.open( false, false, true);
	outsvg.writeLines( svglines);
	outsvg.close();

}

// [min0, max0]に、いい感じの間隔で点をとる。
// k0個以上で最小の点を返す。
// newminがtrueのとき、得られた間隔に乗る新しいminも返す。
// newmaxがtrueのとき、得られた間隔に乗る新しいmaxも返す。
std::vector <double>
getGridPoints( double min0, double max0, int k0 /*= 4*/, bool newmin /*= true*/, bool newmax /*= true*/)
{

	using namespace std;

	vector <double> ret;

	// この数以上の最小の点を返すようにする。
	int minnpoints = k0;

	// error
	if ( min0 >= max0){
		alert( "getGripPoints()");
		return ret;
	}

	double max0ab = abs( max0);
	double min0ab = abs( min0);

	// max0abとmin0abのうち大きい方は何桁？（その値マイナス1）
	double digits_m1 = floor( log10( max( max0ab, min0ab)));

	// 基準となる10のべき乗値
	double base10val = pow( 10.0, digits_m1);

	// 候補となる、intervalの先頭の桁の値
	vector <double> headcands = { 5.0, 2.5, 2.0, 1.0};

	double interval;

	bool loop = true;
	while ( loop){

		for ( auto h : headcands){

			ret.clear();
			interval = base10val * h;

			// setting startpoint; to avoid startpoint being "-0", we do a little trick.
			double startpoint = ceil( min0 / interval);
			if ( startpoint > -1.0 && startpoint < 1.0){
				startpoint = 0.0;
			}
			startpoint *= interval;

			for ( double p = startpoint; p <= max0; p += interval){
				ret.push_back( p);
			}
			if ( ret.size() >= minnpoints){
				loop = false;
				break;
			}

		}

		base10val /= 10.0;

	}

	if ( newmin == true){
		double oldmin = ret.front();
		if ( oldmin == min0){
			// if the first point already obtained is equal to min0
			// do nothing
		} else {
			ret.insert( ret.begin(), oldmin - interval);
		}
	}

	if ( newmax == true){
		double oldmax = ret.back();
		if ( oldmax == max0){
			// if the last point already obtained is equal to max0
			// do nothing
		} else {
			ret.push_back( oldmax + interval);
		}
	}

	return ret;

}

/* ********** Definitions of Member Functions ********** */


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_HYPOT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_HYPOT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing hypot capabilities

    Computes \f$(x^2 + y^2)^{1/2}\f$

    @par semantic:
    For any given value @c x,  @c y of floating type @c T:

    @code
    T r = hypot(x, y);
    @endcode

    The code of the rgular version is very similar to:

    @code
    T r = sqrt(sqr(x)+sqr(y));
    @endcode

    @par Decorators

    - pedantic_ with this decorator provisions are made to avoid overflow as
    possible and to compute  @c hypot accurately in any cases.

    -std_ call std::hypot

  **/
  Value hypot(Option const& o, Value const& x, Value const& y);

  //@overload
  Value hypot(Value const& x, Value const& y);
} }
#endif

#include <boost/simd/function/scalar/hypot.hpp>
#include <boost/simd/function/scalar/hypot.hpp>
#include <boost/simd/function/simd/hypot.hpp>

#endif


/*
simplexSizeGenerator.cpp - This file is part of the Bayesembler (v1.1.1)

The MIT License (MIT)

Copyright (c) 2014 Lasse Maretty and Jonas Andreas Sibbesen

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.
*/

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/lexical_cast.hpp>
#include <simplexSizeGenerator.h>

typedef boost::random::gamma_distribution<> gamma_distribution_t;
typedef boost::random::variate_generator<boost::random::mt19937*, boost::random::gamma_distribution<> > gamma_sampler_t;
typedef boost::random::mt19937* mt_rng_pt_t;
typedef boost::random::uniform_01<boost::random::mt19937*> uniform_01_sampler_t;

int SimplexSizeGenerator::sampleSimplexSize(double pi, double gamma, int count_plus_size) {

	assert(count_plus_size <= num_transcripts);
	assert(pi >= double_underflow);
	assert(pi <= double_almost_one);
	assert(gamma > double_underflow);

	// Init binomial distribution over simplex sizes
	vector <double> simplex_prob_vector;
    simplex_prob_vector.reserve(num_transcripts - count_plus_size + 1);

	// Cardinality of equivalence class of size |s+| is one
	double cardinal_eq_z_log = 0;

	// Calculate probability of member of equivalence class
	double prob_z_log = count_plus_size*log(pi) + (num_transcripts-count_plus_size)*log(1-pi);

	// Probability of assignment given the binary vector
	double prob_t_log = boost::math::lgamma(count_plus_size*gamma) - boost::math::lgamma(num_fragments + count_plus_size*gamma);

	// Full probability of the binary vector
	double prob_eq_z_log = cardinal_eq_z_log + prob_z_log + prob_t_log;
	double row_sum = prob_eq_z_log;

	simplex_prob_vector.push_back(row_sum);

	for (int i = count_plus_size + 1; i < num_transcripts + 1; i++) {

		// Calculate cardinality of equivalence class
		cardinal_eq_z_log = boost::math::lgamma(num_transcripts-count_plus_size+1)-(boost::math::lgamma(i-count_plus_size+1)+boost::math::lgamma(num_transcripts - i + 1));

		// Calculate probability of member of equivalence class
		prob_z_log = i*log(pi) + (num_transcripts-i)*log(1-pi);

		// Probability of assignment given the binary vector
		prob_t_log = boost::math::lgamma(i*gamma) - boost::math::lgamma(num_fragments + i*gamma);

		// Full probability of the binary vector
		prob_eq_z_log = cardinal_eq_z_log + prob_z_log + prob_t_log;

		row_sum += log(1 + exp(prob_eq_z_log - row_sum));
		simplex_prob_vector.push_back(row_sum);

    	if (double_compare(simplex_prob_vector.back(), *(simplex_prob_vector.rbegin() + 1))) {

            break;
        }
	}

	// Row-normalise and transform back from log-space
	for (int i = 0; i < simplex_prob_vector.size(); i++) {

	    simplex_prob_vector[i] = exp(simplex_prob_vector[i] - row_sum);

	}

    assert (simplex_prob_vector.back() > double_almost_one);

	uniform_01_sampler_t sample_uniform_01(mt_rng_pt);

	int b = int(upper_bound(simplex_prob_vector.begin(), simplex_prob_vector.end(), sample_uniform_01()) - simplex_prob_vector.begin()) + count_plus_size;

	return b;
}

bool SimplexSizeGenerator::isFixed() {

	return is_fixed;

}

/* SIMPLEX-SIZE GENERATOR CLASS */
FixedBinomialFixedGammaSimplexSizeGenerator::FixedBinomialFixedGammaSimplexSizeGenerator(double pi_in, double gamma_in, int num_transcripts_in, mt_rng_pt_t mt_rng_pt_in, int num_fragments_in) {

    // Set members
    pi = pi_in;
    gamma = gamma_in;
    num_transcripts = num_transcripts_in;
    is_fixed = true;
    mt_rng_pt = mt_rng_pt_in;
    num_fragments = num_fragments_in;

    simplex_prob_matrix = vector<vector<double> >(num_transcripts, vector<double>());

    // Loop over count plus sizes
    for (int i=0; i < num_transcripts; i++) {

        int count_plus_size = i + 1;

    	// Init binomial distribution over simplex sizes
    	vector < double> simplex_prob_vector;

    	// Cardinality of equivalence class of size |s+| is zero
    	double cardinal_eq_z_log = 0;

    	// Calculate probability of member of equivalence class
    	double prob_z_log = count_plus_size*log(pi) + (num_transcripts-count_plus_size)*log(1-pi);

    	// Probability of assignment given the binary vector
    	double prob_t_log = boost::math::lgamma(count_plus_size*gamma) - boost::math::lgamma(num_fragments + count_plus_size*gamma);

    	// Full probability of the binary vector
    	double prob_eq_z_log = cardinal_eq_z_log + prob_z_log + prob_t_log;
    	double row_sum = prob_eq_z_log;

    	simplex_prob_vector.push_back(row_sum);

    	for (int j = count_plus_size + 1; j < num_transcripts + 1; j++) {

    		// Calculate cardinality of equivalence class
    		cardinal_eq_z_log = boost::math::lgamma(num_transcripts-count_plus_size+1)-(boost::math::lgamma(j-count_plus_size+1)+boost::math::lgamma(num_transcripts - j + 1));

    		// Calculate probability of member of equivalence class
    		prob_z_log = j*log(pi) + (num_transcripts-j)*log(1-pi);

    		// Probability of assignment given the binary vector
    		prob_t_log = boost::math::lgamma(j*gamma) - boost::math::lgamma(num_fragments + j*gamma);

    		// Full probability of the binary std::vector<char> v;
    		prob_eq_z_log = cardinal_eq_z_log + prob_z_log + prob_t_log;

            row_sum += log(1 + exp(prob_eq_z_log - row_sum));
    		simplex_prob_vector.push_back(row_sum);

            if (double_compare(simplex_prob_vector.back(), *(simplex_prob_vector.rbegin() + 1))) {

                break;
            }
    	}

    	// Row-normalise and transform back from log-space
    	for (int j = 0; j < simplex_prob_vector.size(); j++) {

    	    simplex_prob_vector[j] = exp(simplex_prob_vector[j] - row_sum);

    	}

        assert (simplex_prob_vector.back() > double_almost_one);

        simplex_prob_matrix[i] = simplex_prob_vector;

    }
}

string FixedBinomialFixedGammaSimplexSizeGenerator::getParameterString(){

    string parameter_str;
    parameter_str += "pi";
    parameter_str += boost::lexical_cast<string>(pi);
    parameter_str += "gamma";
    parameter_str += boost::lexical_cast<string>(gamma);

    return parameter_str;
}

// Initialise simplex probability matrix
pair<int, double> FixedBinomialFixedGammaSimplexSizeGenerator::initSimplexSize(int count_plus_size, double gamma) {

    int b = sampleSimplexSize(pi, gamma, count_plus_size);

    return pair<int, double>(b, pi);
}

// Samples a simplex size from the simplex size probability matrix
pair<int, double> FixedBinomialFixedGammaSimplexSizeGenerator::generateSimplexSize(int expression_plus_size, int count_plus_size, double gamma) {

    int b = sampleSimplexSize(pi, gamma, count_plus_size);

    return pair<int, double>(b, pi);
}

int FixedBinomialFixedGammaSimplexSizeGenerator::sampleSimplexSize(double pi, double gamma, int count_plus_size) {

    uniform_01_sampler_t sample_uniform_01(mt_rng_pt);

    int b = int(upper_bound(simplex_prob_matrix[count_plus_size-1].begin(), simplex_prob_matrix[count_plus_size-1].end(), sample_uniform_01()) - simplex_prob_matrix[count_plus_size-1].begin()) + count_plus_size;

    return b;

}

FixedBinomialSimplexSizeGenerator::FixedBinomialSimplexSizeGenerator(double pi_in, int num_transcripts_in, mt_rng_pt_t mt_rng_pt_in, int num_fragments_in) {

	// Set members
	pi = pi_in;
	num_transcripts = num_transcripts_in;
	is_fixed = true;
	mt_rng_pt = mt_rng_pt_in;
	num_fragments = num_fragments_in;
}

string FixedBinomialSimplexSizeGenerator::getParameterString(){

    string parameter_str;
	parameter_str += "pi";
    parameter_str += boost::lexical_cast<string>(pi);

    return parameter_str;
}

// Initialise simplex probability matrix
pair<int, double> FixedBinomialSimplexSizeGenerator::initSimplexSize(int count_plus_size, double gamma) {

	int b = sampleSimplexSize(pi, gamma, count_plus_size);

	return pair<int, double>(b, pi);
}

// Samples a simplex size from the simplex size probability matrix
pair<int, double> FixedBinomialSimplexSizeGenerator::generateSimplexSize(int expression_plus_size, int count_plus_size, double gamma) {

	int b = sampleSimplexSize(pi, gamma, count_plus_size);

	return pair<int, double>(b, pi);
}

BetaBinomialSimplexSizeGenerator::BetaBinomialSimplexSizeGenerator(double alpha_in, double beta_in, int num_transcripts_in, mt_rng_pt_t mt_rng_pt_in, int num_fragments_in, int slice_iterations_in, double slice_window_size_in) {

	alpha = alpha_in;
	beta = beta_in;
	num_transcripts = num_transcripts_in;
	is_fixed = false;
	mt_rng_pt = mt_rng_pt_in;
	num_fragments = num_fragments_in;
	slice_iterations = slice_iterations_in;
    slice_window_size = slice_window_size_in;

}

string BetaBinomialSimplexSizeGenerator::getParameterString(){

	string parameter_str;
	parameter_str += "alpha";
    parameter_str += boost::lexical_cast<string>(alpha);
	parameter_str += "_";
	parameter_str += "beta";
    parameter_str += boost::lexical_cast<string>(beta);

    return parameter_str;
}

pair<int, double> BetaBinomialSimplexSizeGenerator::initSimplexSize(int count_plus_size, double gamma) {

	// Sample pi from beta-prior
	gamma_distribution_t gamma_dist_alpha(alpha,1);
	gamma_distribution_t gamma_dist_beta(beta,1);

	gamma_sampler_t sample_gamma_alpha(mt_rng_pt, gamma_dist_alpha);
	gamma_sampler_t sample_gamma_beta(mt_rng_pt, gamma_dist_beta);

	double sample_alpha = sample_gamma_alpha();
	double sample_beta = sample_gamma_beta();

	double pi = sample_alpha / (sample_alpha + sample_beta);

	if (pi > double_almost_one) {

		pi = double_almost_one;
	}

	if (pi < double_precision) {

		pi = double_precision;
	}

	int b = sampleSimplexSize(pi, gamma, count_plus_size);

	return pair<int, double>(b, pi);

}

pair<int, double> BetaBinomialSimplexSizeGenerator::generateSimplexSize(int expression_plus_size, int count_plus_size, double gamma) {

	double pi = samplePi(expression_plus_size);

	if (pi > double_almost_one) {

		pi = double_almost_one;
	}

	if (pi < double_precision) {

		pi = double_precision;
	}

	int b = sampleSimplexSize(pi, gamma, count_plus_size);

	return pair<int, double>(b, pi);
}

double BetaBinomialSimplexSizeGenerator::posteriorPiLogDensity(int expression_plus_size, double pi) {

	return ((alpha + expression_plus_size - 1)*log(pi) + (beta + num_transcripts - expression_plus_size - 1)*log(1-pi) - log(1 - exp(num_transcripts*log(1-pi))));

}

double BetaBinomialSimplexSizeGenerator::samplePi(int expression_plus_size) {

	// Init gamma and uniform sampler
	uniform_01_sampler_t sample_uniform_01(mt_rng_pt);

	double pi_current = sample_uniform_01();
	double pi = pi_current;

	if (pi_current < double_precision) {

		pi_current = double_precision;

	}

	if (pi_current > double_almost_one) {

		pi_current = double_almost_one;

	}

	// Output all samples for convergence assessment
	// ofstream pi_slice_out("pi_slice_out.txt", ios::app);

	for (int i=0; i < slice_iterations; i++) {

		// Sample height
		double y = posteriorPiLogDensity(expression_plus_size, pi_current) + log(1-sample_uniform_01());

		// Find slice by "step-out"
		double left = pi_current - sample_uniform_01() * slice_window_size;
		double right = left + slice_window_size;

		int j = 1;
		int k = 1;

		// Truncate distribution at zero
		if (left < double_precision) {
			left = double_precision;
			j = 0;
		}

		// Truncate distribution at one
		if (right > double_almost_one) {
			right = double_almost_one;
			k = 0;
		}

		// Expand window to the left
		while (j == 1 && y < (posteriorPiLogDensity(expression_plus_size, left))) {
			left = left - slice_window_size;

			if (left < double_precision) {
				left = double_precision;
				break;
			}
		}

		// Expand window to the right
		while (k == 1 && y < (posteriorPiLogDensity(expression_plus_size, right))) {
			right = right + slice_window_size;

			if (right > double_almost_one) {
				right = double_almost_one;
				break;
			}
		}

		// Sample from the window and step-in window boundaries until in slice
		pi = sample_uniform_01()*(right-left) + left;

		while ( y >= (posteriorPiLogDensity(expression_plus_size, pi))) {

            if (pi < pi_current) {

				left = pi;
				pi = sample_uniform_01()*(right-left) + left;

			} else {

				right = pi;
				pi = sample_uniform_01()*(right-left) + left;
			}
		}

		pi_current = pi;
		// pi_slice_out << pi << endl;
	}

	// pi_slice_out.close();

	return pi;
}


#include <sequential-line-search/gaussianprocessregressor.h>
#include <sequential-line-search/utils.h>
#include <iostream>
#include <cmath>
#include <Eigen/LU>
#include <nlopt-util.hpp>

//#define NOISELESS

using Eigen::MatrixXd;
using Eigen::VectorXd;

namespace
{
    using namespace sequential_line_search;

    const bool   useLogNormalPrior     = true;
    const double a_prior_mu            = std::log(0.500);
    const double a_prior_sigma_squared = 0.10;
#ifdef NOISELESS
    const double b_fixed               = 1e-06;
#else
    const double b_prior_mu            = std::log(0.001);
    const double b_prior_sigma_squared = 0.10;
#endif
    const double r_prior_mu            = std::log(0.500);
    const double r_prior_sigma_squared = 0.10;

    double calc_grad_a_prior(const double a)
    {
        return (a_prior_mu - a_prior_sigma_squared - std::log(a)) / (a_prior_sigma_squared * a);
    }

#ifndef NOISELESS
    double calc_grad_b_prior(const double b)
    {
        return (b_prior_mu - b_prior_sigma_squared - std::log(b)) / (b_prior_sigma_squared * b);
    }
#endif

    double calc_grad_r_i_prior(const Eigen::VectorXd &r, const int index)
    {
        return (r_prior_mu - r_prior_sigma_squared - std::log(r(index))) / (r_prior_sigma_squared * r(index));
    }

    double calc_a_prior(const double a)
    {
        return std::log(utils::log_normal(a, a_prior_mu, a_prior_sigma_squared));
    }

#ifndef NOISELESS
    double calc_b_prior(const double b)
    {
        return std::log(utils::log_normal(b, b_prior_mu, b_prior_sigma_squared));
    }
#endif

    double calc_r_i_prior(const Eigen::VectorXd &r, const int index)
    {
        return std::log(utils::log_normal(r(index), r_prior_mu, r_prior_sigma_squared));
    }

    double calc_grad_a(const MatrixXd& X, const MatrixXd& C_inv, const VectorXd& y, const double a, const double b, const VectorXd& r)
    {
        const MatrixXd C_grad_a = Regressor::calc_C_grad_a(X, a, b, r);
        const double term1 = + 0.5 * y.transpose() * C_inv * C_grad_a * C_inv * y;
        const double term2 = - 0.5 * (C_inv * C_grad_a).trace();
        return term1 + term2 + (useLogNormalPrior ? calc_grad_a_prior(a) : 0.0);
    }

#ifndef NOISELESS
    double calc_grad_b(const MatrixXd& X, const MatrixXd& C_inv, const VectorXd& y, const double a, const double b, const VectorXd& r)
    {
        const MatrixXd C_grad_b = Regressor::calc_C_grad_b(X, a, b, r);
        const double term1 = + 0.5 * y.transpose() * C_inv * C_grad_b * C_inv * y;
        const double term2 = - 0.5 * (C_inv * C_grad_b).trace();
        return term1 + term2 + (useLogNormalPrior ? calc_grad_b_prior(b) : 0.0);
    }
#endif

    double calc_grad_r_i(const MatrixXd& X, const MatrixXd& C_inv, const VectorXd& y, const double a, const double b, const VectorXd& r, const int index)
    {
        const MatrixXd C_grad_r_i = Regressor::calc_C_grad_r_i(X, a, b, r, index);
        const double term1 = + 0.5 * y.transpose() * C_inv * C_grad_r_i * C_inv * y;
        const double term2 = - 0.5 * (C_inv * C_grad_r_i).trace();
        return term1 + term2 + (useLogNormalPrior ? calc_grad_r_i_prior(r, index) : 0.0);
    }

    VectorXd calc_grad(const MatrixXd& X, const MatrixXd& C_inv, const VectorXd& y, const double a, const double b, const VectorXd& r)
    {
        const unsigned D = X.rows();

        VectorXd grad(D + 2);
        grad(0) = calc_grad_a(X, C_inv, y, a, b, r);
#ifdef NOISELESS
        grad(1) = 0.0;
#else
        grad(1) = calc_grad_b(X, C_inv, y, a, b, r);
#endif

        for (unsigned i = 2; i < D + 2; ++ i)
        {
            const unsigned index = i - 2;
            grad(i) = calc_grad_r_i(X, C_inv, y, a, b, r, index);
        }

        return grad;
    }

    struct Data
    {
        const MatrixXd X;
        const VectorXd y;
    };

    // For counting the number of function evaluations
    unsigned count;

    // Log likelihood that will be maximized
    double objective(const std::vector<double> &x, std::vector<double>& grad, void* data)
    {
        // For counting the number of function evaluations
        ++ count;

        const MatrixXd& X = static_cast<const Data*>(data)->X;
        const VectorXd& y = static_cast<const Data*>(data)->y;

        const unsigned N = X.cols();

        const double   a = x[0];
#ifdef NOISELESS
        const double   b = b_fixed;
#else
        const double   b = x[1];
#endif
        const VectorXd r = Eigen::Map<const VectorXd>(&x[2], x.size() - 2);

        const MatrixXd C     = Regressor::calc_C(X, a, b, r);
        const MatrixXd C_inv = C.inverse();

        // When the algorithm is gradient-based, compute the gradient vector
        if (grad.size() == x.size())
        {
            const VectorXd g = calc_grad(X, C_inv, y, a, b, r);
            for (unsigned i = 0; i < g.rows(); ++ i) grad[i] = g(i);
        }

        const double term1 = - 0.5 * y.transpose() * C_inv * y;
        const double term2 = - 0.5 * std::log(C.determinant());
        const double term3 = - 0.5 * N * std::log(2.0 * M_PI);

        // Computing the regularization terms from a prior assumptions
        const double a_prior = calc_a_prior(a);
#ifdef NOISELESS
        const double b_prior = 1.0;
#else
        const double b_prior = calc_b_prior(b);
#endif
        const double r_prior = [&r]()
        {
            double sum = 0.0;
            for (unsigned i = 0; i < r.rows(); ++ i) sum += calc_r_i_prior(r, i);
            return sum;
        }();
        const double regularization = useLogNormalPrior ? (a_prior + b_prior + r_prior) : 0.0;

        return term1 + term2 + term3 + regularization;
    }
}

namespace sequential_line_search
{
    GaussianProcessRegressor::GaussianProcessRegressor(const MatrixXd& X, const VectorXd& y)
    {
        this->X = X;
        this->y = y;

        if (X.rows() == 0) return;

        compute_MAP();

        C     = calc_C(X, a, b, r);
        C_inv = C.inverse();
    }

    GaussianProcessRegressor::GaussianProcessRegressor(const Eigen::MatrixXd &X, const Eigen::VectorXd &y, double a, double b, const Eigen::VectorXd &r)
    {
        this->X = X;
        this->y = y;
        this->a = a;
        this->b = b;
        this->r = r;

        C     = calc_C(X, a, b, r);
        C_inv = C.inverse();
    }

    double GaussianProcessRegressor::estimate_y(const VectorXd &x) const
    {
        const VectorXd k = calc_k(x, X, a, b, r);
        return k.transpose() * C_inv * y;
    }

    double GaussianProcessRegressor::estimate_s(const VectorXd &x) const
    {
        const VectorXd k = calc_k(x, X, a, b, r);
        return std::sqrt(a + b - k.transpose() * C_inv * k);
    }

    void GaussianProcessRegressor::compute_MAP()
    {
        const unsigned D = X.rows();

        Data data{ X, y };

        const VectorXd x_ini = VectorXd::Constant(D + 2, 1e+00);
        const VectorXd upper = VectorXd::Constant(D + 2, 5e+01);
        const VectorXd lower = VectorXd::Constant(D + 2, 1e-08);

        const VectorXd x_glo = nloptutil::solve(x_ini, upper, lower, objective, nlopt::GN_DIRECT, &data, 300);
        const VectorXd x_loc = nloptutil::solve(x_glo, upper, lower, objective, nlopt::LD_TNEWTON, &data, 1000);

        a = x_loc(0);
        b = x_loc(1);
        r = x_loc.block(2, 0, D, 1);

#ifdef NOISELESS
        b = b_fixed;
#endif
    }
}


#ifndef __LM_H__
#define __LM_H__

#include <armadillo>

#include <glm/glm_info.hpp>
#include <glm/models/glm_model.hpp>

/**
 * Computes the log-likelihood of the linear model.
 *
 * @param residuals The residual vector, zero if missing.
 * @param sigma_square Estimate of the variance.
 * @param n The number of non-missing individuals.
 *
 * @return The log-likelihood of the linear model.
 */
double loglikelihood(const arma::vec &residuals, double sigma_square, double n);

/**
 * This function solves the linear least squares problem.
 *
 * @param X The design matrix (caller is responsible for
 *          adding an intercept).
 * @param y The observations.
 * @param missing Identifies missing sampels by 1 and non-missing by 0.
 * @param output Output statistics of the estimated betas.
 *
 * @return Estimated beta coefficients.
 */
arma::vec lm(const arma::mat &X, const arma::vec &y, const arma::uvec &missing, const glm_model &model, glm_info &output);

#endif /* End of __LM_H__ */


/* @file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include <boost/test/unit_test.hpp>

#include "Molassembler/Shapes/CoordinateSystemTransformation.h"

BOOST_AUTO_TEST_CASE(CoordinateSystemTransformation, *boost::unit_test::label("Shapes")) {
  using namespace Scine::Molassembler::Shapes;
  for(unsigned i = 0; i < 20; ++i) {
    // Create two random coordinate systems
    const CoordinateSystem a = CoordinateSystem::random();
    const CoordinateSystem b = CoordinateSystem::random();

    const auto rot = rotationMatrix(a, b);

    BOOST_CHECK((rot * a.x).isApprox(b.x, 1e-10));
    BOOST_CHECK((rot * a.y).isApprox(b.y, 1e-10));
    BOOST_CHECK((rot * a.z).isApprox(b.z, 1e-10));
  }
}


#include <iostream>
#include <functional>
#include <numeric>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <map>
#include <Eigen\dense>

#include "markov.h"
#include "TransitionMatrix.h"

int main(){

	SetTransitionMatrix();

	//Output Vector
	v.setZero();
	v(0) = 1.0;

	// Print Results to File
	std::ofstream myfile;
	myfile.open("markov_results.txt");

   // TODO add Markov vector - Matrix multiplication

	std::cout <<  v << std::endl;
	//myfile << v << std::endl;  //this is just a sample, becareful how you print to file so you can mine useful stats

	myfile.close();

  return 1;
}


/**
 * \file LinkwitzRileyFilter.cpp
 */

#include <boost/math/tools/polynomial.hpp>

#include "LinkwitzRileyFilter.h"
#include "IIRFilter.h"

namespace ATK
{
  template<typename DataType>
  LinkwitzRileyLowPassCoefficients<DataType>::LinkwitzRileyLowPassCoefficients(int nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void LinkwitzRileyLowPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    DataType omega = boost::math::constants::pi<DataType>() * cut_frequency;
    DataType kappa = omega / std::tan(omega / input_sampling_rate);
    DataType delta = kappa * kappa + omega * omega + 2 * kappa * omega;

    coefficients_in[2] = omega * omega / delta;
    coefficients_in[1] = 2 * omega * omega / delta;
    coefficients_in[0] = omega * omega / delta;
    coefficients_out[1] = 2 * (kappa * kappa - omega * omega) / delta;
    coefficients_out[0] = -(omega * omega + kappa * kappa - 2 * kappa * omega) / delta;
  }

  template<typename DataType>
  LinkwitzRileyHighPassCoefficients<DataType>::LinkwitzRileyHighPassCoefficients(int nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void LinkwitzRileyHighPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    DataType omega = boost::math::constants::pi<DataType>() * cut_frequency;
    DataType kappa = omega / std::tan(omega / input_sampling_rate);
    DataType delta = kappa * kappa + omega * omega + 2 * kappa * omega;

    coefficients_in[2] = kappa * kappa / delta;
    coefficients_in[1] = - 2 * kappa * kappa / delta;
    coefficients_in[0] = kappa * kappa / delta;
    coefficients_out[1] = 2 * (kappa * kappa - omega * omega) / delta;
    coefficients_out[0] = -(omega * omega + kappa * kappa - 2 * kappa * omega) / delta;
  }

  template class LinkwitzRileyLowPassCoefficients<float>;
  template class LinkwitzRileyLowPassCoefficients<double>;
  template class LinkwitzRileyHighPassCoefficients<float>;
  template class LinkwitzRileyHighPassCoefficients<double>;

  template class IIRFilter<LinkwitzRileyLowPassCoefficients<float> >;
  template class IIRFilter<LinkwitzRileyLowPassCoefficients<double> >;
  template class IIRFilter<LinkwitzRileyHighPassCoefficients<float> >;
  template class IIRFilter<LinkwitzRileyHighPassCoefficients<double> >;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EULER_FUNCTIONS_SCALAR_GAMMA_HPP_INCLUDED
#define NT2_EULER_FUNCTIONS_SCALAR_GAMMA_HPP_INCLUDED
#include <nt2/euler/functions/gamma.hpp>
#include <nt2/euler/functions/details/gamma_kernel.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/three.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/copysign.hpp>
#include <nt2/include/functions/scalar/floor.hpp>
#include <nt2/include/functions/scalar/is_eqz.hpp>
#include <nt2/include/functions/scalar/is_even.hpp>
#include <nt2/include/functions/scalar/is_ltz.hpp>
#include <nt2/include/functions/scalar/sinpi.hpp>
#include <nt2/include/functions/scalar/stirling.hpp>

#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/functions/scalar/is_nan.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( gamma_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      if (is_eqz(a0)) return copysign(Inf<A0>(), a0);
      #ifndef BOOST_SIMD_NO_INVALIDS
      if( nt2::is_nan(a0) || (a0 == nt2::Minf<A0>()) ) return nt2::Nan<A0>();
      if (a0 == nt2::Inf<A0>()) return a0;
      #endif

      A0 x = a0;
      A0 q = nt2::abs(x);
      if(x < A0(-33.0))
      {
        A0 st = nt2::stirling(q);
        A0 p =  nt2::floor(q);
        bool iseven =  nt2::is_even((int32_t)p);
        if (p == q) return nt2::Nan<A0>();
        A0 z = q - p;
        if( z > nt2::Half<A0>() )
        {
          p += nt2::One<A0>();
          z = q - p;
        }
        z = q*nt2::sinpi(z);
        if( nt2::is_eqz(z) ) return nt2::Nan<A0>();
        st = nt2::Pi<A0>()/(nt2::abs(z)*st);
        return iseven  ? -st : st;
      }
      A0 z = nt2::One<A0>();
      while( x >= nt2::Three<A0>() )
      {
        x -= nt2::One<A0>();
        z *= x;
      }
      while( nt2::is_ltz(x) )
      {
        z /= x;
        x += nt2::One<A0>();
      }
      while( x < nt2::Two<A0>() )
      {
        if( nt2::is_eqz(x)) return nt2::Nan<A0>();
        z /= x;
        x +=  nt2::One<A0>();
      }
      if( x == nt2::Two<A0>() ) return(z);
      x -= nt2::Two<A0>();
      return z*details::gamma_kernel<A0>::gamma1(x);
    }
  };
} }

#endif