#include "odom/LaserMatcher.h"
#include "common/feature_utils.h"
#include "common/math_utils.h"
#include "common/nanoflann_pcl.h"
#include "common/ros_utils.h"
#include "common/transform_utils.h"

#include <Eigen/Eigenvalues>
#include <Eigen/QR>

namespace lidar_slam {

using std::sqrt;
using std::fabs;
using std::asin;
using std::atan2;
using std::pow;

LaserMatcher::LaserMatcher()
    : _filesDirectory("~"), _sendRegisteredCloud(true),
      _sendSurroundCloud(true), _useFullCloud(true), _inputFrameSkip(1),
      _surroundMapPubSkip(20), _inputFrameCount(0), _surroundMapPubCount(0),
      _dynamic_feature_map(), _laserCloudCornerLast(new CloudI()), // 221 211 221 || 110 105 110
      _laserCloudSurfLast(new CloudI()), _laserCloudFullRes(new CloudI()),
      _laserCloudCornerStack(new CloudI()), _laserCloudSurfStack(new CloudI()),
      _laserCloudCornerStackDS(new CloudI()),
      _laserCloudSurfStackDS(new CloudI()),
      _laserCloudCornerFromMap(new CloudI()),
      _laserCloudSurfFromMap(new CloudI()) {

  _lidarOdomNew = _lidarOdomLast = _lidarOdomLastMerged = _lidarMappedNew =
      _lidarMappedLast = _lidarPoseLast =Eigen::Isometry3f::Identity();

  cloudReceiveCount = 0;
  map_frame = "map";

}

LaserMatcher::~LaserMatcher() {
  ROS_INFO("[LaserMatcher] _inputFrameCount:%ld", _inputFrameCount);
  ROS_INFO("[LaserMatcher] cloudReceiveCount:%ld", cloudReceiveCount);
}

bool LaserMatcher::setup(ros::NodeHandle &node, ros::NodeHandle &privateNode) {

  imu_que.setup(node, privateNode);

  // fetch laser mapping params
  float fParam;
  int iParam;

  if (privateNode.getParam("filesDirectory", _filesDirectory)) {
    ROS_INFO("Set filesDirectory: %s", _filesDirectory.c_str());
  }

  if ( privateNode.getParam("inputFrameSkip", _inputFrameSkip)) {
    ROS_INFO("Set inputFrameSkip: %d", _inputFrameSkip);
  }

  if (privateNode.getParam("surroundMapPubSkip", _surroundMapPubSkip)) {
    ROS_INFO("Set surroundMapPubSkip: %d", _surroundMapPubSkip);
  }

  if (privateNode.getParam("sendRegisteredCloud", _sendRegisteredCloud)) {
    ROS_INFO("Set sendRegisteredCloud: %d", _sendRegisteredCloud);
  }

  if (privateNode.getParam("sendSurroundCloud", _sendSurroundCloud)) {
    ROS_INFO("Set sendSurroundCloud: %d", _sendSurroundCloud);
  }

  if(privateNode.getParam("dynamicMode", _dynamicMode))
  {
    ROS_INFO("Set dynamicMode: %s", _dynamicMode ? "true" : "false");
  }

  privateNode.getParam("useFullCloud", _useFullCloud);

  float filter_corner = 1.0;
  float filter_surf = 1.0;
  privateNode.getParam("filter_corner", filter_corner);
  privateNode.getParam("filter_surf", filter_surf);
  _downSizeFilterCorner.setLeafSize(filter_corner, filter_corner, filter_corner);
  _downSizeFilterSurf.setLeafSize(filter_surf, filter_surf, filter_surf);

  float map_filter_corner = 1.0;
  float map_filter_surf = 1.0;
  float map_filter = 2.0;
  privateNode.getParam("map_filter_corner", map_filter_corner);
  privateNode.getParam("map_filter_surf", map_filter_surf);
  privateNode.getParam("map_filter", map_filter);

  _scan_match.setConvergeThreshold(0.1, 0.1);
  _scan_match.setUseCore(false);

  // initializa feature cloud
  if(_dynamicMode)
  {
    _dynamic_feature_map.setupLidarFov(16, 7);
    _dynamic_feature_map.setupFilesDirectory(_filesDirectory);
    _dynamic_feature_map.setupFilterSize(map_filter_corner, map_filter_surf, map_filter);
  }
  else{
    int map_cube_x = 121;
    int map_cube_y = 121;
    int map_cube_z = 11;
    privateNode.getParam("map_cube_x", map_cube_x);
    privateNode.getParam("map_cube_y", map_cube_y);
    privateNode.getParam("map_cube_z", map_cube_z);
    _feature_map.reset(new FeatureMap<PointI>(map_cube_x, map_cube_y, map_cube_z));

    _feature_map->setupFilesDirectory(_filesDirectory);
    _feature_map->setupFilterSize(map_filter_corner, map_filter_surf, map_filter);
    _feature_map->loadCloudFromFiles();
  }

  _odomAftMapped.header.frame_id = map_frame;
  _odomAftMapped.child_frame_id = "/aft_mapped";
  _aftMappedTrans.frame_id_ = map_frame;
  _aftMappedTrans.child_frame_id_ = "/aft_mapped";
  _odomTransShift.frame_id_ = map_frame;
  _odomTransShift.child_frame_id_ = "/lidar_odom_init";
  // advertise laser mapping topics
  _pubFullMap = node.advertise<sensor_msgs::PointCloud2>("/FullMap", 1);
  _pubLaserCloudSurroundCorner = node.advertise<sensor_msgs::PointCloud2>(
      "/laser_cloud_surround_corner", 1);

  _pubLaserCloudSurroundSurf =
      node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surround_surf", 1);

  _pubCloudCornerLast2 =
      node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last2", 1);

  _pubCloudSurfLast2 =
      node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last2", 1);

  _pubLaserCloudFullRes =
      node.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_4", 1);

  _pubOdomAftMapped =
      node.advertise<nav_msgs::Odometry>("/aft_mapped_to_init", 1);

  _pubLidarPoseMerged = node.advertise<nav_msgs::Odometry>("/lidar_to_map2", 5);

  _subLaserCloudCornerLast = node.subscribe<sensor_msgs::PointCloud2>(
      "/laser_cloud_corner_last", 2, &LaserMatcher::laserCloudCornerLastHandler,
      this);

  _subLaserCloudSurfLast = node.subscribe<sensor_msgs::PointCloud2>(
      "/laser_cloud_surf_last", 2, &LaserMatcher::laserCloudSurfLastHandler,
      this);

  _subLaserOdometry = node.subscribe<nav_msgs::Odometry>(
      "/laser_odom_to_init", 5, &LaserMatcher::laserOdometryHandler, this);

  if (_useFullCloud) {
    _subLaserCloudFullRes = node.subscribe<sensor_msgs::PointCloud2>(
        "/velodyne_cloud_3", 2, &LaserMatcher::laserCloudFullResHandler, this);
  }

  _fullMapPubSrv = privateNode.advertiseService(
      "pubFullMap", &LaserMatcher::pubFullMap, this);
  _mapSaveSrv =
      privateNode.advertiseService("saveMap", &LaserMatcher::saveMap, this);

  return true;
}

bool LaserMatcher::pubFullMap(std_srvs::Empty::Request &req,
                              std_srvs::Empty::Response &resp) {
  CloudI::Ptr full_map(new CloudI());
  if(_dynamicMode)
    _dynamic_feature_map.getFullMap(full_map);
  else
    _feature_map->getFullMap(full_map);
  publishCloudMsg(_pubFullMap, *full_map, ros::Time::now(), map_frame);
  return true;
}

bool LaserMatcher::saveMap(std_srvs::Empty::Request &req,
                           std_srvs::Empty::Response &resp) {
  if(!_dynamicMode){
    return _feature_map->saveCloudToFiles();
  }
}

void LaserMatcher::laserCloudCornerLastHandler(
    const sensor_msgs::PointCloud2ConstPtr &cornerPointsLastMsg) {
  _timeLaserCloudCornerLast = cornerPointsLastMsg->header.stamp;

  _laserCloudCornerLast->clear();
  pcl::fromROSMsg(*cornerPointsLastMsg, *_laserCloudCornerLast);

  _newLaserCloudCornerLast = true;
}

void LaserMatcher::laserCloudSurfLastHandler(
    const sensor_msgs::PointCloud2ConstPtr &surfacePointsLastMsg) {
  _timeLaserCloudSurfLast = surfacePointsLastMsg->header.stamp;

  _laserCloudSurfLast->clear();
  pcl::fromROSMsg(*surfacePointsLastMsg, *_laserCloudSurfLast);

  _newLaserCloudSurfLast = true;
}

void LaserMatcher::laserCloudFullResHandler(
    const sensor_msgs::PointCloud2ConstPtr &laserCloudFullResMsg) {
  _timeLaserCloudFullRes = laserCloudFullResMsg->header.stamp;

  _laserCloudFullRes->clear();
  pcl::fromROSMsg(*laserCloudFullResMsg, *_laserCloudFullRes);

  _newLaserCloudFullRes = true;
  cloudReceiveCount++;
}

void LaserMatcher::laserOdometryHandler(
    const nav_msgs::Odometry::ConstPtr &laserOdometry) {
  _timeLaserOdometry = laserOdometry->header.stamp;
  Eigen::Isometry3d is3d;
  Odom2Isometry(laserOdometry, is3d);
  _lidarOdomNew = is3d.cast<float>();
  _newLaserOdometry = true;

  mtx_transform.lock();

  Eigen::Isometry3f lidarPoseMerged;
  transformAssociate(_lidarOdomLast, _lidarOdomNew, _lidarMappedLast,
                     lidarPoseMerged);
  mtx_transform.unlock();

  tf::StampedTransform lidarTFMerged;
  lidarTFMerged.frame_id_ = map_frame;
  lidarTFMerged.child_frame_id_ = "/lidar";
  lidarTFMerged.stamp_ = laserOdometry->header.stamp;
  Isometry2TFtransform(lidarPoseMerged.cast<double>(), lidarTFMerged);
  _tfBroadcaster.sendTransform(lidarTFMerged);

  nav_msgs::Odometry lidarOdomMerged;
  Isometry2Odom(lidarPoseMerged.cast<double>(), lidarOdomMerged);
  lidarOdomMerged.header.stamp = laserOdometry->header.stamp;
  lidarOdomMerged.header.frame_id = map_frame;
  lidarOdomMerged.child_frame_id = "/lidar";
  Eigen::Matrix<float, Eigen::Dynamic, 1> VectorXt = imu_que.getMean();
  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> MatrixXt = imu_que.getCov();

  lidarOdomMerged.twist.twist.linear.x = _velocity(0);
  lidarOdomMerged.twist.twist.linear.y = _velocity(1);
  lidarOdomMerged.twist.twist.linear.z = _velocity(2);
  lidarOdomMerged.pose.covariance[0] = MatrixXt(0, 0);
  lidarOdomMerged.pose.covariance[7] = MatrixXt(1, 1);
  lidarOdomMerged.pose.covariance[14] = MatrixXt(2, 2);
  lidarOdomMerged.twist.covariance[0] = MatrixXt(3, 3);
  lidarOdomMerged.twist.covariance[7] = MatrixXt(4, 4);
  lidarOdomMerged.twist.covariance[14] = MatrixXt(5, 5);
  _pubLidarPoseMerged.publish(lidarOdomMerged);
}

void LaserMatcher::reset() {
  _newLaserCloudCornerLast = false;
  _newLaserCloudSurfLast = false;
  _newLaserCloudFullRes = false;
  _newLaserOdometry = false;
}

bool LaserMatcher::hasNewData() {
  if (!_useFullCloud) {
    return _newLaserCloudCornerLast && _newLaserCloudSurfLast &&
           _newLaserOdometry &&
           fabs((_timeLaserCloudCornerLast - _timeLaserOdometry).toSec()) <
               0.005 &&
           fabs((_timeLaserCloudSurfLast - _timeLaserOdometry).toSec()) < 0.005;
  } else {
    return _newLaserCloudCornerLast && _newLaserCloudSurfLast &&
           _newLaserCloudFullRes && _newLaserOdometry &&
           fabs((_timeLaserCloudCornerLast - _timeLaserOdometry).toSec()) <
               0.005 &&
           fabs((_timeLaserCloudSurfLast - _timeLaserOdometry).toSec()) <
               0.005 &&
           fabs((_timeLaserCloudFullRes - _timeLaserOdometry).toSec()) < 0.005;
  }
}

void LaserMatcher::prepareFeatureFrame() {

  _laserCloudCornerStack.swap(_laserCloudCornerLast);
  _laserCloudSurfStack.swap(_laserCloudSurfLast);

  // down sample feature stack clouds
  _laserCloudCornerStackDS->clear();
  _downSizeFilterCorner.setInputCloud(_laserCloudCornerStack);
  _downSizeFilterCorner.filter(*_laserCloudCornerStackDS);

  _laserCloudSurfStackDS->clear();
  _downSizeFilterSurf.setInputCloud(_laserCloudSurfStack);
  _downSizeFilterSurf.filter(*_laserCloudSurfStackDS);
}

void LaserMatcher::prepareFeatureSurround() {

  pcl::PointXYZI currentPos;
  currentPos.getVector3fMap() = _lidarMappedNew.translation();

  Eigen::Vector3d directionZNowD;

  if(_dynamicMode)
    _dynamic_feature_map.update(currentPos, directionZNowD);
  else
    _feature_map->update(currentPos);

  if(_dynamicMode)
    _dynamic_feature_map.getSurroundFeature(*_laserCloudCornerFromMap,
                                    *_laserCloudSurfFromMap);
  else
    _feature_map->getSurroundFeature(*_laserCloudCornerFromMap,
                                  *_laserCloudSurfFromMap);

  if(_laserCloudSurfFromMap->points.size()<10){
    ROS_WARN_STREAM("surround map points too few, currentPos:"<<currentPos);
  }
}

void LaserMatcher::optimizeTransform() {
  _scan_match.scanMatchScan(_laserCloudCornerFromMap, _laserCloudSurfFromMap,
                            _laserCloudCornerStackDS, _laserCloudSurfStackDS,
                            _lidarMappedNew);
}

void LaserMatcher::transformMerge() {
  mtx_transform.lock();
  transformAssociate(_lidarOdomLast, _lidarOdomNew, _lidarMappedLast,
                     _lidarMappedNew);
  _lidarOdomLastMerged = _lidarOdomNew;
  _timeLaserOdometryMerged =_timeLaserOdometry;
  mtx_transform.unlock();
}

void LaserMatcher::transformUpdate() {
  mtx_transform.lock();
  _lidarMappedLast = _lidarMappedNew;
  _lidarOdomLast = _lidarOdomLastMerged;
  mtx_transform.unlock();
}

void LaserMatcher::featureMapUpdate() {
    if(!_dynamicMode){
      _feature_map->addFeatureCloud(*_laserCloudCornerStackDS,
                                  *_laserCloudSurfStackDS, _lidarMappedNew);

    }
}

void LaserMatcher::publishResult() {

  //pub aft_map
  _odomAftMapped.header.stamp = _timeLaserOdometryMerged;
  Isometry2Odom(_lidarMappedNew.cast<double>(), _odomAftMapped);
  _pubOdomAftMapped.publish(_odomAftMapped);

  _aftMappedTrans.stamp_ = _timeLaserOdometryMerged;
  Isometry2TFtransform(_lidarMappedNew.cast<double>(), _aftMappedTrans);
  _tfBroadcaster.sendTransform(_aftMappedTrans);

  //pub odom shift
  Eigen::Isometry3f odomShift;
  odomShift = _lidarMappedNew * _lidarOdomLast.inverse();
  _odomTransShift.stamp_ = _timeLaserOdometryMerged;
  Isometry2TFtransform(odomShift.cast<double>(), _odomTransShift);
  _tfBroadcaster.sendTransform(_odomTransShift);

  if (_sendSurroundCloud) {
    _surroundMapPubCount++;
    if (_surroundMapPubCount % (_surroundMapPubSkip + 1) == 1) {
      publishCloudMsg(_pubLaserCloudSurroundCorner, *_laserCloudCornerFromMap,
                      _timeLaserOdometryMerged, map_frame);
      publishCloudMsg(_pubLaserCloudSurroundSurf, *_laserCloudSurfFromMap,
                      _timeLaserOdometryMerged, map_frame);
    }
  }

  if (_sendRegisteredCloud) {

    publishCloudMsg(_pubCloudCornerLast2, *_laserCloudCornerStack,
                    _timeLaserOdometryMerged, "/aft_mapped");
    publishCloudMsg(_pubCloudSurfLast2, *_laserCloudSurfStack,
                    _timeLaserOdometryMerged, "/aft_mapped");
    publishCloudMsg(_pubLaserCloudFullRes, *_laserCloudFullRes,
                    _timeLaserOdometryMerged, "/aft_mapped");
  }
}

} // end namespace lidar_slam


// Copyright Nick Thompson, 2020
// 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)

#ifndef BOOST_MATH_INTERPOLATORS_CUBIC_HERMITE_HPP
#define BOOST_MATH_INTERPOLATORS_CUBIC_HERMITE_HPP
#include <memory>
#include <boost/math/interpolators/detail/cubic_hermite_detail.hpp>

namespace boost::math::interpolators {

template<class RandomAccessContainer>
class cubic_hermite {
public:
    using Real = typename RandomAccessContainer::value_type;

    cubic_hermite(RandomAccessContainer && x, RandomAccessContainer && y, RandomAccessContainer && dydx)
    : impl_(std::make_shared<detail::cubic_hermite_detail<RandomAccessContainer>>(std::move(x), std::move(y), std::move(dydx)))
    {}

    inline Real operator()(Real x) const {
        return impl_->operator()(x);
    }

    inline Real prime(Real x) const {
        return impl_->prime(x);
    }

    friend std::ostream& operator<<(std::ostream & os, const cubic_hermite & m)
    {
        os << *m.impl_;
        return os;
    }

    void push_back(Real x, Real y, Real dydx)
    {
        impl_->push_back(x, y, dydx);
    }

    int64_t bytes() const
    {
        return impl_->bytes() + sizeof(impl_);
    }

    std::pair<Real, Real> domain() const
    {
        return impl_->domain();
    }

private:
    std::shared_ptr<detail::cubic_hermite_detail<RandomAccessContainer>> impl_;
};

template<class RandomAccessContainer>
class cardinal_cubic_hermite {
public:
    using Real = typename RandomAccessContainer::value_type;

    cardinal_cubic_hermite(RandomAccessContainer && y, RandomAccessContainer && dydx, Real x0, Real dx)
    : impl_(std::make_shared<detail::cardinal_cubic_hermite_detail<RandomAccessContainer>>(std::move(y), std::move(dydx), x0, dx))
    {}

    inline Real operator()(Real x) const
    {
        return impl_->operator()(x);
    }

    inline Real prime(Real x) const
    {
        return impl_->prime(x);
    }

    friend std::ostream& operator<<(std::ostream & os, const cardinal_cubic_hermite & m)
    {
        os << *m.impl_;
        return os;
    }

    int64_t bytes() const
    {
        return impl_->bytes() + sizeof(impl_);
    }

    std::pair<Real, Real> domain() const
    {
        return impl_->domain();
    }

private:
    std::shared_ptr<detail::cardinal_cubic_hermite_detail<RandomAccessContainer>> impl_;
};

template<class RandomAccessContainer>
class cardinal_cubic_hermite_aos {
public:
    using Point = typename RandomAccessContainer::value_type;
    using Real = typename Point::value_type;

    cardinal_cubic_hermite_aos(RandomAccessContainer && data, Real x0, Real dx)
    : impl_(std::make_shared<detail::cardinal_cubic_hermite_detail_aos<RandomAccessContainer>>(std::move(data), x0, dx))
    {}

    inline Real operator()(Real x) const
    {
        return impl_->operator()(x);
    }

    inline Real prime(Real x) const
    {
        return impl_->prime(x);
    }

    friend std::ostream& operator<<(std::ostream & os, const cardinal_cubic_hermite_aos & m)
    {
        os << *m.impl_;
        return os;
    }

    int64_t bytes() const
    {
        return impl_->bytes() + sizeof(impl_);
    }

    std::pair<Real, Real> domain() const
    {
        return impl_->domain();
    }

private:
    std::shared_ptr<detail::cardinal_cubic_hermite_detail_aos<RandomAccessContainer>> impl_;
};

}
#endif


#ifndef INTCODE
#define INTCODE

#include <vector>
#include <deque>
#include <map>  // this structure is cool
#include <string>
using namespace std;

#include <boost/multiprecision/cpp_int.hpp>  // uses boost's bigint
namespace mp = boost::multiprecision;

// exit codes
const int C_END = 0;
const int C_CONTINUE = 1;
const int C_PAUSE = 2;

class IntcodeComputer {
private:
    map<int, mp::int128_t> _extraMemory;

    vector<mp::int128_t> _programBackup;
    vector<mp::int128_t> _program;
    bool _isDebug;

    deque<mp::int128_t> _inputQueue;
    deque<mp::int128_t> _outputQueue;

    // These parameters don't support concurency.
    // (To implement concurency, I will need to make some kind of controller class)
    bool _isReset;
    int _relativeBase;
    int _instructionPointer;
    mp::int128_t _currentInstruction;

    int GetParamMode(int pos);
    mp::int128_t GetValueAt(int pos);
    void SetValueAt(int pos, mp::int128_t value);
    int RunOpCode();

    // Private printers
    void PrintData(deque<mp::int128_t> const& input);
    void PrintData(vector<mp::int128_t> const& input);
    void PrintData(map<int, mp::int128_t> const& input);

public:
    IntcodeComputer(bool isDebug=false);

    void LoadProgram(string filename);
    int RunProgram();  // returns exit code. (exit or pause)

    void ResetProgram();

    void EnqueInput(mp::int128_t val);
    mp::int128_t DequeueOutput();

    // Printing methods
    void PrintInput();
    void PrintOutput();
    void PrintProgram(bool isBackup=false);
    void PrintExtraMemory();
};

#endif //!INTCODE


// MIT License
//
// Copyright (c) 2019 Jelle Spijker
//
// 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 <Eigen/Eigen>
#include <unsupported/Eigen/NonLinearOptimization>

#include <fluids/Solver.h>
#include "../include/fluids/Solver.h"
#include "Functor.h"

namespace Fluids {

Solver::Solver() {

}

Solver::Solver(const std::shared_ptr<System> &system) : m_system(system) {

}

void Solver::Solve() {
  if (m_system == nullptr)
    throw std::logic_error("No system to solve.");
  System_Functor func(m_system);
  Eigen::HybridNonLinearSolver<System_Functor> dl(func);
  auto x_initial = m_system->Get_Initial_vector();
  x_initial(0) = 2.0;
  x_initial(1) = 200000 - 10193.0 - 8587.;
  dl.solve(x_initial);
}

void Solver::Solve(const std::shared_ptr<System> &system) {
  if (system != Get_System())
    Set_System(system);
  Solver();
}

const std::shared_ptr<System> &Solver::Get_System() const {
  return m_system;
}

void Solver::Set_System(const std::shared_ptr<System> &system) {
  Solver::m_system = system;
}

}


#pragma once
#include <Eigen/Dense>

///
/// @struct Vechicle 3D Twist in rectangular coordinate system
///
struct Twist
{
    double x;  // Vehicle Longitudinal speed
    double y;  // Vehicle Lateral Speed
    double z;
    double roll;
    double pitch;
    double yaw;

    Twist(const double& x, const double& y, const double& z, const double& roll, const double& pitch, const double& yaw) : x(x), y(y), z(z), roll(roll), pitch(pitch), yaw(yaw)
    {
    }
    Twist() : x(0), y(0), z(0), roll(0), pitch(0), yaw(0)
    {
    }

    inline Twist operator=(const Twist& p)
    {
        this->x     = p.x;
        this->y     = p.y;
        this->z     = p.z;
        this->roll  = p.roll;
        this->pitch = p.pitch;
        this->yaw   = p.yaw;
        return *this;
    }

    inline Twist operator-(const Twist& p) const
    {
        Twist diff;
        diff.x     = this->x - p.x;
        diff.y     = this->y - p.y;
        diff.z     = this->z - p.z;
        diff.roll  = this->roll - p.roll;
        diff.pitch = this->pitch - p.pitch;
        diff.yaw   = this->yaw - p.yaw;
        return diff;
    }

    inline Twist operator+(const Twist& p) const
    {
        Twist diff;
        diff.x     = this->x + p.x;
        diff.y     = this->y + p.y;
        diff.z     = this->z + p.z;
        diff.roll  = this->roll + p.roll;
        diff.pitch = this->pitch + p.pitch;
        diff.yaw   = this->yaw + p.yaw;
        return diff;
    }

    inline Twist operator+=(const Twist& p)
    {
        this->x += p.x;
        this->y += p.y;
        this->z += p.z;
        this->roll += p.roll;
        this->pitch += p.pitch;
        this->yaw += p.yaw;
        return *this;
    }

    inline Twist operator*(double d) const
    {
        Twist diff;
        diff.x     = this->x * d;
        diff.y     = this->y * d;
        diff.z     = this->z * d;
        diff.roll  = this->roll * d;
        diff.pitch = this->pitch * d;
        diff.yaw   = this->yaw * d;
        return diff;
    }

    inline Twist operator/(double d) const
    {
        Twist diff;
        diff.x     = this->x / d;
        diff.y     = this->y / d;
        diff.z     = this->z / d;
        diff.roll  = this->roll / d;
        diff.pitch = this->pitch / d;
        diff.yaw   = this->yaw / d;
        return diff;
    }

    inline Twist operator/=(double d)
    {
        this->x /= d;
        this->y /= d;
        this->z /= d;
        this->roll /= d;
        this->pitch /= d;
        this->yaw /= d;
        return *this;
    }

    inline bool operator==(const Twist& p)
    {
        bool is_equal = true;
        is_equal      = is_equal && (this->x == p.x);
        is_equal      = is_equal && (this->y == p.y);
        is_equal      = is_equal && (this->z == p.z);
        is_equal      = is_equal && (this->roll == p.roll);
        is_equal      = is_equal && (this->pitch == p.pitch);
        is_equal      = is_equal && (this->yaw == p.yaw);
        return is_equal;
    }

    Eigen::Vector3d xyz() const;
    Eigen::Vector2d xy() const;
    Eigen::Vector3d rpy() const;
    Eigen::Vector3d xyyaw() const;
};

inline Eigen::Vector3d Twist::xyz() const
{
    return Eigen::Vector3d(x, y, z);
}

inline Eigen::Vector2d Twist::xy() const
{
    return Eigen::Vector2d(x, y);
}

inline Eigen::Vector3d Twist::rpy() const
{
    return Eigen::Vector3d(roll, pitch, yaw);
}

inline Eigen::Vector3d Twist::xyyaw() const
{
    return Eigen::Vector3d(x, y, yaw);
}


// Copyright (c) 2020, ETH Zurich
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//
//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
//       its contributors may be used to endorse or promote products derived
//       from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: Pascal Sommer (pasommer-at-student-dot-ethz-dot-ch)

#define TEST_NAME "base/gravity_relative_pose"
#include "util/testing.h"

#include <Eigen/Geometry>

#include "base/gravity_relative_pose.h"
#include "base/projection.h"
#include "util/random.h"

using namespace colmap;
using namespace Eigen;

BOOST_AUTO_TEST_CASE(TestEstimateRelativePoseGravity) {
  SetPRNGSeed(0);

  const std::size_t kNumPoints = 100;

  std::vector<Eigen::Vector3d> points3D;
  for (std::size_t i = 0; i < kNumPoints; ++i) {
    points3D.emplace_back(RandomReal<double>(0, 1),
                          RandomReal<double>(-1, 1),
                          RandomReal<double>(0.5, 5));
  }

  std::array<Pose, 2> poses_orig;
  poses_orig[0].tvec = Vector3d::Zero();
  poses_orig[0].qvec = Vector4d(0.981445, 0.104679, -0.15143, 0.053634);
  poses_orig[1].tvec = Vector3d(-4, -0.5, 5).normalized();
  poses_orig[1].qvec = Vector4d(0.960427, -0.101919, 0.258302, -0.021735);

  std::array<Image, 2> images;
  std::array<Camera, 2> cameras;
  std::array<std::vector<Vector2d>, 2> projected;
  std::array<Matrix3x4d, 2> projection_matrix;
  for (auto i : {0, 1}) {
    images[i].SetGravityPrior(
        QuaternionRotatePoint(poses_orig[i].qvec, Vector3d::UnitY()));

    cameras[i].SetCameraId(i);
    cameras[i].InitializeWithName("PINHOLE", 1, 1, 1);

    projection_matrix[i] =
        ComposeProjectionMatrix(poses_orig[i].qvec, poses_orig[i].tvec);

    projected[i].reserve(kNumPoints);
  }

  FeatureMatches matches;
  matches.reserve(kNumPoints);

  for (std::size_t point_id = 0; point_id < kNumPoints; ++point_id) {
    matches.emplace_back(point_id, point_id);

    for (auto i : {0, 1}) {
      const Vector2d projected_normalized =
          (projection_matrix[i] * points3D[point_id].homogeneous())
              .hnormalized();
      projected[i].push_back(cameras[i].WorldToImage(projected_normalized));
    }
  }

  for (auto i : {0, 1}) {
    images[i].SetPoints2D(projected[i]);
  }

  RANSACOptions ransac_options;
  ransac_options.min_num_trials = 10;
  ransac_options.max_num_trials = 50;
  ransac_options.max_error = 1e-5;

  std::array<Pose, 2> poses_est;

  const auto report = EstimateRelativePoseGravity(images, cameras, matches,
                                                  ransac_options, &poses_est,
                                                  nullptr);
  const unsigned nr_inliers = report.support.num_inliers;

  BOOST_CHECK_EQUAL(poses_est[0].tvec, Vector3d::Zero());
  BOOST_CHECK_CLOSE(poses_est[0].qvec.norm(), 1., 1e-9);
  BOOST_CHECK_CLOSE(poses_est[1].tvec.norm(), 1., 1e-9);
  BOOST_CHECK_CLOSE(poses_est[1].qvec.norm(), 1., 1e-9);
  BOOST_CHECK_EQUAL(nr_inliers, kNumPoints);

  BOOST_CHECK_SMALL((poses_est[1].tvec - poses_orig[1].tvec).norm(), 1e-9);

  const Vector4d relative_rot_est = ConcatenateQuaternions(
      InvertQuaternion(poses_est[1].qvec), poses_est[0].qvec);
  const Vector4d relative_rot_orig = ConcatenateQuaternions(
      InvertQuaternion(poses_orig[1].qvec), poses_orig[0].qvec);

  BOOST_CHECK_SMALL((relative_rot_est - relative_rot_orig).norm(), 1e-9);
}


#include <cstdio>
#include <cstdlib>
#include <cmath>

#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/stepper/dense_output_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>
#include "ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver.h"

#include "ch_ethz_bhepp_ode_boost_NativeStepperType.h"
#include "Utilities.hpp"

using namespace boost::numeric::odeint;

typedef boost::numeric::ublas::vector<double> vector_type;
typedef double time_type;
typedef vector_type state_type;

template< class ode_system >
class stepper_wrapper {
public:

	virtual ~stepper_wrapper() { };

	virtual void initialize(const state_type& x0, const time_type t0, const time_type dt0) = 0;

	virtual std::pair< time_type, time_type > do_step(ode_system& sys) = 0;

	virtual void calc_state(const time_type t_inter, state_type& x) = 0;

	virtual const time_type current_time() = 0;

	virtual const state_type& current_state() = 0;

	virtual const time_type current_time_step() = 0;

};

template < typename base_stepper_type, typename ode_system >
class stepper_wrapper_impl : public stepper_wrapper< ode_system > {
	base_stepper_type base_stepper;

public:

	stepper_wrapper_impl(base_stepper_type base_stepper)
	: base_stepper(base_stepper) { }

	void initialize(const state_type& x0, const time_type t0, const time_type dt0) {
		base_stepper.initialize(x0, t0, dt0);
	}

	std::pair< time_type, time_type > do_step(ode_system& sys) {
		return base_stepper.do_step(sys);
	}

	void calc_state(const time_type t_inter, state_type& x) {
		base_stepper.calc_state(t_inter, x);
	}

	const time_type current_time() {
		return base_stepper.current_time();
	}

	const state_type& current_state() {
		return base_stepper.current_state();
	}

	const time_type current_time_step() {
		return base_stepper.current_time_step();
	}

};

struct ode_system_wrapper;

struct boost_solver_data {

//	dense_output_runge_kutta< runge_kutta_dopri5< state_type > > stepper;
//	runge_kutta_dopri5< state_type > stepper;
	stepper_wrapper< ode_system_wrapper >* stepper;
	ode_system_wrapper* ode_system;
//	dense_dopri5_stepper_type stepper;
    JNIEnv* env;
    jobject ode;
    jmethodID vectorFieldMethodID;
    jdouble initialStepSize;
    jdouble t;
    jdouble t1;
    jdouble* x;
    jdouble* xTmp;
    jdouble* xDot;
    state_type y;
    state_type yTmp;
    state_type yDot;
	jdouble lastStepSize;
	jdouble currentStepSize;

	void operator()(const state_type &x, state_type &dxdt, double t) {
//	    for (state_type::const_iterator it=x.begin(); it != x.end(); ++it) {
		for (int i=0; i < x.size(); i++)
	    	xTmp[i] = x[i];
	    env->CallVoidMethod(ode, vectorFieldMethodID, t);
	    if (env->ExceptionCheck() == JNI_TRUE) {
	        fprintf(stderr, "f_direct_bridge: Failed to call computeVectorField!\n");
	        return;
	    }
//	    for (state_type::const_iterator it=x.begin(); it != x.end(); ++it)
		for (int i=0; i < dxdt.size(); i++)
	    	dxdt[i] = xDot[i];
    }

};

struct ode_system_wrapper {
	boost_solver_data* data;

	ode_system_wrapper(boost_solver_data* data)
	: data(data) { }

	void operator()(const state_type &x, state_type &dxdt, double t) {
		data->operator()(x, dxdt, t);
	}
};

typedef result_of::make_dense_output<
    runge_kutta_dopri5< state_type > >::type dense_dopri5_stepper_type;
typedef bulirsch_stoer_dense_out< state_type > dense_bulirsch_stoer_stepper_type;
//typedef result_of::make_dense_output<
//    bulirsch_stoer< state_type > >::type dense_bulirsch_stoer_stepper_type;
//typedef result_of::make_dense_output<
//	rosenbrock4< double > >::type dense_rosenbrock4_stepper_type;

template class stepper_wrapper_impl< dense_dopri5_stepper_type, ode_system_wrapper >;
template class stepper_wrapper_impl< dense_bulirsch_stoer_stepper_type, ode_system_wrapper >;
//template class stepper_wrapper_impl< dense_rosenbrock4_stepper_type, ode_system_wrapper >;

/*
 * Class:     ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver
 * Method:    jni_initialize
 * Signature: (Lch/ethz/bhepp/ode/BufferOdeAdapter;Ljava/nio/DoubleBuffer;Ljava/nio/DoubleBuffer;Ljava/nio/DoubleBuffer;DDDI)J
 */
JNIEXPORT jlong JNICALL Java_ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver_jni_1initialize
	(JNIEnv *env, jobject obj, jobject ode, jobject xBuffer, jobject xTmpBuffer, jobject xDotBuffer,
			jdouble initialStepSize, jdouble relTol, jdouble absTol, jint stepperType) {
    jclass odeCls = env->GetObjectClass(ode);
    if (env->ExceptionCheck() == JNI_TRUE) {
        fprintf(stderr, "jni_initialize: Failed to get class of ode object\n");
        return 0;
    }
    jmethodID methodID = env->GetMethodID(odeCls, "getDimensionOfVectorField", "()I");
    if (env->ExceptionCheck() == JNI_TRUE) {
        fprintf(stderr, "jni_initialize: Failed to get method ID of getDimensionOfVectorField\n");
        return 0;
    }
    jint jdimension = env->CallIntMethod(ode, methodID);
    state_type::size_type dimension = jdimension;
    if (env->ExceptionCheck() == JNI_TRUE) {
        fprintf(stderr, "jni_initialize: Failed to call getDimensionOfVectorField\n");
        return 0;
    }

    // Allocate and initialize boost_solver_data
    boost_solver_data* data = new boost_solver_data();

    // FIXME
    if (env->ExceptionCheck() == JNI_TRUE) {
        fprintf(stderr, "jni_initialize: Failed to allocate boost_solver_data\n");
        return 0;
    }

    switch (stepperType) {
    case ch_ethz_bhepp_ode_boost_NativeStepperType_BOOSTSTEPPERTYPE_DORMANDPRINCE5:
    {
    	dense_dopri5_stepper_type base_stepper1 = make_dense_output< runge_kutta_dopri5< state_type > >(absTol, relTol);
    	data->stepper = new stepper_wrapper_impl< dense_dopri5_stepper_type, ode_system_wrapper >(base_stepper1);
    	break;
    }
    case ch_ethz_bhepp_ode_boost_NativeStepperType_BOOSTSTEPPERTYPE_BULIRSCHSTOER:
    {
    	dense_bulirsch_stoer_stepper_type base_stepper2 = dense_bulirsch_stoer_stepper_type(absTol, relTol);
    	data->stepper = new stepper_wrapper_impl< dense_bulirsch_stoer_stepper_type, ode_system_wrapper >(base_stepper2);
    	break;
    }
//    case ch_ethz_bhepp_ode_boost_NativeStepperType_BOOSTSTEPPERTYPE_ROSENBROCK4:
//    {
//    	dense_rosenbrock4_stepper_type base_stepper3 = make_dense_output< rosenbrock4< double > >(absTol, relTol);
//    	data->stepper = new stepper_wrapper_impl< dense_rosenbrock4_stepper_type, ode_system_wrapper >(base_stepper3);
//    	break;
//    }
    default:
	{
        throw_java_exception(env, "Unknown stepper type\n");
    	delete data;
    	return 0;
	}
    }

    // FIXME
    data->ode_system = new ode_system_wrapper(data);
    data->initialStepSize = initialStepSize;

    data->env = env;
    // Acquire method IDs of callbacks and put them into boost_solver_data
    data->ode = env->NewGlobalRef(ode);
    if (env->ExceptionCheck() == JNI_TRUE) {
        fprintf(stderr, "jni_initialize: Failed to create global reference to ode object\n");
        delete data;
        return 0;
    }
    data->vectorFieldMethodID = env->GetMethodID(odeCls, "computeVectorField", "(D)V");
    if (env->ExceptionCheck() == JNI_TRUE) {
        fprintf(stderr, "jni_initialize: Failed to get method ID of computeVectorField\n");
        // Delete global java references
        env->DeleteGlobalRef(data->ode);
        delete data;
        return 0;
    }

    // Check capacities of direct buffer objects
    if (env->GetDirectBufferCapacity(xBuffer) < dimension) {
        throw_java_exception(env, "jni_initialize: Direct buffer xBuffer is not big enough\n");
        // Delete global java references
        env->DeleteGlobalRef(data->ode);
        delete data;
        return 0;
    }
    if (env->GetDirectBufferCapacity(xTmpBuffer) < dimension) {
        throw_java_exception(env, "jni_initialize: Direct buffer xTmpBuffer is not big enough\n");
        // Delete global java references
        env->DeleteGlobalRef(data->ode);
        delete data;
        return 0;
    }
    if (env->GetDirectBufferCapacity(xDotBuffer) < dimension) {
        throw_java_exception(env, "jni_initialize: Direct buffer xDotBuffer is not big enough\n");
        // Delete global java references
        env->DeleteGlobalRef(data->ode);
        delete data;
        return 0;
    }
    // Get addresses of direct buffer objects
    data->x = static_cast<jdouble*>(env->GetDirectBufferAddress(xBuffer));
    if (data->x == NULL) {
        fprintf(stderr, "jni_initialize: Failed to get direct address of xBuffer\n");
        // Delete global java references
        env->DeleteGlobalRef(data->ode);
        delete data;
        return 0;
    }
    data->xTmp = static_cast<jdouble*>(env->GetDirectBufferAddress(xTmpBuffer));
    if (data->xTmp == NULL) {
        fprintf(stderr, "jni_initialize: Failed to get direct address of xTmpBuffer\n");
        // Delete global java references
        env->DeleteGlobalRef(data->ode);
        delete data;
        return 0;
    }
    data->xDot = static_cast<jdouble*>(env->GetDirectBufferAddress(xDotBuffer));
    if (data->xDot == NULL) {
        fprintf(stderr, "jni_initialize: Failed to get direct address of xDotBuffer\n");
        // Delete global java references
        env->DeleteGlobalRef(data->ode);
        delete data;
        return 0;
    }

    data->y.resize(dimension, false);
    data->yTmp.resize(dimension, false);
    data->yDot.resize(dimension, false);

    data->lastStepSize = data->initialStepSize;
    data->currentStepSize = data->initialStepSize;

    // Pass pointer-address of boost_solver_data back to Java
    jlong jni_pointer = (jlong)data;
    return jni_pointer;
}

/*
 * Class:     ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver
 * Method:    jni_dispose
 * Signature: (J)V
 */
JNIEXPORT void JNICALL Java_ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver_jni_1dispose
  (JNIEnv *env, jobject obj, jlong jni_pointer) {
    boost_solver_data* data = (boost_solver_data*)jni_pointer;
    if (data != NULL) {
        // Delete global java references
        if (data->ode != NULL)
            env->DeleteGlobalRef(data->ode);
        // Free data structure
        delete data;
    }
}

/*
 * Class:     ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver
 * Method:    jni_prepareStep
 * Signature: (JDD)V
 */
JNIEXPORT void JNICALL Java_ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver_jni_1prepareStep
	(JNIEnv *env, jobject obj, jlong jni_pointer, jdouble t0, jdouble t1) {
    boost_solver_data* data = reinterpret_cast< boost_solver_data* >(jni_pointer);
    data->t = t0;
    data->t1 = t1;
    for (int i=0; i < data->y.size(); i++)
    	data->y[i] = data->x[i];
    data->stepper->initialize(data->y, data->t, data->currentStepSize);
}

/*
 * Class:     ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver
 * Method:    jni_getCurrentState
 * Signature: (J)D
 */
JNIEXPORT jdouble JNICALL Java_ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver_jni_1getCurrentState
  (JNIEnv *env, jobject obj, jlong jni_pointer) {
    boost_solver_data* data = reinterpret_cast< boost_solver_data* >(jni_pointer);
    const dense_dopri5_stepper_type::state_type& current_state = data->stepper->current_state();
    for (int i=0; i < data->y.size(); i++)
    	data->xTmp[i] = current_state[i];
    return data->stepper->current_time();
}

/*
 * Class:     ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver
 * Method:    jni_integrateStep
 * Signature: (J)D
 */
JNIEXPORT jdouble JNICALL Java_ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver_jni_1integrateStep
(JNIEnv *env, jobject obj, jlong jni_pointer) {
    boost_solver_data* data = reinterpret_cast< boost_solver_data* >(jni_pointer);
    std::pair< double , double > times = data->stepper->do_step(*data->ode_system);
    data->lastStepSize = data->currentStepSize;
    data->currentStepSize = data->stepper->current_time_step();
    return times.second;
}

/*
 * Class:     ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver
 * Method:    jni_computeInterpolatedSolution
 * Signature: (JD)V
 */
JNIEXPORT void JNICALL Java_ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver_jni_1computeInterpolatedSolution
  (JNIEnv *env, jobject obj, jlong jni_pointer, jdouble t) {
    boost_solver_data* data = reinterpret_cast< boost_solver_data* >(jni_pointer);
    data->stepper->calc_state(t, data->yTmp);
    for (int i=0; i < data->yTmp.size(); i++)
    	data->xTmp[i] = data->yTmp[i];
}

/*
 * Class:     ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver
 * Method:    jni_setCurrentStepSize
 * Signature: (JD)V
 */
JNIEXPORT void JNICALL Java_ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver_jni_1setCurrentStepSize
  (JNIEnv *env, jobject obj, jlong jni_pointer, jdouble stepSize) {
    boost_solver_data* data = reinterpret_cast< boost_solver_data* >(jni_pointer);
    data->currentStepSize = stepSize;
}

/*
 * Class:     ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver
 * Method:    jni_getCurrentStepSize
 * Signature: (J)D
 */
JNIEXPORT jdouble JNICALL Java_ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver_jni_1getCurrentStepSize
  (JNIEnv *env, jobject obj, jlong jni_pointer) {
    boost_solver_data* data = reinterpret_cast< boost_solver_data* >(jni_pointer);
    return data->currentStepSize;
}

/*
 * Class:     ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver
 * Method:    jni_getLastStepSize
 * Signature: (J)D
 */
JNIEXPORT jdouble JNICALL Java_ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver_jni_1getLastStepSize
(JNIEnv *env, jobject obj, jlong jni_pointer) {
  boost_solver_data* data = reinterpret_cast< boost_solver_data* >(jni_pointer);
  return data->lastStepSize;
}


//==============================================================================
//         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_EXPONENTIAL_CONSTANTS_LOG10_ELO_HPP_INCLUDED
#define NT2_EXPONENTIAL_CONSTANTS_LOG10_ELO_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>

namespace nt2
{
  namespace tag
  {
    /*!
      @brief log10_elo generic tag

      Represents the log10_elo constant in generic contexts.

      @par Models:
      Hierarchy
    **/
    BOOST_SIMD_CONSTANT_REGISTER( Log10_elo, double
                                , 0, 0x3a37b152UL  // 7.0073188e-04
                                , 0x0ULL
                                )
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::Log10_elo, Site> dispatching_Log10_elo(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::Log10_elo, Site>();
    }
    template<class... Args>
    struct impl_Log10_elo;
  }
 /*!
    Generates constant log10_elo.This constant is coupled with Log10_ehi and is
    used in the float logarithms computations
    We have abs( double(log10_elo<float>())+double(Log10_ehi<float>()) - Log_10(e)) < 3e-11

    @par Semantic:

    @code
    T r = log10_elo<T>();
    @endcode

  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Log10_elo, Log10_elo);
}

#endif


//////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::include.hpp                         //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  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)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_INCLUDE_HPP_ER_2009

#include <boost/statistics/detail/distribution_toolkit/distributions/chi_squared/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/exponential/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/gamma/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/location_scale/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/normal/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/students_t/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/uniform/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/wrapper/include.hpp>

#endif


//          Copyright Gavin Band 2008 - 2012.
// Distributed under 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 <iostream>
#include <iomanip>
#include <vector>
#include <utility>
#include <algorithm>
#include <boost/format.hpp>
#include <boost/iterator/counting_iterator.hpp>
#include "metro/regression/BinomialLogistic.hpp"
#include "metro/intersect_ranges.hpp"
#include "metro/count_range.hpp"
#include "metro/summation.hpp"
#include "genfile/string_utils.hpp"
#include "genfile/Error.hpp"

// #define DEBUG_LOGLIKELIHOOD 1

namespace metro {
	namespace regression {
		namespace {
			std::string standard_parameter_name( std::string const& predictor_name, std::string const& outcome_name ) {
				return predictor_name + "/" + outcome_name ;
			}
		}

		BinomialLogistic::UniquePtr BinomialLogistic::create(
			regression::Design& design
		) {
			return BinomialLogistic::UniquePtr(
				new BinomialLogistic( design )
			) ;
		}

		BinomialLogistic::UniquePtr BinomialLogistic::create(
			regression::Design::UniquePtr design
		) {
			return BinomialLogistic::UniquePtr(
				new BinomialLogistic( design )
			) ;
		}

		BinomialLogistic::UniquePtr BinomialLogistic::create(
			regression::Design& design,
			std::vector< metro::SampleRange > included_samples
		) {
			return BinomialLogistic::UniquePtr(
				new BinomialLogistic( design, included_samples )
			) ;
		}

		BinomialLogistic::UniquePtr BinomialLogistic::create(
			regression::Design::UniquePtr design,
			std::vector< metro::SampleRange > included_samples
		) {
			return BinomialLogistic::UniquePtr(
				new BinomialLogistic( design, included_samples )
			) ;
		}

		namespace {
			void check_design( regression::Design const& design ) {
				// Verify design looks sane.
				BinomialLogistic::Matrix const& outcome = design.outcome() ;
				// We insist that outcomes are smallish non-negative integers.
				// Check this by comparing to absolute valued, floored version.
				if( outcome.rows() > 0 && outcome.cols() > 0
					&& (outcome.array() - outcome.array().abs().floor()).abs().maxCoeff() != 0 ) {
					throw genfile::BadArgumentError(
						"metro::case_control::BinomialLogistic::check_design()",
						"design",
						"Outcome does not consist solely of nonnegative integers."
					) ;
				}
			}
		}

		BinomialLogistic::BinomialLogistic(
			regression::Design::UniquePtr design
		):
			m_design( design.release() ),
			m_design_owned( true ),
			m_get_parameter_name( &standard_parameter_name ),
			m_parameters( Vector::Zero( m_design->matrix().cols() ) ),
			m_included_samples( 1, SampleRange( 0, m_design->matrix().rows() )),
			m_numberOfDerivativesComputed( -1 ),
			m_numberOfCDLDerivativesComputed( -1 )
		{
			check_design( *m_design ) ;
#if DEBUG_LOGLIKELIHOOD
			std::cerr << "BinomialLogistic::BinomialLogistic(): created with " << impl::count_range( m_included_samples ) << " samples.\n" ;
#endif
			setup_storage( m_included_samples ) ;
		}

		BinomialLogistic::BinomialLogistic(
			regression::Design::UniquePtr design,
			std::vector< metro::SampleRange > included_samples
		):
			m_design( design.release() ),
			m_design_owned( true ),
			m_get_parameter_name( &standard_parameter_name ),
			m_parameters( Vector::Zero( m_design->matrix().cols() ) ),
			m_included_samples( included_samples ),
			m_numberOfDerivativesComputed( -1 ),
			m_numberOfCDLDerivativesComputed( -1 )
		{
			check_design( *m_design ) ;
#if DEBUG_LOGLIKELIHOOD
			std::cerr << "BinomialLogistic::BinomialLogistic(): created with " << impl::count_range( m_included_samples ) << " samples.\n" ;
#endif
			setup_storage( m_included_samples ) ;
		}

		BinomialLogistic::BinomialLogistic(
			regression::Design& design
		):
			m_design( &design ),
			m_design_owned( false ),
			m_get_parameter_name( &standard_parameter_name ),
			m_parameters( Vector::Zero( m_design->matrix().cols() ) ),
			m_included_samples( 1, SampleRange( 0, m_design->matrix().rows() )),
			m_numberOfDerivativesComputed( -1 ),
			m_numberOfCDLDerivativesComputed( -1 )
		{
			assert( m_design != 0 ) ;
			check_design( *m_design ) ;
#if DEBUG_LOGLIKELIHOOD
			std::cerr << "BinomialLogistic::BinomialLogistic(): created with " << impl::count_range( m_included_samples ) << " samples.\n" ;
#endif
			setup_storage( m_included_samples ) ;
		}

		BinomialLogistic::BinomialLogistic(
			regression::Design& design,
			std::vector< metro::SampleRange > included_samples
		):
			m_design( &design ),
			m_design_owned( false ),
			m_get_parameter_name( &standard_parameter_name ),
			m_parameters( Vector::Zero( m_design->matrix().cols() ) ),
			m_included_samples( included_samples ),
			m_numberOfDerivativesComputed( -1 ),
			m_numberOfCDLDerivativesComputed( -1 )
		{
			assert( m_design != 0 ) ;
			check_design( *m_design ) ;
#if DEBUG_LOGLIKELIHOOD
			std::cerr << "BinomialLogistic::BinomialLogistic(): created with " << impl::count_range( m_included_samples ) << " samples.\n" ;
#endif
			setup_storage( m_included_samples ) ;
		}

		BinomialLogistic::~BinomialLogistic() {
			if( m_design_owned ) {
				delete m_design ;
			}
		}

		std::string BinomialLogistic::get_summary() const {
			using genfile::string_utils::to_string ;
			std::string result = "BinomialLogistic( "
				+ to_string( impl::count_range( m_included_samples ))
				+ " of "
				+ to_string( m_design->outcome().rows() )
				+ " samples ): " ;
			result += "(" + m_design->get_outcome_name(1) + ") ~ " ;
			IntegerMatrix const identity = identify_parameters() ;
			for( int i = 0; i < identity.rows(); ++i ) {
				result +=
					((i>0) ? " + " : "")
					+ get_parameter_name(i) ;
			}
			return result ;
		}

		void BinomialLogistic::set_parameter_naming_scheme( BinomialLogistic::GetParameterName get_parameter_name ) {
			assert( get_parameter_name ) ;
			m_get_parameter_name = get_parameter_name ;
		}

		std::string BinomialLogistic::get_parameter_name( std::size_t i ) const {
			IntegerMatrix const identity = identify_parameters() ;
			return m_get_parameter_name( m_design->get_predictor_name( identity(i,1) ), m_design->get_outcome_name(identity(i,0)) ) ;
		}

		LogLikelihood::IntegerMatrix BinomialLogistic::identify_parameters() const {
			IntegerMatrix result = IntegerMatrix::Zero( m_design->matrix().cols(), 2 ) ;
			// Each parameter corresponds to an effect for the non-baseline outcome level
			// and a column of the design matrix.
			result.col(0).setConstant( 1.0 ) ;
			// There is one parameter per design matrix column.
			for( int i = 0; i < m_design->matrix().cols(); ++i ) {
				result( i, 1 ) = i ;
			}
			return result ;
		}

		int BinomialLogistic::number_of_outcomes() const {
			return 2 ;
		}

		void BinomialLogistic::evaluate_at( Point const& parameters, int const numberOfDerivatives ) {
			evaluate_at_impl(
				parameters,
				metro::impl::intersect_ranges(
					m_included_samples,
					m_design->nonmissing_samples()
				),
				numberOfDerivatives
			) ;
		}

		void BinomialLogistic::evaluate_at_impl(
			Point const& parameters,
			std::vector< metro::SampleRange > const& evaluation_samples,
			int const numberOfDerivatives
		) {
			assert( parameters.size() == m_design->matrix().cols() ) ;
			assert( numberOfDerivatives < 3 ) ;

			if(
				evaluation_samples != m_evaluated_samples
				|| m_design->get_number_of_predictor_levels() != m_hx.cols()
				|| m_design->matrix().cols() != parameters.size()
			) {
				m_evaluated_samples = evaluation_samples ;
				m_numberOfDerivativesComputed = -1 ;
				m_numberOfCDLDerivativesComputed = -1 ;
				setup_storage( m_evaluated_samples ) ;
			}

			if( parameters != m_parameters ) {
				m_parameters = parameters ;
				m_numberOfDerivativesComputed = -1 ;
				m_numberOfCDLDerivativesComputed = -1 ;
			}

#if DEBUG_LOGLIKELIHOOD
			std::cerr << std::fixed << std::setprecision(4) ;
			int const N = m_design->outcome().rows() ;
			std::cerr << "==== BinomialLogistic::evaluate_at() ====\n" ;
			std::cerr << "  m_numberOfDerivativesComputed: " << m_numberOfDerivativesComputed << ", "
				<< "Number of samples: " << N << ", "
				<< "Included samples:" ;
			for( std::size_t i = 0; i < evaluation_samples.size(); ++i ) {
				std::cerr << " " << evaluation_samples[i].begin() << "-" << evaluation_samples[i].end() ;
			}
			std::cerr << "\n" ;
			std::cerr << "   Params: " << parameters.transpose() << ".\n" ;
			std::cerr << "   Phenotypes: " << m_design->outcome().col(1).segment(0, std::min( N, 5 )).transpose() << "...\n";
			std::cerr << "   Design:\n" << m_design->matrix().block( 0, 0, std::min( N, 5 ), m_design->matrix().cols() ) << "...\n";
#endif

			evaluate( numberOfDerivatives ) ;
		}

		void BinomialLogistic::setup_storage( std::vector< metro::SampleRange > const& evaluation_samples ) {
			int const R = metro::impl::count_range( evaluation_samples ) ;
			int const L = m_design->get_number_of_predictor_levels() ;
			int const D = m_design->matrix().cols() ;

#if DEBUG_LOGLIKELIHOOD
			std::cerr << "metro::regression::BinomialLogistic::setup_storage(): Number of evaluated samples is " << R << "\n" ;
#endif

			// To avoid reallocation we only resize the stored matrices
			// if we need more rows
			if( R > m_hx.rows() || L != m_hx.cols() ) {
				m_hx.resize( R, L ) ;
				m_normalisedDhx.resize( R, L ) ;
				m_normalisedDdhx.resize( R, L ) ;
				m_first_derivative_terms.resize( R, D ) ;

#if DEBUG_LOGLIKELIHOOD
				std::cerr << "allocated storage: " << m_hx.rows() << " x " << m_hx.cols() << ".\n" ;
#endif
			}

			// Since the allocated matrices may be larger than the number of
			// actual samples analysed, we keep track of this number as well.
			m_number_of_stored_samples = R ;

			// For each range in the evaluated samples, map to a range
			// in the stored samples:
			m_storage_samples.resize( evaluation_samples.size() ) ;
			int storage_row = 0 ;
			for( std::size_t i = 0; i < evaluation_samples.size(); ++i ) {
				int const size = evaluation_samples[i].size() ;
				m_storage_samples[i] = metro::SampleRange( storage_row, storage_row + size ) ;
				storage_row += size ;
			}
		}

		void BinomialLogistic::evaluate( int const numberOfDerivatives ) {
			bool computeFunction = ( m_numberOfDerivativesComputed < 0 ) ;
			bool compute1stDerivative = ( (numberOfDerivatives > 0) & (m_numberOfDerivativesComputed < 1) ) ;
			bool compute2ndDerivative = ( (numberOfDerivatives > 1) & (m_numberOfDerivativesComputed < 2) ) ;

#if DEBUG_LOGLIKELIHOOD
			std::cerr << "BinomialLogistic::evaluate(): numberOfDerivatives = " << numberOfDerivatives << ".\n" ;
			std::cerr << "BinomialLogistic::evaluate(): m_numberOfDerivativesComputed = " << m_numberOfDerivativesComputed << ".\n" ;
			std::cerr << "Computing: "
				<< (computeFunction ? "function " : "" )
				<< (compute1stDerivative ? "1st derivative " : "" )
				<< (compute2ndDerivative ? "2nd derivative " : "" )
				<< "\n" ;
			//std::cerr << "  Predictor levels: " << m_design->get_predictor_levels().transpose() << ".\n" ;
#endif

			if( numberOfDerivatives > m_numberOfCDLDerivativesComputed ) {
#if DEBUG_LOGLIKELIHOOD
				std::cerr << "BinomialLogistic::evaluate(): computing CDL\n" ;
#endif
				// TODO: compute fx, dfx, ddfx here.
				compute_complete_data_likelihood_and_derivatives(
					m_parameters,
					&m_hx,
					&m_normalisedDhx,
					&m_normalisedDdhx,
					2
				) ;

				// Convert from complete data likelihood into expected complete data likelihood
				// and derivatives
				int const L = m_design->get_number_of_predictor_levels() ;
				assert( m_hx.rows() >= m_number_of_stored_samples && m_hx.cols() == L ) ;
				assert( m_normalisedDhx.rows() >= m_number_of_stored_samples && m_normalisedDhx.cols() == L ) ;
				assert( m_normalisedDdhx.rows() >= m_number_of_stored_samples && m_normalisedDdhx.cols() == L ) ;
				for( std::size_t i = 0; i < m_evaluated_samples.size(); ++i ) {
					SampleRange const& range = m_evaluated_samples[i] ;
					SampleRange const& storage_range = m_storage_samples[i] ;
					ConstMatrixBlock probabilityBlock = m_design->get_predictor_level_probabilities().block( range.begin(), 0, range.size(), L ) ;

					m_hx.block( storage_range.begin(), 0, storage_range.size(), L ).array() *= probabilityBlock.array() ;
					m_normalisedDhx.block( storage_range.begin(), 0, storage_range.size(), L ).array() *= probabilityBlock.array() ;
					m_normalisedDdhx.block( storage_range.begin(), 0, storage_range.size(), L ).array() *= probabilityBlock.array() ;
				}

				// normalise the derivatives by h, as required in derivative computations
				typedef Eigen::DiagonalMatrix< double, Eigen::Dynamic > DiagonalMatrix ;
				DiagonalMatrix one_over_h = (m_hx.rowwise().sum().asDiagonal().inverse()) ;
				m_normalisedDhx = one_over_h * m_normalisedDhx ;
				m_normalisedDdhx = one_over_h * m_normalisedDdhx ;

				m_numberOfCDLDerivativesComputed = 2 ;
			}
#if DEBUG_LOGLIKELIHOOD
			else {
				std::cerr << "BinomialLogistic::evaluate(): skipping CDL computation.\n" ;
			}
#endif

			if( computeFunction ) {
#if DEBUG_LOGLIKELIHOOD
				std::cerr << "BinomialLogistic::evaluate(): computing fx\n" ;
#endif
				compute_value_of_loglikelihood(
					m_hx,
					&m_value_of_function
				) ;
				m_numberOfDerivativesComputed = 0 ;
			}

			if( compute1stDerivative ) {
#if DEBUG_LOGLIKELIHOOD
				std::cerr << "BinomialLogistic::evaluate(): computing dfx\n" ;
#endif
				compute_value_of_first_derivative(
					m_normalisedDhx,
					&m_first_derivative_terms,
					&m_value_of_first_derivative
				) ;
				m_numberOfDerivativesComputed = 1 ;
			}

			if( compute2ndDerivative ) {
#if DEBUG_LOGLIKELIHOOD
				std::cerr << "BinomialLogistic::evaluate(): computing ddfx\n" ;
#endif
				compute_value_of_second_derivative(
					m_first_derivative_terms,
					m_normalisedDdhx,
					&m_value_of_second_derivative
				) ;
				m_numberOfDerivativesComputed = 2 ;
			}

#if DEBUG_LOGLIKELIHOOD
			std::cerr << "  " << ( computeFunction ? "(computed) " : "(memoized) " ) << "function value = " << m_value_of_function << "\n" ;
#endif

#if DEBUG_LOGLIKELIHOOD
			if( m_numberOfDerivativesComputed > 0 ) {
				std::cerr << "  " << ( compute1stDerivative ? "(computed) " : "(memoized) " ) << "derivative = " << m_value_of_first_derivative.transpose() << "\n" ;
			} else {
				std::cerr << "  (1st derivative not computed).\n" ;
			}
#endif

#if DEBUG_LOGLIKELIHOOD
			if( m_numberOfDerivativesComputed > 1 ) {
				std::cerr << "  " << ( compute2ndDerivative ? "(computed) " : "(memoized) " ) << "2nd derivative = \n"
					<< m_value_of_second_derivative << ".\n" ;
			} else {
				std::cerr << "  (2nd derivative not computed).\n" ;
			}
			std::cerr << "====\n" ;
#endif
		}

		void BinomialLogistic::compute_complete_data_likelihood_and_derivatives(
			Point const& parameters,
			Matrix* fx,
			Matrix* dfx,
			Matrix* ddfx,
			int const numberOfDerivatives
		) {
			int const L = m_design->get_number_of_predictor_levels() ;

			// Outcome data is two columns, conceptually row sum is n
			// (the number of trials) and 2nd column is k (number of successes / non-baseline outcomes)
			// The computation requires us to compute f0 and f1, the prob of a single
			// baseline / non-baseline observation.
			m_f1.resize( fx->rows(), 1 ) ;
			Vector linear_combination ;

			for( int g = 0; g < L; ++g ) {
				for( std::size_t i = 0; i < m_evaluated_samples.size(); ++i ) {
					metro::SampleRange const& range = m_evaluated_samples[i] ;
					metro::SampleRange const& storage_range = m_storage_samples[i] ;

					// Setup relevant matrix nlocks
					ConstMatrixBlock design_matrix_block
						= m_design->set_predictor_level( g, range ).matrix( range ) ;

					MatrixBlock f1 = m_f1.block( storage_range.begin(), 0, storage_range.size(), 1 ) ;

					ConstMatrixBlock outcome = m_design->outcome().block( range.begin(), 0, range.size(), m_design->outcome().cols() ) ;
					assert( outcome.cols() == 2 ) ;

					MatrixBlock fxBlock = fx->block( storage_range.begin(), 0, storage_range.size(), L ) ;
					MatrixBlock dfxBlock = dfx->block( storage_range.begin(), 0, storage_range.size(), L ) ;
					MatrixBlock ddfxBlock = ddfx->block( storage_range.begin(), 0, storage_range.size(), L ) ;

					auto one = Vector::Ones( range.size() ) ;

#if DEBUG_LOGLIKELIHOOD
					std::cerr << "design(" << g << ") =\n"
						<< design_matrix_block.block( 0, 0, std::min( 10, int(design_matrix_block.rows())), design_matrix_block.cols() ) << ".\n" ;
					std::cerr << "outcome(" << g << ") =\n"
						<< outcome.block( 0, 0, std::min( 10, int(outcome.rows())), outcome.cols() ) << ".\n" ;
#endif
					f1 = (design_matrix_block * parameters).array().exp() ;
					f1 = f1.array() * ((one + f1).array().inverse()) ;

				// Likelihood is obtained by raising f1 and f0 to appropriate powers

#if DEBUG_LOGLIKELIHOOD
					std::cerr << "(*fx) = \n"
						<< fx->block( 0, 0, std::min( 10, int(fx->rows())), fx->cols() ) << ".\n" ;
					std::cerr << "f1 = \n"
						<< f1.block( 0, 0, std::min( 10, int(f1.rows())), f1.cols() ) << ".\n" ;
#endif
					fxBlock.col(g)
						= f1.array().pow( outcome.col(1).array() )
						* (one.array() - f1.array() ).pow( outcome.col(0).array() ) ;

#if DEBUG_LOGLIKELIHOOD
					std::cerr << "Computed f1 and fx, f1 = " << f1.block( 0, 0, std::min( 5, int(f1.rows())), 1 ).transpose() << ", \n"
						<< "fx(" << g << ") = " << fxBlock.col(g).block(0, 0, std::min( 5, int(f1.rows())), 1 ).transpose() << ".\n" ;
#endif

					if( numberOfDerivatives > 0 ) {
						// compute coefficient of x^t in 1st derivative
						// To share computation with 2nd derivative,
						// first compute the shared term, we multiple by fx below.

#if DEBUG_LOGLIKELIHOOD
						std::cerr << "    dfx: " << dfx->rows() << "x" << dfx->cols() << ".\n" ;
						std::cerr << "outcome: " << outcome.rows() << "x" << outcome.cols() << ".\n" ;
#endif
						dfxBlock.col(g)
							= ( outcome.col(1).array() * ( one - f1 ).array() - outcome.col(0).array() * f1.array() )
						;

#if DEBUG_LOGLIKELIHOOD
						std::cerr << "dfx(" << g << ") before fx term = "
							<< dfxBlock.col(g).segment(0, std::min( 5, int(dfxBlock.rows()) )).transpose() << ".\n" ;
#endif

						if( numberOfDerivatives > 1 ) {
							// compute coefficient of x ⊗ x^t in 2nd derivative
							ddfxBlock.col(g)
								= dfxBlock.col(g).array().square()
								- (
										(outcome.col(0) + outcome.col(1)).array()
										* (f1.array() * ( one.array() - f1.array() ))
								).array()
							;
							ddfxBlock.col(g).array() *= fxBlock.col(g).array() ;

#if DEBUG_LOGLIKELIHOOD
							std::cerr << "ddfx(" << g << ") = "
								<< ddfxBlock.col(g).segment(0, std::min( 5, int(ddfxBlock.rows()) )).transpose() << ".\n" ;
#endif
						}
						dfxBlock.col(g).array() *= fxBlock.col(g).array() ;

#if DEBUG_LOGLIKELIHOOD
						std::cerr << "dfx(" << g << ") = "
							<< dfxBlock.col(g).segment(0, std::min( 5, int(dfxBlock.rows()) )).transpose() << ".\n" ;
#endif
					}
				}
			}
		}

		void BinomialLogistic::compute_value_of_loglikelihood(
			Matrix const& hx,
			double* result
		) {
			(*result) = neumaier_sum(
				hx.block( 0, 0, m_number_of_stored_samples, hx.cols() )
				.rowwise().sum()
				.array().log()
			) ;
		}

		void BinomialLogistic::compute_value_of_first_derivative(
			Matrix const& normalisedDhx,
			Matrix* result_terms,
			Vector* result
		) {
			int const N = m_design->outcome().rows() ;
			int const D = m_design->matrix().cols() ;
			int const N_predictor_levels = m_design->get_number_of_predictor_levels() ;

			// Currently we assume
			// dimension of design matrix = dimension of parameters = dimension of derivative
			// TODO: More generally we may have more parameters, e.g. multinomial logistic or normal linear model in which case something different must be done.  (Probably, identify_parameters can be used to generalise to this case?)
			assert( D == m_parameters.size() ) ;
			assert( result_terms->rows() >= m_number_of_stored_samples && result_terms->cols() == D ) ;
			result_terms->setZero() ;

			for( int g = 0; g < N_predictor_levels; ++g ) {
				// At this point we have two sets of indexes - the originals, which index into the
				// design matrix rows, and the normalisedDhx which only contains evaluated samples.
				// Keep track of this with a counter.
				for( std::size_t i = 0; i < m_evaluated_samples.size(); ++i ) {
					SampleRange const range = m_evaluated_samples[i] ;
					SampleRange const stored_range = m_storage_samples[i] ;

					ConstMatrixBlock design_matrix_block
						= m_design->set_predictor_level( g, range ).matrix( range ) ;

					MatrixBlock termsBlock = result_terms->block(
						stored_range.begin(), 0,
						stored_range.size(), result_terms->cols()
					) ;

					ConstMatrixBlock normalisedDhxBlock = normalisedDhx.block(
						stored_range.begin(), 0,
						stored_range.size(), normalisedDhx.cols()
					) ;

					// Compute the contribution of these samples & this predictor level
					termsBlock +=
						normalisedDhxBlock.col(g)
						.asDiagonal()
						* design_matrix_block
					;

				}
			}

			MatrixBlock termsBlock = result_terms->block(
				0, 0,
				m_number_of_stored_samples, result_terms->cols()
			) ;

			// Now the rows of terms are the contribution of each sample to the derivative
			(*result) = termsBlock.colwise().sum() ;
		}

		void BinomialLogistic::compute_value_of_second_derivative(
			Matrix const& firstDerivativeTerms,
			Matrix const& normalisedDdhx,
			Matrix* result
		) {
			int const D = m_design->matrix().cols() ;
			int const N_predictor_levels = m_design->get_number_of_predictor_levels() ;

			// Currently we assume
			// dimension of design matrix = dimension of parameters = dimension of derivative
			// More generally we may have more parameters, e.g. multinomial logistic or normal linear model
			// in which case something different must be done.  (Probably, identify_parameters can be
			// used to generalise to this case?)
			assert( D == m_parameters.size() ) ;

			result->setZero( D, D ) ;

			for( int g1 = 0; g1 < N_predictor_levels; ++g1 ) {
				for( std::size_t i = 0; i < m_evaluated_samples.size(); ++i ) {
					SampleRange const& range = m_evaluated_samples[i] ;
					SampleRange const& stored_range = m_storage_samples[i] ;

					ConstMatrixBlock design_matrix_block = m_design->set_predictor_level( g1, range ).matrix( range ) ;

					// We are implementing the formula: sum_i (sum_x DD^t h_i)/(sum_x h_i) × x ⊗ x^t
					// Or, written in the way we compute it here, sum_g ( sum_i Z_i × x_i(g) ⊗ x_i(g)^t )
					// where Z_i = (DD^t h_i(g)) / h_i and x_i(g)^t is the design matrix row for sample i given predictor level g.
					// We let matrix multiplication do the sum over i for us, and compute this as
					// diag(Z) × X(g)^t ⊗ X(g)
					(*result) += (
							normalisedDdhx.col(g1).segment( stored_range.begin(), stored_range.size() ).asDiagonal()
							* design_matrix_block
						)
						.transpose()
						* design_matrix_block
					;
				}
			}

			ConstMatrixBlock firstDerivativeTermsBlock = firstDerivativeTerms.block(
				0, 0,
				m_number_of_stored_samples, firstDerivativeTerms.cols()
			) ;

			(*result) -= ( firstDerivativeTermsBlock.transpose() * firstDerivativeTermsBlock ) ;

#if DEBUG_LOGLIKELIHOOD
			std::cerr << "D = " << D << "\n" ;
			std::cerr << "first_derivative_terms: " << firstDerivativeTermsBlock.rows() << "x" << firstDerivativeTermsBlock.cols() << "\n" ;
			std::cerr << "result: " << result->rows() << "x" << result->cols() << "\n" ;
#endif
		}

		BinomialLogistic::Point const& BinomialLogistic::parameters() const {
			return m_parameters ;
		}

		double BinomialLogistic::get_value_of_function() const {
			return m_value_of_function ;
		}

		BinomialLogistic::Vector BinomialLogistic::get_value_of_first_derivative() const {
			return m_value_of_first_derivative ;
		}

		BinomialLogistic::Matrix BinomialLogistic::get_value_of_second_derivative() const {
			return m_value_of_second_derivative ;
		}
	}
}