// -----------------------------------------------------------------------------
/**
 *  @brief 2D homography motion model (function) in direct methods tracking.
 *  @author Jose M. Buenaposada
 *  @date 2012/10/14
 *  @version $revision$
 *
 *  $id$
 *
 *  Grupo de investigación en Percepción Computacional y Robótica)
 *  (Perception for Computers & Robots research Group)
 *  Facultad de Informática (Computer Science School)
 *  Universidad Politécnica de Madrid (UPM) (Madrid Technical University)
 *  http://www.dia.fi.upm.es/~pcr
 *
 */
// -----------------------------------------------------------------------------

#include "homography_2d.hpp"
#include "trace.hpp"
#include <limits>
#include <boost/concept_check.hpp>

namespace upm { namespace pcr
{

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
// -----------------------------------------------------------------------------
Homography2D::Homography2D
  ()
{
};

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
// -----------------------------------------------------------------------------
Homography2D::~Homography2D
  ()
{
};

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
// -------------------------------------------------------------------------
// cv::Mat
// Homography2D::computeMotionJacobian
//   (
//   cv::Mat params
//   )
// {
// };

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
// -----------------------------------------------------------------------------
cv::Mat
Homography2D::scaleInputImageResolution
  (
  cv::Mat params,
  double scale
  )
{
  cv::Mat newH;
  cv::Mat new_params = cv::Mat::eye(params.rows, 1, cv::DataType<MAT_TYPE>::type);
  cv::Mat H          = params.reshape(1,3).t();

  cv::Mat S    = cv::Mat::eye(3,3,cv::DataType<MAT_TYPE>::type);
  S.at<MAT_TYPE>(0,0) = scale;
  S.at<MAT_TYPE>(1,1) = scale;

  newH = S*H;
  newH = newH.t();
  newH.reshape(1,9).copyTo(new_params);

  return new_params;
}

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
//    The motion params are always from template to current image and with this
//    method we have to use the inverse motion model.
// -----------------------------------------------------------------------------
cv::Mat
Homography2D::transformCoordsToTemplate
  (
  cv::Mat coords,
  cv::Mat params
  )
{
  cv::Mat H = params.reshape(1,3).t();
  cv::Mat invH      = H.inv().t();

  cv::Mat inv_params = invH.reshape(1,9);

  return transformCoordsToImage(coords, inv_params);
};

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
// -----------------------------------------------------------------------------
cv::Mat
Homography2D::transformCoordsToImage
  (
  cv::Mat coords,
  cv::Mat params
  )
{
  assert(coords.cols == 2); // We need two dimensional coordinates

  cv::Mat H                      = params.reshape(1, 3).t();
  cv::Mat homogeneous_coords     = cv::Mat::ones(coords.rows, 3, cv::DataType<MAT_TYPE>::type);
  cv::Mat homogeneous_coords_ref = homogeneous_coords(cv::Range::all(), cv::Range(0, 2));
  coords.copyTo(homogeneous_coords_ref);

  cv::Mat homogeneous_new_coords = (homogeneous_coords * H.t());

  // Divide by the third homogeneous coordinates to get the cartersian coordinates.
  for (int j=0; j<3; j++)
  {
    cv::Mat col     = homogeneous_new_coords.col(j).mul(1.0 / homogeneous_new_coords.col(2));
    cv::Mat col_new = homogeneous_new_coords.col(j);
    col.copyTo(col_new);
  }

  cv::Mat homogeneous_new_coords_ref = homogeneous_new_coords(cv::Range::all(), cv::Range(0, 2));
  cv::Mat new_coords;
  homogeneous_new_coords_ref.copyTo(new_coords);

#ifdef DEBUG
  // write Mat objects to the file
  cv::FileStorage fs("transformCoordsToImage.xml", cv::FileStorage::WRITE);
  fs << "H" << H;
  fs << "coords" << coords;
  fs << "new_coords" << new_coords;
  fs.release();
#endif

  return new_coords;
};

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
// -----------------------------------------------------------------------------
cv::Mat
Homography2D::warpImage
  (
  cv::Mat image,
  cv::Mat params,
  cv::Mat template_coords,
  std::vector<int>& template_ctrl_points_indices
  )
{
  MAT_TYPE min_x, max_x, min_y, max_y;
  cv::Mat warped_image;

  cv::Mat M;
  cv::Mat H = params.reshape(1,3).t();

  // Find minimum x and minimum y in template coords
  cv::MatConstIterator_<MAT_TYPE> it;
  max_x = std::numeric_limits<MAT_TYPE>::min();
  max_y = std::numeric_limits<MAT_TYPE>::min();
  min_x = std::numeric_limits<MAT_TYPE>::max();
  min_y = std::numeric_limits<MAT_TYPE>::max();

  for (int i = 0; i < template_coords.rows; i++)
  {
    MAT_TYPE x = template_coords.at<MAT_TYPE>(i,0);
    MAT_TYPE y = template_coords.at<MAT_TYPE>(i,1);

    if (x > max_x) max_x = x;
    if (x < min_x) min_x = x;
    if (y > max_y) max_y = y;
    if (y < min_y) min_y = y;
  }

  cv::Mat TR  = (cv::Mat_<MAT_TYPE>(3,3) << 1.,    0,    min_x,
		                            0,    1.,    min_y,
		                            0,     0,    1.);
  warped_image = cv::Mat::zeros(max_y-min_y+1, max_x-min_x+1, cv::DataType<uint8_t>::type);

//   if (scale < 0.000000001)
//   {
//     return warped_image;
//   }

  // TR is necessary because the Warpers do warping taking
  // the pixel (0,0) as the left and top most pixel of the template.
  // So, we have move the (0,0) to the center of the Template.
  M = H*TR;

#ifdef DEBUG
  // write Mat objects to the file
  cv::FileStorage fs("template_coords_warpImage.xml", cv::FileStorage::WRITE);
  fs << "template_coords" << template_coords;
  fs << "M" << M;
  fs.release();
#endif

  cv::warpPerspective(image, warped_image, M,
		      cv::Size(warped_image.cols,  warped_image.rows),
		      cv::INTER_AREA | cv::WARP_INVERSE_MAP);
  return warped_image;
};

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
// -----------------------------------------------------------------------------
bool
Homography2D::consistentPoints
  (
  cv::Mat points,
  cv::Mat transformed_points,
  int t1,
  int t2,
  int t3
  )
{
  cv::Mat A = (cv::Mat_<MAT_TYPE>(3, 3) << points.at<MAT_TYPE>(t1, 0), points.at<MAT_TYPE>(t1, 1), 1,
                                           points.at<MAT_TYPE>(t2, 0), points.at<MAT_TYPE>(t2, 1), 1,
                                           points.at<MAT_TYPE>(t3, 0), points.at<MAT_TYPE>(t3, 1), 1);
  cv::Mat B = (cv::Mat_<MAT_TYPE>(3, 3) << transformed_points.at<MAT_TYPE>(t1, 0), transformed_points.at<MAT_TYPE>(t1, 1), 1,
                                           transformed_points.at<MAT_TYPE>(t2, 0), transformed_points.at<MAT_TYPE>(t2, 1), 1,
                                           transformed_points.at<MAT_TYPE>(t3, 0), transformed_points.at<MAT_TYPE>(t3, 1), 1);

  double detA = cv::determinant(A);
  double detB = cv::determinant(B);

  return ((detA*detB) >= 0.0);
}

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
// -----------------------------------------------------------------------------
bool
Homography2D::invalidParams
  (
  cv::Mat params
  )
{
  cv::Mat M;
  cv::Mat H = params.reshape(1,3).t();

  cv::SVD svd(H, cv::SVD::NO_UV);

  int rank = 0;
  for (int i=0; i<H.rows; i++)
  {
    if (svd.w.at<MAT_TYPE>(i,0)>1.E-6)
    {
      rank++;
    }
  }

  SHOW_VALUE(H);

  // Stablish the
  cv::Mat points = (cv::Mat_<MAT_TYPE>(4, 2) <<   0, 0,
                                                100, 0,
		                                100, 100,
		                                  0, 100);
  cv::Mat transformed_points;
  points.copyTo(transformed_points);
//   cv::perspectiveTransform(points, transformed_points, H);
  transformed_points = transformCoordsToImage(points, params);

  double detH = cv::determinant(H);

//   return (rank<3) || (!consistentPoints(points, transformed_points, 0, 1, 2))
//                   || (!consistentPoints(points, transformed_points, 1, 2, 3))
//                   || (!consistentPoints(points, transformed_points, 0, 2, 3))
//                   || (!consistentPoints(points, transformed_points, 0, 1, 3));
  return (rank<3) ||(detH < (1./10.)) || (detH > 10.)
                  || (!consistentPoints(points, transformed_points, 0, 1, 2))
                  || (!consistentPoints(points, transformed_points, 1, 2, 3))
                  || (!consistentPoints(points, transformed_points, 0, 2, 3))
                  || (!consistentPoints(points, transformed_points, 0, 1, 3));
};

}; }; // namespace


/* Copyright (c) 2017, Waterloo Autonomous Vehicles Laboratory (WAVELab),
 * Waterloo Intelligent Systems Engineering Lab (WISELab),
 * University of Waterloo.
 *
 * Refer to the accompanying LICENSE file for license information.
 *
 * ############################################################################
 ******************************************************************************
 |                                                                            |
 |                         /\/\__/\_/\      /\_/\__/\/\                       |
 |                         \          \____/          /                       |
 |                          '----________________----'                        |
 |                              /                \                            |
 |                            O/_____/_______/____\O                          |
 |                            /____________________\                          |
 |                           /    (#UNIVERSITY#)    \                         |
 |                           |[**](#OFWATERLOO#)[**]|                         |
 |                           \______________________/                         |
 |                            |_""__|_,----,_|__""_|                          |
 |                            ! !                ! !                          |
 |                            '-'                '-'                          |
 |       __    _   _  _____  ___  __  _  ___  _    _  ___  ___   ____  ____   |
 |      /  \  | | | ||_   _|/ _ \|  \| |/ _ \| \  / |/ _ \/ _ \ /     |       |
 |     / /\ \ | |_| |  | |  ||_||| |\  |||_|||  \/  |||_||||_|| \===\ |====   |
 |    /_/  \_\|_____|  |_|  \___/|_| \_|\___/|_|\/|_|\___/\___/ ____/ |____   |
 |                                                                            |
 ******************************************************************************
 * ############################################################################
 *
 * File: world_frame_conversions.cpp
 * Desc: Implementation file for world frame conversion functions
 * Auth: Michael Smart <michael.smart@uwaterloo.ca>
 *
 * ############################################################################
*/

#include <Eigen/Core>
#include "wave/geography/world_frame_conversions.hpp"

namespace wave {

void ecefPointFromLLH(const double llh[3], double ecef[3]) {
    double latitude = llh[0], longitude = llh[1], height = llh[2];

    GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84();

    double X, Y, Z;
    earth.Forward(latitude, longitude, height, X, Y, Z);

    ecef[0] = X;
    ecef[1] = Y;
    ecef[2] = Z;
}

void llhPointFromECEF(const double ecef[3], double llh[3]) {
    double X = ecef[0], Y = ecef[1], Z = ecef[2];

    GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84();

    double latitude, longitude, height;
    earth.Reverse(X, Y, Z, latitude, longitude, height);

    llh[0] = latitude;
    llh[1] = longitude;
    llh[2] = height;
}

void ecefFromENUTransformMatrix(const double datum[3],
                                double T_ecef_enu[4][4],
                                bool datum_is_llh) {
    // Both Forward() and Reverse() return the same rotation matrix from ENU
    // to ECEF
    std::vector<double> R_ecef_enu(9, 0.0);
    GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84();
    double datum_X, datum_Y, datum_Z;

    if (datum_is_llh) {
        double latitude = datum[0], longitude = datum[1], height = datum[2];

        earth.Forward(
          latitude, longitude, height, datum_X, datum_Y, datum_Z, R_ecef_enu);
    } else {
        // Datum is already given in ECEF
        datum_X = datum[0];
        datum_Y = datum[1];
        datum_Z = datum[2];

        double latitude, longitude, height;
        earth.Reverse(
          datum_X, datum_Y, datum_Z, latitude, longitude, height, R_ecef_enu);
    }

    T_ecef_enu[0][0] = R_ecef_enu[0];
    T_ecef_enu[0][1] = R_ecef_enu[1];
    T_ecef_enu[0][2] = R_ecef_enu[2];
    T_ecef_enu[0][3] = datum_X;
    T_ecef_enu[1][0] = R_ecef_enu[3];
    T_ecef_enu[1][1] = R_ecef_enu[4];
    T_ecef_enu[1][2] = R_ecef_enu[5];
    T_ecef_enu[1][3] = datum_Y;
    T_ecef_enu[2][0] = R_ecef_enu[6];
    T_ecef_enu[2][1] = R_ecef_enu[7];
    T_ecef_enu[2][2] = R_ecef_enu[8];
    T_ecef_enu[2][3] = datum_Z;
    T_ecef_enu[3][0] = 0.0;
    T_ecef_enu[3][1] = 0.0;
    T_ecef_enu[3][2] = 0.0;
    T_ecef_enu[3][3] = 1.0;
}

void enuFromECEFTransformMatrix(const double datum[3],
                                double T_enu_ecef[4][4],
                                bool datum_is_llh) {
    // Get T_ecef_enu and then invert it
    double T_ecef_enu[4][4];
    ecefFromENUTransformMatrix(datum, T_ecef_enu, datum_is_llh);

    // Affine inverse: [R | t]^(-1) = [ R^T | - R^T * t]
    // TODO(msmart/benskikos) - Move these functions to somewhere where we have
    // matrix classes and ownership since it is ours. Manual transposition is
    // undesirable. The below should be 2 lines:
    // R_new = R.transpose(); t_new = - R.transpose()*t;
    T_enu_ecef[0][0] = T_ecef_enu[0][0];
    T_enu_ecef[0][1] = T_ecef_enu[1][0];
    T_enu_ecef[0][2] = T_ecef_enu[2][0];
    T_enu_ecef[1][0] = T_ecef_enu[0][1];
    T_enu_ecef[1][1] = T_ecef_enu[1][1];
    T_enu_ecef[1][2] = T_ecef_enu[2][1];
    T_enu_ecef[2][0] = T_ecef_enu[0][2];
    T_enu_ecef[2][1] = T_ecef_enu[1][2];
    T_enu_ecef[2][2] = T_ecef_enu[2][2];

    // Affine inverse translation component: -R_inverse * b
    //    with b as the 4th column of T_ecef_enu
    T_enu_ecef[0][3] = -T_enu_ecef[0][0] * T_ecef_enu[0][3]    //
                       - T_enu_ecef[0][1] * T_ecef_enu[1][3]   //
                       - T_enu_ecef[0][2] * T_ecef_enu[2][3];  //

    T_enu_ecef[1][3] = -T_enu_ecef[1][0] * T_ecef_enu[0][3]    //
                       - T_enu_ecef[1][1] * T_ecef_enu[1][3]   //
                       - T_enu_ecef[1][2] * T_ecef_enu[2][3];  //

    T_enu_ecef[2][3] = -T_enu_ecef[2][0] * T_ecef_enu[0][3]    //
                       - T_enu_ecef[2][1] * T_ecef_enu[1][3]   //
                       - T_enu_ecef[2][2] * T_ecef_enu[2][3];  //

    // Last row is the same
    T_enu_ecef[3][0] = 0.0;
    T_enu_ecef[3][1] = 0.0;
    T_enu_ecef[3][2] = 0.0;
    T_enu_ecef[3][3] = 1.0;
}

void enuPointFromLLH(const double point_llh[3],
                     const double enu_datum[3],
                     double point_enu[3],
                     bool datum_is_llh) {
    double enu_datum_llh[3];
    if (datum_is_llh) {
        enu_datum_llh[0] = enu_datum[0];
        enu_datum_llh[1] = enu_datum[1];
        enu_datum_llh[2] = enu_datum[2];
    } else {
        // Datum is ECEF
        llhPointFromECEF(enu_datum, enu_datum_llh);
    }

    GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84();
    GeographicLib::LocalCartesian localENU(
      enu_datum_llh[0], enu_datum_llh[1], enu_datum_llh[2], earth);

    localENU.Forward(point_llh[0],
                     point_llh[1],
                     point_llh[2],
                     point_enu[0],
                     point_enu[1],
                     point_enu[2]);
}

void llhPointFromENU(const double point_enu[3],
                     const double enu_datum[3],
                     double point_llh[3],
                     bool datum_is_llh) {
    double enu_datum_llh[3];
    if (datum_is_llh) {
        enu_datum_llh[0] = enu_datum[0];
        enu_datum_llh[1] = enu_datum[1];
        enu_datum_llh[2] = enu_datum[2];
    } else {
        // Datum is ECEF
        llhPointFromECEF(enu_datum, enu_datum_llh);
    }

    GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84();
    GeographicLib::LocalCartesian localENU(
      enu_datum_llh[0], enu_datum_llh[1], enu_datum_llh[2], earth);

    localENU.Reverse(point_enu[0],
                     point_enu[1],
                     point_enu[2],
                     point_llh[0],
                     point_llh[1],
                     point_llh[2]);
}

}  // namespace wave


#include "TensorTopologyEvaluator.hh"

#include <Eigen/Geometry>

#include <boost/algorithm/cxx11/any_of.hpp>
#include <boost/range/algorithm/max_element.hpp>
#include <boost/range/algorithm/min_element.hpp>

#include <tuple>
#include <utility>
#include <vector>

using namespace cpp_utils;

namespace tl
{
template <typename T, std::size_t... Degrees>
using TPBT = TensorProductBezierTriangle<T, double, Degrees...>;

std::array<TPBT<double, 3, 0>, 7> tensorTopologyCoeffs(const TensorInterp& t)
{
    using Coords = TPBT<double, 3, 0>::Coords;
    // Constraint functions according to Zheng et al. 2004

    auto fx = [&](const Coords& coords) -> double {
        auto tv = t(coords.head<3>());
        return tv(0, 0) * ((tv(1, 1) * tv(1, 1) - tv(2, 2) * tv(2, 2))
                           + (tv(0, 1) * tv(0, 1) - tv(0, 2) * tv(0, 2)))
               + tv(1, 1) * ((tv(2, 2) * tv(2, 2) - tv(0, 0) * tv(0, 0))
                             + (tv(1, 2) * tv(1, 2) - tv(0, 1) * tv(0, 1)))
               + tv(2, 2) * ((tv(0, 0) * tv(0, 0) - tv(1, 1) * tv(1, 1))
                             + (tv(0, 2) * tv(0, 2) - tv(1, 2) * tv(1, 2)));
    };

    auto fy1 = [&](const Coords& coords) -> double {
        auto tv = t(coords.head<3>());
        return tv(1, 2) * (2 * (tv(1, 2) * tv(1, 2) - tv(0, 0) * tv(0, 0))
                           - (tv(0, 2) * tv(0, 2) + tv(0, 1) * tv(0, 1))
                           + 2 * (tv(1, 1) * tv(0, 0) + tv(2, 2) * tv(0, 0)
                                  - tv(1, 1) * tv(2, 2)))
               + tv(0, 1) * tv(0, 2) * (2 * tv(0, 0) - tv(2, 2) - tv(1, 1));
    };

    auto fy2 = [&](const Coords& coords) -> double {
        auto tv = t(coords.head<3>());
        return tv(0, 2) * (2 * (tv(0, 2) * tv(0, 2) - tv(1, 1) * tv(1, 1))
                           - (tv(0, 1) * tv(0, 1) + tv(1, 2) * tv(1, 2))
                           + 2 * (tv(2, 2) * tv(1, 1) + tv(0, 0) * tv(1, 1)
                                  - tv(2, 2) * tv(0, 0)))
               + tv(1, 2) * tv(0, 1) * (2 * tv(1, 1) - tv(0, 0) - tv(2, 2));
    };

    auto fy3 = [&](const Coords& coords) -> double {
        auto tv = t(coords.head<3>());
        return tv(0, 1) * (2 * (tv(0, 1) * tv(0, 1) - tv(2, 2) * tv(2, 2))
                           - (tv(1, 2) * tv(1, 2) + tv(0, 2) * tv(0, 2))
                           + 2 * (tv(0, 0) * tv(2, 2) + tv(1, 1) * tv(2, 2)
                                  - tv(0, 0) * tv(1, 1)))
               + tv(0, 2) * tv(1, 2) * (2 * tv(2, 2) - tv(1, 1) - tv(0, 0));
    };

    auto fz1 = [&](const Coords& coords) -> double {
        auto tv = t(coords.head<3>());
        return tv(1, 2) * (tv(0, 2) * tv(0, 2) - tv(0, 1) * tv(0, 1))
               + tv(0, 1) * tv(0, 2) * (tv(1, 1) - tv(2, 2));
    };

    auto fz2 = [&](const Coords& coords) -> double {
        auto tv = t(coords.head<3>());
        return tv(0, 2) * (tv(0, 1) * tv(0, 1) - tv(1, 2) * tv(1, 2))
               + tv(1, 2) * tv(0, 1) * (tv(2, 2) - tv(0, 0));
    };

    auto fz3 = [&](const Coords& coords) -> double {
        auto tv = t(coords.head<3>());
        return tv(0, 1) * (tv(1, 2) * tv(1, 2) - tv(0, 2) * tv(0, 2))
               + tv(0, 2) * tv(1, 2) * (tv(0, 0) - tv(1, 1));
    };

    return {TPBT<double, 3, 0>{fx},
            TPBT<double, 3, 0>{fy1},
            TPBT<double, 3, 0>{fy2},
            TPBT<double, 3, 0>{fy3},
            TPBT<double, 3, 0>{fz1},
            TPBT<double, 3, 0>{fz2},
            TPBT<double, 3, 0>{fz3}};
}

using TSHE = TensorTopologyEvaluator;

TSHE::TensorTopologyEvaluator(const DoubleTri& tri,
                              const TensorInterp& t,
                              const Options& opts)
        : _tri(tri), _target_funcs(tensorTopologyCoeffs(t)), _opts(opts)
{
}

std::array<TSHE, 4> TSHE::split() const
{
    auto part = [&](std::size_t i) {
        return TensorTopologyEvaluator(_tri.split<0>(i),
                                       {_target_funcs[0].split<0>(i),
                                        _target_funcs[1].split<0>(i),
                                        _target_funcs[2].split<0>(i),
                                        _target_funcs[3].split<0>(i),
                                        _target_funcs[4].split<0>(i),
                                        _target_funcs[5].split<0>(i),
                                        _target_funcs[6].split<0>(i)},
                                       _split_level + 1,
                                       _opts);
    };
    return {part(0), part(1), part(2), part(3)};
}

Result TSHE::eval()
{
    // Check if any of the error components can not become zero in the
    // current subdivision triangles
    auto has_nonzero =
            boost::algorithm::any_of(_target_funcs, [](const auto& c) {
                return sameSign(c.coefficients()) != 0;
            });

    // Discard triangles if no roots can occur inside
    if(has_nonzero)
    {
        return Result::Discard;
    }

    // Compute upper bound for target functions
    auto max_error = abs_max_upper_bound(_target_funcs);

    if(max_error < _opts.tolerance)
    {
        return Result::Accept;
    }

    return Result::Split;
}

double TSHE::error() const
{
    return upper_bound_norm(_target_funcs);
}

double distance(const TSHE& t1, const TSHE& t2)
{
    return distance(t1.tris(), t2.tris());
}

bool operator==(const TSHE& t1, const TSHE& t2)
{
    return t1._tri == t2._tri && t1._target_funcs == t2._target_funcs
           && t1._split_level == t2._split_level && t1._opts == t2._opts;
}

bool operator!=(const TSHE& t1, const TSHE& t2)
{
    return !(t1 == t2);
}
}


/*
Copyright 2013 Henrik Mühe and Florian Funke

This file is part of CampersCoreBurner.

CampersCoreBurner is free software: you can redistribute it and/or modify
it under the terms of the GNU Affero General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

CampersCoreBurner is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Affero General Public License for more details.

You should have received a copy of the GNU Affero General Public License
along with CampersCoreBurner.  If not, see <http://www.gnu.org/licenses/>.
*/

/*
COMMENT SUMMARY FOR THIS FILE:

This implements two histogram (character frequency) based filters. The histogram of a
word is an array that gives the number of times each possible letter in the alphabet
occurs inside a certain word. For the word "food", the array would be

a b c d e f g h ... o ... z
0 0 0 1 0 1 0 0 ... 2 ... 0

We define the delta d between two such histogram arrays as the sum of all absolute
differences of the counts of each pair of characters. Shortened example, see deltaSlow()
for the algorithm:

vec a 0 0 1 3 2 1
vec b 0 1 0 3 2 0
delta 0+1+1+0+0+1

The filter can be used for exact matches, hamming distance and edit distance although
it is only beneficial for edit distnace as matching exact words and hamming distance if
comparatively cheap anyway. For two words to be within edit distance t, their frequency
delta must be <= 2*t. This can be improved by looking at the possible edit operations. If
the length of the two words differs, that can only be caused by insert or delete operations.
These operations change the delta by 1, not 2 like substitution. Therefore, the criterion can
be improved to <= 2*t-lengthdiff where lengthdiff is the absolute length difference
between the two words.

While not overly costly, the naive way of executing the filter does not yield a sufficient
result. We therefore use SSE instructions to impove the filter by about a factor of 3. Also,
we have a shortened version of the filter which combines some characters. Its pruning
capability is not as good as the full version of the filter but it can be executed on one
sse register instead of two.
*/

#pragma once

#include <boost/utility/string_ref.hpp>
#include <cassert>
#include <x86intrin.h>

namespace campers {

/// The full 26 character frequency filter
struct Frequency {
    /// Maps each letter in the alphabet to a slot inside two sse 16 byte registers. A good mapping for english words
    /// was obtained by randomized search while monitoring the prune rate using a script such as:
    /// #!/bin/bash
    /// while true; do
    ///     seq 0 31 | sort -R | head -n 26 | xargs | tr ' ' ',' > include/mapping.hpp
    ///     make -j >/dev/null 2>&1 && ./driver test_data/inter* | grep prune | awk '{ print $2 }' | tr '\n' ' ' | cat - include/mapping.hpp
    /// done
    ///
    /// Alternatively, a good but not ideal mapping is simply mapping a-m to the first register and n-z to the second.
    static constexpr unsigned char charMap[26]={ 24,26,22,2,13,20,14,1,30,27,23,16,5,6,8,4,3,9,25,7,29,28,31,17,21,15 };
    /// Union trick. We use two sse 128 bit registers to represent 32 char values for 26 characters
    union { uint64_t l[4]; unsigned char f[32]; __m128i p[2]; };

    /// Comparison for adding Frequency to maps
    bool operator<(const Frequency& other) const {
        return std::lexicographical_compare(l,l+4,other.l,other.l+4);
    }

    /// Comparison for adding Frequency to hash maps
    bool operator==(const Frequency& other) const {
        return l[0]==other.l[0]&&l[1]==other.l[1]&&l[2]==other.l[2]&&l[3]==other.l[3];
    }

    /// Constructor
    Frequency() {}

    /// Constructor which builds the frequency vector for a given word
    explicit Frequency(const boost::string_ref& word) {
        for (unsigned index=0;index!=32;++index) f[index]=0;
        for (unsigned index=0;index<word.length();++index)
            ++f[charMap[word[index]-'a']];
    }

    /// Delta computation using the naive mechanism adding the absolute of each character count difference
    unsigned deltaSlow(const Frequency& f2) const {
        unsigned result=0; for (unsigned index=0;index!=26;++index)
        if (f[index]<f2.f[index])
            result+=f2.f[index]-f[index];
        else
            result+=f[index]-f2.f[index];
        return result;
    }

    /// Get coefficient
    unsigned getCoefficient() const {
        auto sum1=_mm_sad_epu8(p[0],_mm_setzero_si128());
        auto fsum1=_mm_extract_epi64(_mm_add_epi8(sum1,_mm_srli_si128(sum1,8)),0);
        auto sum2=_mm_sad_epu8(p[1],_mm_setzero_si128());
        auto fsum2=_mm_extract_epi64(_mm_add_epi8(sum2,_mm_srli_si128(sum2,8)),0);
        return 32+fsum1-fsum2;
    }

    /// Delta computation using sse, the sse instruction compute the same result as the naive approach but
    /// in less cycles.
    unsigned delta(const Frequency& f2) const {
        __m128i half=_mm_add_epi8(_mm_sad_epu8(p[0],f2.p[0]),_mm_sad_epu8(p[1],f2.p[1]));
        return _mm_extract_epi64(_mm_add_epi8(half,_mm_srli_si128(half,8)),0);
    }
};

/// The frequency filter which maps 26 characters on 16 slots so that it can be evaluated on one sse
/// register instead of two. The mapping is constructed from a dictionary such that letters which do
/// not occur together frequently are mapped to the same slot.
struct FrequencyFast {
    /// A mapping compacting the 26 characters of the alphabet into 16 slots of one sse register. The mapping
    /// was extracted similarly to that of the regular Frequency filter.
    static constexpr unsigned char charMap[26]={13,0,1,14,9,2,3,4,5,5,6,0,7,15,2,8,9,12,11,6,10,11,12,13,14,15};
    /// Union trick. We use two sse 128 bit registers to represent 32 char values for 26 characters
    union { uint64_t l[2]; unsigned char f[16]; __m128i p; };

    /// Comparison for adding Frequency to maps
    bool operator<(const FrequencyFast& other) const {
        return _mm_extract_epi64(p,1) < _mm_extract_epi64(other.p,1) ||
        (_mm_extract_epi64(p,1) == _mm_extract_epi64(other.p,1) && _mm_extract_epi64(p,0) < _mm_extract_epi64(other.p,0));
    }

    /// Comparison for adding Frequency to hash maps
    bool operator==(const FrequencyFast& other) const {
        return l[0]==other.l[0]&&l[1]==other.l[1];
    }

    /// Constructor
    FrequencyFast() {}

    /// Constructor which builds the frequency vector for a given word
    explicit FrequencyFast(const boost::string_ref& word) {
        for (unsigned index=0;index!=16.;++index) f[index]=0;
        for (unsigned index=0;index<word.length();++index)
            ++f[charMap[word[index]-'a']];
    }

    /// Delta computation using the naive mechanism adding the absolute of each character count difference
    unsigned deltaSlow(const FrequencyFast& f2) const {
        unsigned result=0; for (unsigned index=0;index!=16;++index)
        if (f[index]<f2.f[index])
            result+=f2.f[index]-f[index];
        else
            result+=f[index]-f2.f[index];
        return result;
    }

    /// Delta computation using sse, the sse instruction compute the same result as the naive approach but
    /// in less cycles.
    unsigned delta(const FrequencyFast& f2) const {
        __m128i half=_mm_sad_epu8(p,f2.p);
        return _mm_extract_epi64(_mm_add_epi8(half,_mm_srli_si128(half,8)),0);
    }
};

}

namespace std {

/// Hash for FrequencyFast crc
template<>
struct hash<campers::FrequencyFast> {
    size_t operator()(const campers::FrequencyFast& f) const {
        return _mm_crc32_u64(_mm_crc32_u64(0,reinterpret_cast<const uint64_t*>(&f)[0]),reinterpret_cast<const uint64_t*>(&f)[1]);
    }
};

/// Hash for Frequency using crc
template<>
struct hash<campers::Frequency> {
    size_t operator()(const campers::Frequency& f) const {
        return _mm_crc32_u64(_mm_crc32_u64(_mm_crc32_u64(_mm_crc32_u64(0,reinterpret_cast<const uint64_t*>(&f)[0]),
            reinterpret_cast<const uint64_t*>(&f)[1]),reinterpret_cast<const uint64_t*>(&f)[2]),(reinterpret_cast<const uint64_t*>(&f)[3]));
    }
};
}


#include <Eigen/Dense>
#include <aslam/Exceptions.hpp>
#include <aslam/cameras/OmniCameraGeometry.hpp>

namespace aslam {
namespace cameras {

OmniCameraGeometry::OmniCameraGeometry() { *this = createTestGeometry(); }

OmniCameraGeometry::OmniCameraGeometry(double xi, double k1, double k2, double p1, double p2, double gamma1,
                                       double gamma2, double u0, double v0, int width, int height)
    : _xi(xi),
      _k1(k1),
      _k2(k2),
      _p1(p1),
      _p2(p2),
      _gamma1(gamma1),
      _gamma2(gamma2),
      _u0(u0),
      _v0(v0),
      _width(width),
      _height(height) {
    updateTemporaries();
}

OmniCameraGeometry::~OmniCameraGeometry() {}

// This updates the intrinsic parameters with a small step: i <-- i + di
// The Jacobians above are with respect to this update function.
void OmniCameraGeometry::updateIntrinsicsOplus(double* di) {
    _xi += di[0];
    _k1 += di[1];
    _k2 += di[2];
    _p1 += di[3];
    _p2 += di[4];
    _gamma1 += di[5];
    _gamma2 += di[6];
    _u0 += di[7];
    _v0 += di[8];
    updateTemporaries();
}

// The amount of time elapsed between the start of the image and the
// keypoint. For a global shutter camera, this can return Duration(0).
Duration OmniCameraGeometry::temporalOffset(const keypoint_t& keypoint) const { return Duration(0); }

OmniCameraGeometry::keypoint_t OmniCameraGeometry::maxKeypoint() const { return keypoint_t(_width, _height); }

OmniCameraGeometry::keypoint_t OmniCameraGeometry::minKeypoint() const { return keypoint_t(0, 0); }

std::string OmniCameraGeometry::typeName() const { return "OmniCamera"; }

OmniCameraGeometry OmniCameraGeometry::createTestGeometry() {
    return OmniCameraGeometry(1.2080, -2.103030975849235e-01, 1.511327079893021e-02, 6.035288960688402e-04,
                              -5.277839371727245e-04, 5.671146836756682e+02, 5.665554204210736e+02,
                              6.196542975873804e+02, 5.106649931478771e+02, 1224, 1024);
}

/**
 * \brief Lifts a point from the image plane to the unit sphere
 *
 * \param u u image coordinate
 * \param v v image coordinate
 * \param X X coordinate of the point on the sphere
 * \param Y Y coordinate of the point on the sphere
 * \param Z Z coordinate of the point on the sphere
 */
void OmniCameraGeometry::lift_sphere(double u, double v, double* X, double* Y, double* Z) const {
    double mx_d, my_d, mx_u, my_u;
    double lambda;

    // Lift points to normalised plane
    // Matlab points start at 1 (calibration)
    mx_d = _inv_K11 * (u) + _inv_K13;
    my_d = _inv_K22 * (v) + _inv_K23;

    // Recursive distortion model
    // int n = 6;
    // double dx_u, dy_u;
    // distortion(mx_d,my_d,&dx_u,&dy_u);
    // // Approximate value
    // mx_u = mx_d-dx_u;
    // my_u = my_d-dy_u;

    // for(int i=1;i<n;i++) {
    // 	distortion(mx_u,my_u,&dx_u,&dy_u);
    // 	mx_u = mx_d-dx_u;
    // 	my_u = my_d-dy_u;
    // }
    // PTF: March 31, 2012. The above was not that accurate.
    //      Substitute Gauss-Newton.
    undistortGN(mx_d, my_d, &mx_u, &my_u);

    // Lift normalised points to the sphere (inv_hslash)
    lambda = (_xi + sqrt(1 + (1 - _xi * _xi) * (mx_u * mx_u + my_u * my_u))) / (1 + mx_u * mx_u + my_u * my_u);
    *X = lambda * mx_u;
    *Y = lambda * my_u;
    *Z = lambda - _xi;
}

/**
 * \brief Lifts a point from the image plane to its projective ray
 *
 * \param u u image coordinate
 * \param v v image coordinate
 * \param X X coordinate of the projective ray
 * \param Y Y coordinate of the projective ray
 * \param Z Z coordinate of the projective ray
 */
void OmniCameraGeometry::lift_projective(double u, double v, double* X, double* Y, double* Z) const {
    double mx_d, my_d, mx_u, my_u;
    double rho2_d;

    // Lift points to normalised plane
    // Matlab points start at 1 (calibration)
    mx_d = _inv_K11 * (u) + _inv_K13;
    my_d = _inv_K22 * (v) + _inv_K23;

    // Recursive distortion model
    // int n = 8;
    // double dx_u, dy_u;
    // distortion(mx_d,my_d,&dx_u,&dy_u);
    // // Approximate value
    // mx_u = mx_d-dx_u;
    // my_u = my_d-dy_u;

    // for(int i=1;i<n;i++) {
    // 	distortion(mx_u,my_u,&dx_u,&dy_u);
    // 	mx_u = mx_d-dx_u;
    // 	my_u = my_d-dy_u;
    // }
    // PTF: March 31, 2012. The above was not that accurate.
    //      Substitute Gauss-Newton.
    undistortGN(mx_d, my_d, &mx_u, &my_u);

    // std::cout << "lift projective: u: " << mx_u << ", v: " << my_u << std::endl;

    // Obtain a projective ray
    // Reuse variable
    rho2_d = mx_u * mx_u + my_u * my_u;
    *X = mx_u;
    *Y = my_u;
    *Z = 1 - _xi * (rho2_d + 1) / (_xi + sqrt(1 + (1 - _xi * _xi) * rho2_d));

    // std::cout << "lift projective: p: " << *X << ", " << *Y << ", " << *Z << std::endl;
}

/**
 * \brief Project a 3D points (\a x,\a y,\a z) to the image plane in (\a u,\a v)
 *
 * \param x 3D point x coordinate
 * \param y 3D point y coordinate
 * \param z 3D point z coordinate
 * \param u return value, contains the image point u coordinate
 * \param v return value, contains the image point v coordinate
 */
void OmniCameraGeometry::space2plane(double x, double y, double z, double* u, double* v) const {
    double mx_u, my_u, mx_d, my_d;

    // Project points to the normalised plane
    z = z + _xi * sqrt(x * x + y * y + z * z);
    mx_u = x / z;
    my_u = y / z;

    // Apply distortion
    double dx_u, dy_u;
    distortion(mx_u, my_u, &dx_u, &dy_u);
    mx_d = mx_u + dx_u;
    my_d = my_u + dy_u;

    // Apply generalised projection matrix
    // Matlab points start at 1
    *u = _gamma1 * mx_d + _u0;
    *v = _gamma2 * my_d + _v0;
}

/**
 * \brief Project a 3D points (\a x,\a y,\a z) to the image plane in (\a u,\a v)
 *        and calculate jacobian
 *
 * \param x 3D point x coordinate
 * \param y 3D point y coordinate
 * \param z 3D point z coordinate
 * \param u return value, contains the image point u coordinate
 * \param v return value, contains the image point v coordinate
 */
void OmniCameraGeometry::space2plane(double x, double y, double z, double* u, double* v, double* dudx, double* dvdx,
                                     double* dudy, double* dvdy, double* dudz, double* dvdz) const {
    double mx_u, my_u, mx_d, my_d;
    double norm, inv_denom;
    double dxdmx, dydmx, dxdmy, dydmy;

    norm = sqrt(x * x + y * y + z * z);
    // Project points to the normalised plane
    inv_denom = 1 / (z + _xi * norm);
    mx_u = inv_denom * x;
    my_u = inv_denom * y;

    // Calculate jacobian
    inv_denom = inv_denom * inv_denom / norm;
    *dudx = inv_denom * (norm * z + _xi * (y * y + z * z));
    *dvdx = -inv_denom * _xi * x * y;
    *dudy = *dvdx;
    *dvdy = inv_denom * (norm * z + _xi * (x * x + z * z));
    inv_denom = inv_denom * (-_xi * z - norm);  // reuse variable
    *dudz = x * inv_denom;
    *dvdz = y * inv_denom;

    // Apply distortion
    double dx_u, dy_u;
    distortion(mx_u, my_u, &dx_u, &dy_u, &dxdmx, &dydmx, &dxdmy, &dydmy);
    mx_d = mx_u + dx_u;
    my_d = my_u + dy_u;

    // Make the product of the jacobians
    // and add projection matrix jacobian
    inv_denom = _gamma1 * (*dudx * dxdmx + *dvdx * dxdmy);  // reuse
    *dvdx = _gamma2 * (*dudx * dydmx + *dvdx * dydmy);
    *dudx = inv_denom;

    inv_denom = _gamma1 * (*dudy * dxdmx + *dvdy * dxdmy);  // reuse
    *dvdy = _gamma2 * (*dudy * dydmx + *dvdy * dydmy);
    *dudy = inv_denom;

    inv_denom = _gamma1 * (*dudz * dxdmx + *dvdz * dxdmy);  // reuse
    *dvdz = _gamma2 * (*dudz * dydmx + *dvdz * dydmy);
    *dudz = inv_denom;

    // Apply generalised projection matrix
    // Matlab points start at 1
    *u = _gamma1 * mx_d + _u0;
    *v = _gamma2 * my_d + _v0;
}

/**
 * \brief Projects an undistorted 2D point (\a mx_u,\a my_u) to the image plane in (\a u,\a v)
 *
 * \param mx_u 2D point x coordinate
 * \param my_u 3D point y coordinate
 * \param u return value, contains the image point u coordinate
 * \param v return value, contains the image point v coordinate
 */
void OmniCameraGeometry::undist2plane(double mx_u, double my_u, double* u, double* v) const {
    double mx_d, my_d;

    // Apply distortion
    double dx_u, dy_u;
    distortion(mx_u, my_u, &dx_u, &dy_u);
    mx_d = mx_u + dx_u;
    my_d = my_u + dy_u;

    // Apply generalised projection matrix
    // Matlab points start at 1
    *u = _gamma1 * mx_d + _u0;
    *v = _gamma2 * my_d + _v0;
}

/**
 * \brief Apply distortion to input point (from the normalised plane)
 *
 * \param mx_u undistorted x coordinate of point on the normalised plane
 * \param my_u undistorted y coordinate of point on the normalised plane
 * \param dx return value, to obtain the distorted point : mx_d = mx_u+dx_u
 * \param dy return value, to obtain the distorted point : my_d = my_u+dy_u
 */
void OmniCameraGeometry::distortion(double mx_u, double my_u, double* dx_u, double* dy_u) const {
    double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;

    mx2_u = mx_u * mx_u;
    my2_u = my_u * my_u;
    mxy_u = mx_u * my_u;
    rho2_u = mx2_u + my2_u;
    rad_dist_u = _k1 * rho2_u + _k2 * rho2_u * rho2_u;
    *dx_u = mx_u * rad_dist_u + 2 * _p1 * mxy_u + _p2 * (rho2_u + 2 * mx2_u);
    *dy_u = my_u * rad_dist_u + 2 * _p2 * mxy_u + _p1 * (rho2_u + 2 * my2_u);
}

/**
 * \brief Apply distortion to input point (from the normalised plane)
 *        and calculate jacobian
 *
 * \param mx_u undistorted x coordinate of point on the normalised plane
 * \param my_u undistorted y coordinate of point on the normalised plane
 * \param dx return value, to obtain the distorted point : mx_d = mx_u+dx_u
 * \param dy return value, to obtain the distorted point : my_d = my_u+dy_u
 */
void OmniCameraGeometry::distortion(double mx_u, double my_u, double* dx_u, double* dy_u, double* dxdmx, double* dydmx,
                                    double* dxdmy, double* dydmy) const {
    double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;

    mx2_u = mx_u * mx_u;
    my2_u = my_u * my_u;
    mxy_u = mx_u * my_u;
    rho2_u = mx2_u + my2_u;
    rad_dist_u = _k1 * rho2_u + _k2 * rho2_u * rho2_u;
    *dx_u = mx_u * rad_dist_u + 2 * _p1 * mxy_u + _p2 * (rho2_u + 2 * mx2_u);
    *dy_u = my_u * rad_dist_u + 2 * _p2 * mxy_u + _p1 * (rho2_u + 2 * my2_u);

    *dxdmx = 1 + rad_dist_u + _k1 * 2 * mx2_u + _k2 * rho2_u * 4 * mx2_u + 2 * _p1 * my_u + 6 * _p2 * mx_u;
    *dydmx = _k1 * 2 * mx_u * my_u + _k2 * 4 * rho2_u * mx_u * my_u + _p1 * 2 * mx_u + 2 * _p2 * my_u;
    *dxdmy = *dydmx;
    *dydmy = 1 + rad_dist_u + _k1 * 2 * my2_u + _k2 * rho2_u * 4 * my2_u + 6 * _p1 * my_u + 2 * _p2 * mx_u;
}

void OmniCameraGeometry::updateTemporaries() {
    // Inverse camera projection matrix parameters
    _inv_K11 = 1.0 / _gamma1;
    _inv_K13 = -_u0 / _gamma1;
    _inv_K22 = 1.0 / _gamma2;
    _inv_K23 = -_v0 / _gamma2;
    _one_over_xixi_m_1 = 1.0 / (_xi * _xi - 1.0);
}

void OmniCameraGeometry::setIntrinsicsVectorImplementation(const Eigen::VectorXd& V) {
    _xi = V[0];
    _k1 = V[1];
    _k2 = V[2];
    _p1 = V[3];
    _p2 = V[4];
    _gamma1 = V[5];
    _gamma2 = V[6];
    _u0 = V[7];
    _v0 = V[8];
    updateTemporaries();
}

Eigen::VectorXd OmniCameraGeometry::getIntrinsicsVectorImplementation() const {
    Eigen::VectorXd V(9);
    V[0] = _xi;
    V[1] = _k1;
    V[2] = _k2;
    V[3] = _p1;
    V[4] = _p2;
    V[5] = _gamma1;
    V[6] = _gamma2;
    V[7] = _u0;
    V[8] = _v0;
    return V;
}

Eigen::Matrix3d OmniCameraGeometry::getCameraMatrix() const {
    Eigen::Matrix3d K;
    K << _gamma1, 0, _u0, 0, _gamma2, _v0, 0, 0, 1;
    return K;
}

Eigen::VectorXd OmniCameraGeometry::createRandomKeypoint() const {
    // This is tricky...The camera model defines a circle on the normalized image
    // plane and the projection equations don't work outside of it.
    // With some manipulation, we can see that, on the normalized image plane,
    // the edge of this circle is at u^2 + v^2 = 1/(xi^2 - 1)
    // So: this function creates keypoints inside this boundary.

    // Create a point on the normalized image plane inside the boundary.
    // This is not efficient, but it should be correct.

    Eigen::Vector2d u(width() + 1, height() + 1);

    while (u[0] < 0 || u[0] > width() - 1 || u[1] < 0 || u[1] > height() - 1) {
        u.setRandom();
        u = u - Eigen::Vector2d(0.5, 0.5);
        u /= u.norm();
        u *= ((double)rand() / (double)RAND_MAX) * _one_over_xixi_m_1;

        // Now we run the point through distortion and projection.
        // Apply distortion
        double dx_u, dy_u;
        distortion(u[0], u[1], &dx_u, &dy_u);
        double mx_d = u[0] + dx_u;
        double my_d = u[1] + dy_u;

        // Apply generalised projection matrix
        // Matlab points start at 1
        u[0] = _gamma1 * mx_d + _u0;
        u[1] = _gamma2 * my_d + _v0;
    }

    // I would like to do this below but it is way, way worse than above.
    // // The output
    // Eigen::Vector2d u;
    // // The output projected to the normalized image plane.
    // Eigen::Vector2d u_norm;

    // // The singularity radius minus an epsilon
    // double nRadius = 1.0/_one_over_xixi_m_1;

    // do
    // 	{
    // 	  // Create a random keypoint in the image.
    // 	  //u.setRandom();
    // 	  u[0] = ( (double)rand() / (double)RAND_MAX)*_width;
    // 	  u[1] = ( (double)rand() / (double)RAND_MAX)*_height;

    // 	  // Lift points to normalised plane
    // 	  // Matlab points start at 1 (calibration)
    // 	  double u0_d = _inv_K11*(u[0])+_inv_K13;
    // 	  double u1_d = _inv_K22*(u[1])+_inv_K23;
    // 	  undistortGN(u0_d, u1_d, &u_norm[0], &u_norm[1]);

    // 	}
    // while( u_norm.dot(u_norm) >= nRadius );

    return u;
}

// Use Gauss-Newton to undistort.
void OmniCameraGeometry::undistortGN(double u_d, double v_d, double* u, double* v) const {
    *u = u_d;
    *v = v_d;

    double ubar = u_d;
    double vbar = v_d;
    const int n = 5;
    Eigen::Matrix2d F;

    double hat_u_d;
    double hat_v_d;

    // void OmniCameraGeometry::distortion(double mx_u, double my_u,
    // 					  double *dx_u, double *dy_u,
    // 					  double *dxdmx, double *dydmx,
    // 					  double *dxdmy, double *dydmy) const
    for (int i = 0; i < n; i++) {
        distortion(ubar, vbar, &hat_u_d, &hat_v_d, &F(0, 0), &F(1, 0), &F(0, 1), &F(1, 1));

        Eigen::Vector2d e(u_d - ubar - hat_u_d, v_d - vbar - hat_v_d);
        Eigen::Vector2d du = (F.transpose() * F).inverse() * F.transpose() * e;

        ubar += du[0];
        vbar += du[1];

        if (e.dot(e) < 1e-15) break;
    }
    *u = ubar;
    *v = vbar;
}

/// \brief initialize the intrinsics based on one view of a gridded calibration target
/// \return true on success
///
/// These functions were developed with the help of Lionel Heng and the excellent camodocal
/// https://github.com/hengli/camodocal
bool OmniProjection::initializeIntrinsics(const GridCalibrationTargetObservation& obs) {
    if (!obs.target()) {
        return false;
    }

    double square(double x) { return x * x; }
    float square(float x) { return x * x; }
    double hypot(double a, double b) { return sqrt(square(a) + square(b)); }

    // First, initialize the image center at the center of the image.
    _xi = 1.0;
    _cu = obs.imCols() / 2.0;
    _cv = obs.imRows() / 2.0;
    _ru = obs.imCols();
    _rv = obs.imRows();

    _distortion.clear();

    // Grab a reference to the target for easy access.
    const GridCalibrationTarget& target = *obs.target();

    /// Initialize some temporaries needed.
    double gamma0 = 0.0;
    double minReprojErr = std::numeric_limits<double>::max();

    // Now we try to find a non-radial line to initialize the focal length
    bool success = false;
    for (size_t r = 0; r < target.rows(); ++r) {
        // Grab all the valid corner points for this checkerboard observation
        cv::Mat P(target.cols(); 4, CV_64F);
        size_t count = 0;
        for (size_t c = 0; c < target.cols(); ++c) {
            Eigen::Vector2d imagePoint;
            Eigen::Vector3d gridPoint;
            if (obs.imageGridPoint(r, c, imagePoint)) {
                double u = imagePoint[0] - _cu;
                double v = imagePoint[1] - _cv;
                P.at<double>(count, 0) = u;
                P.at<double>(count, 1) = v;
                P.at<double>(count, 2) = 0.5;
                P.at<double>(count, 3) = -0.5 * (square(u) + square(v));
                ++count;
            }
        }

        const int MIN_CORNERS = 8;
        // MIN_CORNERS is an arbitrary threshold for the number of corners
        if (count > MIN_CORNERS) {
            // Resize P to fit with the count of valid points.
            cv::Mat C;
            cv::SVD::solveZ(P.colRange(0, count), C);

            double t = square(C.at<double>(0)) + square(C.at<double>(1)) + C.at<double>(2) * C.at<double>(3);
            if (t < 0) {
                continue;
            }

            // check that line image is not radial
            double d = sqrt(1.0 / t);
            double nx = C.at<double>(0) * d;
            double ny = C.at<double>(1) * d;
            if (hypot(nx, ny) > 0.95) {
                continue;
            }

            double nz = sqrt(1.0 - square(nx) - square(ny));
            double gamma = fabs(C.at<double>(2) * d / nz);

            _fu = gamma;
            _fv = gamma;
            sm::kinematics::Transformation T_target_camera;
            if (!estimateTransformation(obs, T_target_camera)) {
                continue;
            }

            double reprojErr = 0.0;
            size_t numReprojected = computeReprojectionError(obs, T_target_camera, reprojErr);

            if (numReprojected > MIN_CORNERS) {
                double avgReprojErr = reprojErr / numReprojected;

                if (avgReprojErr < minReprojErr) {
                    minReprojErr = avgReprojErr;
                    gamma0 = gamma;
                    success = true;
                }
            }

        }  // If this observation has enough valid corners
    }      // For each row in the image.

    _fu = gamma0;
    _fv = gamma0;

    return success;

}  // initializeIntrinsics()

bool OmniProjection::computeReprojectionError(const GridCalibrationTargetObservation& obs,
                                              const sm::kinematics::Transformation& T_target_camera,
                                              double& outErr) const {
    outErr = 0.0;
    size_t count = 0;
    sm::kinematics::Transformation T_camera_target = T_target_camera.inverse();

    for (size_t i = 0; i < obs.size(); ++i) {
        Eigen::Vector2d y, yhat;
        if (obs.imagePoint(i, y) && euclideanToKeypoint(T_camera_target * obs.target()->point(i), yhat)) {
            outErr += (y - yhat).norm();
            ++count;
        }
    }

    return count;
}

/// \brief estimate the transformation of the camera with respect to the calibration target
///        On success out_T_t_c is filled in with the transformation that takes points from
///        the camera frame to the target frame
/// \return true on success
///
/// These functions were developed with the help of Lionel Heng and the excellent camodocal
/// https://github.com/hengli/camodocal
bool OmniProjection::estimateTransformation(const GridCalibrationTargetObservation& obs,
                                            sm::kinematics::Transformation& out_T_t_c) const {
    // Convert all chessboard corners to a fakey pinhole view.
    // Call the OpenCV pnp function.
}

}  // namespace cameras

}  // namespace aslam


/*
 * patSpeedDistributions.cc
 *
 *  Created on: Mar 26, 2012
 *      Author: jchen
 */

#include "patSpeedDistributions.h"
#include "patDisplay.h"
#include "patNBParameters.h"
#include "patErrMiscError.h"

#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/lognormal.hpp> // for normal_distribution
#include <boost/math/distributions/exponential.hpp> // for exponential distribution
#include "patDisplay.h"
#include "patError.h"
#include <sstream>
#include <vector>
#include <cstdlib>
using boost::math::normal;
using boost::math::lognormal;
using boost::math::exponential_distribution;
 map<TransportMode, TrafficModelParam> patSpeedDistributions::tm_params;
patSpeedDistributions::patSpeedDistributions() {

}

patSpeedDistributions::~patSpeedDistributions() {
}

patSpeedDistributions* patSpeedDistributions::ins = NULL;

patSpeedDistributions* patSpeedDistributions::the() {
	if (patSpeedDistributions::ins == NULL) {
		patSpeedDistributions::ins = new patSpeedDistributions;
	}
	return patSpeedDistributions::ins;
}
double patSpeedDistributions::pdf(double v, TransportMode mode) {
	TrafficModelParam tm_param = patSpeedDistributions::tm_params[mode];
	double rtn = 0.0;
	if (v < 0.0) {
//		WARNING("wrong speed" << v);
		rtn = 0.0;
	} else if (v == 0.0) {
		rtn = tm_param.w * tm_param.lambda;
	} else {

		if (tm_param.mode == TransportMode(WALK)) {
			boost::math::exponential e(tm_param.lambda);
			boost::math::normal n(tm_param.mu,tm_param.sigma);

			rtn = tm_param.w * boost::math::pdf(e,v )  + (1.0-tm_param.w) *boost::math::pdf(n,v ) ;
			// rtn = tm_param.w * tm_param.lambda * exp(-tm_param.lambda * v)
			// 		+ (1.0 - tm_param.w)
			// 				* exp(-(v - tm_param.mu) * (v - tm_param.mu) /(2.0 * tm_param.sigma * tm_param.sigma))
			// 				/ (tm_param.sigma * sqrt(2.0 * 	M_PI) );
		} else {
			boost::math::exponential e(tm_param.lambda );
			boost::math::lognormal n(tm_param.mu,tm_param.sigma);
			 rtn = tm_param.w * boost::math::pdf(e,v )  + (1.0-tm_param.w) *boost::math::pdf(n,v ) ;
			// rtn = tm_param.w * tm_param.lambda * exp(-tm_param.lambda * v)
			// 		+ (1.0 - tm_param.w)
			// 				* exp( -(log(v) - tm_param.mu) * (log(v) - tm_param.mu) /(2.0 * tm_param.sigma * tm_param.sigma))
			// 				/ (v * tm_param.sigma * sqrt(2.0 * M_PI));
		}
	}
	if (rtn >= 1.0 || rtn < 0.0) {
		WARNING("WRONG");
	}
	return rtn;
}
void patSpeedDistributions::readParams(patError*& err) {

	vector<TransportMode> modes;
	modes.push_back(TransportMode(CAR));
	modes.push_back(TransportMode(BUS));
	modes.push_back(TransportMode(METRO));
	modes.push_back(TransportMode(TRAIN));
	modes.push_back(TransportMode(WALK));
	modes.push_back(TransportMode(BIKE));
	for (int i = 0; i < modes.size(); ++i) {
		TrafficModelParam params;
		params.mode = modes[i];
		string file_name = patNBParameters::the()->paramFolder + "speed/"
				+ getTransportModeString(modes[i]);
		//patAccelMeasurementModel::m_params[modes[i]];
		ifstream file_stream_handler;
		file_stream_handler.open(file_name.c_str(), ios::in);
		if (!file_stream_handler) {
			stringstream str;
			str << "Error while parsing " << file_name;
			err = new patErrMiscError(str.str());
			WARNING(err->describe());
			return;
		}
//		DEBUG_MESSAGE("Read file:" << file_name);
		int components = 0;
		string line;
		if (getline(file_stream_handler, line)) {

			istringstream linestream(line);
			string item;

			getline(linestream, item, ',');
			//	DEBUG_MESSAGE(item);
			params.w = atof(item.c_str());

			getline(linestream, item, ',');
			//DEBUG_MESSAGE(item);
			params.lambda = atof(item.c_str());
			//		DEBUG_MESSAGE(item);

			getline(linestream, item, ',');
			params.mu = atof(item.c_str());
//			DEBUG_MESSAGE(item);

			getline(linestream, item, ',');
			params.sigma = atof(item.c_str());

//			DEBUG_MESSAGE(
//					params.w << "," << params.lambda << "," << params.mu << ","
//							<< params.sigma)
			patSpeedDistributions::tm_params[modes[i]] = params;
		} else {
			stringstream str;
			str << "Speed distribution for  " << modes[i] << "is wrong";
			err = new patErrMiscError(str.str());
			WARNING(err->describe());
			return;

		}

	}
}


//==================================================================================================
/*!
  @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_COT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_COT_HPP_INCLUDED

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

  /*!
    @ingroup group-trigonometric
    This function object returns the cotangent of the input in radian: \f$\cos(x)/\sin(x)\f$.

    @par Header <boost/simd/function/cot.hpp>

    @par Note

      As most other trigonometric function cot can be called
      with a second optional parameter  which is a tag on speed and accuracy
      (see @ref cos for further details)

    @see cos, sin, tan, cotd, cotpi

    @par Example:

      @snippet cot.cpp cot

    @par Possible output:

      @snippet cot.txt cot

  **/
  IEEEValue cot(IEEEValue const& x);
} }
#endif

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

#endif


/// @file model.hpp Various log-PDF evaluators for parts of the Bayesian model

#ifndef BIGGLES_MODEL_HPP__
#define BIGGLES_MODEL_HPP__

#include <vector>
#include <boost/tuple/tuple.hpp>
#include <Eigen/Dense>

#include "partition.hpp"
#include "types.hpp"

namespace biggles
{

/// @brief Functions and types defining the Biggles dynamic model
namespace model
{

/// @name Access to the tracking control parameters
/// @{
inline const float& birth_rate(const parameters& p) { return p.birth_rate; }
inline float& birth_rate(parameters& p) { return p.birth_rate; }

inline const float& clutter_rate(const parameters& p) { return p.clutter_rate; }
inline float& clutter_rate(parameters& p) { return p.clutter_rate; }

inline const float& survival_probability(const parameters& p) { return p.survival_probability; }
inline float& survival_probability(parameters& p) { return p.survival_probability; }

inline const float& observation_probability(const parameters& p) { return p.observation_probability; }
inline float& observation_probability(parameters& p) { return p.observation_probability; }

inline const matrix4f& process_noise_covariance(const parameters& p) { return p.process_noise_covariance; }
inline matrix4f& process_noise_covariance(parameters& p) { return p.process_noise_covariance; }
/// @}

/// @brief Access the mean number of tracks per-frame parameter from a biggles::parameters tuple.
///
/// @param p
inline const float& mean_new_tracks_per_frame(const parameters& p) { return p.birth_rate; }

/// @brief Access the mean number of tracks per-frame parameter from a biggles::parameters tuple.
///
/// @param p
inline float& mean_new_tracks_per_frame(parameters& p) { return p.birth_rate; }

/// @brief Access the mean number of spurious observations per-frame parameter from a biggles::parameters tuple.
///
/// @param p
inline const float& mean_false_observations_per_frame(const parameters& p) { return p.clutter_rate; }

/// @brief Access the mean number of spurious observations per-frame parameter from a biggles::parameters tuple.
///
/// @param p
inline float& mean_false_observations_per_frame(parameters& p) { return p.clutter_rate; }

/// @brief Access the frame-to-frame survival probability parameter from a biggles::parameters tuple.
///
/// @param p
inline const float& frame_to_frame_survival_probability(const parameters& p) { return p.survival_probability; }

/// @brief Access the frame-to-frame survival probability parameter from a biggles::parameters tuple.
///
/// @param p
inline float& frame_to_frame_survival_probability(parameters& p) { return p.survival_probability; }

/// @brief Access the probability that a track will generate an observation from a biggles::parameters tuple.
///
/// @param p
inline const float& generate_observation_probability(const parameters& p) { return p.observation_probability; }

/// @brief Access the probability that a track will generate an observation from a biggles::parameters tuple.
///
/// @param p
inline float& generate_observation_probability(parameters& p) { return p.observation_probability; }

/// @brief Access the covariance matrix of the observation error from a biggles::parameters tuple.
///
/// @param p
inline const Eigen::Matrix2f& observation_error_covariance(const parameters& p) { return p.observation_error_covariance; }

/// @brief Access the covariance matrix of the observation error from a biggles::parameters tuple.
///
/// @param p
inline Eigen::Matrix2f& observation_error_covariance(parameters& p) { return p.observation_error_covariance; }

/// @brief Access the constraint radius from a biggles::parameters tuple.
///
/// @param p
inline const float& constraint_radius(const parameters& p) { return p.constraint_radius; }

/// @brief Access the constraint radius from a biggles::parameters tuple.
///
/// @param p
inline float& constraint_radius(parameters& p) { return p.constraint_radius; }

/// @brief Calculate the log-pdf of a partition given a set of model parameters, independent of observed data.
///
/// **The following needs to be reviewed. Binomial coefficients need to be included**
///
/// Calculate the value of \f$ \ell(T | \theta) \f$ from the partition and model parameters given. Since the model
/// parameters are independent of one another, we may factorise the pdf as follows:
///
/// \f[
/// P(T | \theta) = P(T | p_s) P(T | p_d) P(T | \lambda_b) P(T | \lambda_f).
/// \f]
///
/// Note that \f$ R \f$ does not appear here because it depends on data; these terms are all the data-independent parts
/// of the posterior on \f$ T \f$. Each of these terms may defined in terms of the following values:
///
/// - \f$ N_t^s \f$: the number of tracks which survive from \f$ t-1 \f$ to \f$ t \f$;
/// - \f$ N_t^d \f$: the number of tracks which did not survive from \f$ t-1 \f$ to \f$ t \f$;
/// - \f$ N_t^b \f$: the number of tracks which newly appeared at \f$ t \f$;
/// - \f$ N_t^o \f$: the number of observations assigned to a track at \f$ t \f$;
/// - \f$ N_t^f \f$: the number of observations deemed spurious at \f$ t \f$,
///
/// where \f$ N_0^s = N_o^b = 0 \f$ by convention. Each of the individual parameter pdf terms can be written down based
/// on the definition of the corresponding parameters:
///
/// - \f$ P(T | p_s) = \prod_{t=1}^K p_s^{N_t^s} (1-p_s)^{N_t^d}; \f$
/// - \f$ P(T | p_d) = \prod_{t=1}^K p_d^{N_t^o} (1-p_d)^{N_t^s + N_t^b - N_t^o}; \f$
/// - \f$ P(T | \lambda_b) = \prod_{t=1}^K \mathcal{P}(N_t^b ; \lambda_b); \f$
/// - \f$ P(T | \lambda_f) = \prod_{t=1}^K \mathcal{P}(N_t^f ; \lambda_f), \f$
///
/// where \f$ \mathcal{P}(x ; \lambda) \f$ is the Poisson pmf with mean \f$ \lambda \f$ evaluated at \f$ x \f$. The
/// corresponding log terms become:
///
/// - \f$ \ell(T | p_s) = \sum_{t=1}^K N_t^s \log(p_s) + N_t^d \log(1-p_s); \f$
/// - \f$ \ell(T | p_d) = \sum_{t=1}^K N_t^o \log(p_d) + (N_t^s + N_t^b - N_t^o) \log(1-p_d); \f$
/// - \f$ \ell(T | \lambda_b) = \sum_{t=1}^K N_t^b \log(\lambda_b) - \lambda_b - \log(\Gamma(1 + N_t^b)); \f$
/// - \f$ \ell(T | \lambda_f) = \sum_{t=1}^K N_t^f \log(\lambda_f) - \lambda_f - \log(\Gamma(1 + N_t^f)); \f$
///
/// @sa biggles::parameters
///
/// @param part A reference to the partition to consider.
/// @param parameters A reference to the model parameters.
///
/// @return The value of \f$ \ell(T | \theta) \f$.
float log_partition_given_parameters_density(const partition& part, const parameters& parameters);

float log_partition_given_observation_prob_density(const partition& part, const parameters& parameters);
float log_partition_given_survival_prob_density(const partition& part, const parameters& parameters);
float log_partition_given_birth_rate_density(const partition& part, const parameters& parameters);
float log_partition_given_clutter_rate_density(const partition& part, const parameters& parameters);

/// @brief Calculate the log-pdf for the track observations given the parameters.
///
/// This function works by using a biggles::kalman_filter to sample missing states for a track and then to calculate
/// the likelihood of the observations we've seen given the parameters.
///
/// Specifically, suppose we have an observation \f$ y \f$, a predicted state, \f$ \hat{x} \f$ and state estimation
/// error estimate, \f$ \hat{P} \f$. Then the total error in the predicted observation, \f$ \hat{y} = B \hat{x} \f$ is
/// given by \f$ \hat{\Sigma} = B \hat{P} B^T + R \f$. We therefore calculate the likelihood of \f$ y \f$ assuming a
/// Gaussian model:
///
/// \f[
/// P(y | \hat{x}, \hat{P}) = \mathcal{N}(y ; \hat{y}, \hat{\Sigma}).
/// \f]
///
/// We combine all these likelihoods for each observation in the track.
///
/// @sa biggles::kalman_filter
///
/// @param track_p
/// @param parameters
///
/// @return The value of \f$ \ell(d_i|t_i, \theta) \f$.
float log_track_given_parameters_density(const boost::shared_ptr<const track>& track_p,
                                         const parameters& parameters);

/// @brief Compute the log-prior on the model parameters.
///
/// The prior on the model parameters is as follows:
///
/// - \f$ \lambda_b \f$ and \f$ \lambda_s \f$ have improper uninformative priors on them being positive.
/// - \f$ p_s \f$ and \f$ p_d \f$ have uniform priors over [0, 1].
/// - \f$ R \f$ has an inverse Wishart prior with parameters \f$ \Phi = 2I \f$ and \f$ s = 5 \f$. These are the same
/// parameters as in sample_parameters_given_partition().
///
/// @sa sample_parameters_given_partition()
///
/// @param parameters The model parameters whose prior should be calculated.
///
/// @return The value of \f$ \ell(\theta) \f$.
float log_parameters_prior_density(const parameters& parameters);

/// @brief Calculate the log pdf of the clutter observations given the model parameters.
///
/// The clutter observations themselves are independent of the model parameter \f$ R \f$ and so their distribution is
/// very simple:
///
/// \f[
/// P(d_0 | t_0, \theta) = \prod_{t=1}^K \left( \frac{1}{V} \right)^{N_t^f}
/// \f]
///
/// where \f$ V \f$ is the total number of pixels in the image.
///
/// @note Currently it is assumed that \f$ V = 256^2 \f$. This is really a bug but for the moment, this is the only
/// place where the absolute image size matters.
///
/// @param clutter
/// @param parameters
///
/// @return The value of \f$ \ell(d_0 | t_0, \theta) \f$.
float log_clutter_given_parameters_density(const partition& part, const parameters& parameters);

/// @brief Calculate the log pdf of the track observations given the model parameters.
///
/// This uses the Kalman filter. track likelihood depends on the model parameter \f$ R \f$ only.
float log_tracks_given_parameters_density(const partition& part, const model::parameters& parameters);

/** \brief returns log( p(data| partition, parameters) )
 *
 *
 */
float log_likelihood(const partition& part_sample, const model::parameters& para_sample);

/// @brief Evaluate the full log pdf for a particular partition
///
/// In the documentation for biggles::partition_sampler, it was shown that
///
/// \f[
/// \ell(T | \theta, D) = \kappa + \ell(d_0|t_0, \theta) + \sum_{i=1}^K \ell(d_i|t_i, \theta) + \ell(T|\theta) +
/// \ell(\theta).
/// \f]
///
/// This function computes this value by calling other log density calculation functions.
///
/// @sa biggles::partition_sampler
/// @sa log_clutter_given_parameters_density()
/// @sa log_track_given_parameters_density()
/// @sa log_partition_given_parameters_density()
/// @sa log_parameters_prior_density()
///
/// @param part
/// @param parameters
///
/// @return The value of \f$ \ell(T | \theta, D) + \kappa \f$ where \f$ \kappa \f$ is some constant offset.
float log_partition_given_parameters_and_data_density(const partition& part, const parameters& parameters);

class log_factorial {
    typedef float VALUE_TYPE;
    std::deque< VALUE_TYPE > factorial_;
    log_factorial() {
        factorial_.push_back(0.f);
        factorial_.push_back(0.f);
    }
    log_factorial& operator=(log_factorial&);
    log_factorial(const log_factorial&);
public:
    /// \brief the call operator (non-const)
    VALUE_TYPE operator() (size_t n) {
        while (n >= factorial_.size()) {
            factorial_.push_back(factorial_.back() + logf(factorial_.size()));
        }
        return factorial_.at(n);
    }
    /// \brief calculate without creating a reference
    static VALUE_TYPE calc (size_t n) {
        static log_factorial fact;
        return fact(n);
    }
    /// \brief return an instance of the binomial coefficent
    static log_factorial& get() {
        static log_factorial fact;
        return fact;
    }
};

/// \brief calculates the binomial coefficent
///
/// Each value is calculated once and is stored in Pascal's triangle.
/// The calculation includes an overflow control.
/// It is a recursive procedure.
/// This is implemented as a singleton
class binomial_coefficient {
    typedef float VALUE_TYPE;
    /// \brief the row type; the size of each vector is fixed
    typedef std::vector<VALUE_TYPE> row_t;
    std::deque< row_t > triangle_; /// \brief Pascal's triangle
    size_t num_rows_; /// \brief the current number of rows of Pascal's triangle
    /// \brief The calculation routine
    void build_row(size_t n) {
        row_t row(n+1, VALUE_TYPE(1));
        for (size_t k = 1; k < n; ++k) {
            VALUE_TYPE s1 = operator()(n-1, k - 1);
            VALUE_TYPE s2 = operator()(n-1, k);
            if (s1 + s2 < s1) {
                std::stringstream errmsg;
                errmsg << "maximum reached n = " << n << ", k = " << k;
                throw std::overflow_error(errmsg.str());
            }
            row[k] = s1 + s2;
        }
        triangle_.push_back(row);
        num_rows_ = triangle_.size();
    }
    /// \brief The constructor initalises the Pascal's triangle with (0 choose 0)
    binomial_coefficient() {
        triangle_.push_back(row_t(1,VALUE_TYPE(1)));
        num_rows_ = triangle_.size();
    }
    binomial_coefficient& operator=(binomial_coefficient&);
    binomial_coefficient(const binomial_coefficient&);
public:
    /// \brief the call operator (non-const)
    VALUE_TYPE operator() (size_t n, size_t k) {
        if (num_rows_ <= n) build_row(n);
        return triangle_[n][k];
    }
    /// \brief return an instance of the binomial coefficent
    static binomial_coefficient& get() {
        static binomial_coefficient bc;
        return bc;
    }
};

}

}

#endif // BIGGLES_MODEL_HPP__


#ifndef PARMCB_SVA_TREES_HPP_
#define PARMCB_SVA_TREES_HPP_

//    Copyright (C) Dimitrios Michail 2019 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/tuple/detail/tuple_basic.hpp>
#include <boost/timer/timer.hpp>

#include <cstddef>
#include <functional>
#include <iostream>
#include <iterator>
#include <limits>
#include <set>
#include <vector>

#include <parmcb/forestindex.hpp>
#include <parmcb/spvecgf2.hpp>
#include <parmcb/util.hpp>
#include <parmcb/sptrees.hpp>
#include <parmcb/detail/cycles.hpp>

namespace parmcb {

    template<class Graph, class WeightMap, class CycleOutputIterator, class CyclesBuilder, bool ParallelUsingTBB>
    typename boost::property_traits<WeightMap>::value_type _mcb_sva_trees(const Graph &g, WeightMap weight_map,
            CycleOutputIterator out) {
        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;

        /*
         * Index the graph
         */
        ForestIndex<Graph> forest_index(g);
        auto csd = forest_index.cycle_space_dimension();
        std::cout << "Cycle space dimension: " << csd << std::endl;

        /*
         * Initialize support vectors
         */
        std::vector<SpVecGF2<std::size_t>> support;
        for (std::size_t k = 0; k < csd; k++) {
            support.emplace_back(k);
        }

        boost::timer::cpu_timer cycle_timer;
        cycle_timer.stop();
        boost::timer::cpu_timer support_timer;
        support_timer.stop();
        boost::timer::cpu_timer trees_timer;
        trees_timer.stop();

        /*
         * Initialize all shortest path trees
         */
        trees_timer.resume();
        std::vector<parmcb::SPTree<Graph, WeightMap>> trees;
        std::vector<parmcb::CandidateCycle<Graph, WeightMap>> cycles;
        CyclesBuilder cycles_builder;
        cycles_builder(g, weight_map, trees, cycles);
        std::cout << "Total candidate cycles: " << cycles.size() << std::endl;
        const bool sorted_cycles = true;
        if (sorted_cycles) {
            // sort
            std::cout << "Sorting cycles" << std::endl;
            std::sort(cycles.begin(), cycles.end(), [](const auto &a, const auto &b) {
                return a.weight() < b.weight();
            });
        }
        ShortestOddCycleLookup<Graph, WeightMap, ParallelUsingTBB> cycle_lookup(g, weight_map, trees, cycles,
                sorted_cycles);
        trees_timer.stop();

        /*
         * Main loop
         */
        WeightType mcb_weight = WeightType();
        for (std::size_t k = 0; k < csd; k++) {
            if (k % 250 == 0) {
                std::cout << k << std::endl;
            }

            /*
             * Compute shortest odd cycle
             */
            std::set<Edge> signed_edges;
            convert_edges(support[k], std::inserter(signed_edges, signed_edges.end()), forest_index);
            cycle_timer.resume();
            std::tuple<std::set<Edge>, WeightType, bool> best = cycle_lookup(signed_edges);
            cycle_timer.stop();

            /*
             * Update support vectors
             */
            support_timer.resume();
            std::set<std::size_t> cyclek;
            convert_edges(std::get<0>(best), std::inserter(cyclek, cyclek.end()), forest_index);
            for (std::size_t l = k + 1; l < csd; l++) {
                if (support[l] * cyclek == 1) {
                    support[l] += support[k];
                }
            }
            support_timer.stop();

            /*
             * Output new cycle
             */
            std::list<Edge> cyclek_edgelist;
            std::copy(std::get<0>(best).begin(), std::get<0>(best).end(), std::back_inserter(cyclek_edgelist));
            *out++ = cyclek_edgelist;
            mcb_weight += std::get<1>(best);
        }

        std::cout << "trees   timer" << trees_timer.format();
        std::cout << "cycle   timer" << cycle_timer.format();
        std::cout << "support timer" << support_timer.format();

        return mcb_weight;
    }

    template<class Graph, class WeightMap, class CycleOutputIterator>
    typename boost::property_traits<WeightMap>::value_type mcb_sva_fvs_trees(const Graph &g, WeightMap weight_map,
            CycleOutputIterator out) {
        return _mcb_sva_trees<Graph, WeightMap, CycleOutputIterator, parmcb::detail::FVSCyclesBuilder<Graph, WeightMap>,
                false>(g, weight_map, out);
    }

    template<class Graph, class WeightMap, class CycleOutputIterator>
    typename boost::property_traits<WeightMap>::value_type mcb_sva_fvs_trees_tbb(const Graph &g, WeightMap weight_map,
            CycleOutputIterator out) {
        return _mcb_sva_trees<Graph, WeightMap, CycleOutputIterator, parmcb::detail::FVSCyclesBuilder<Graph, WeightMap>,
                true>(g, weight_map, out);
    }

    template<class Graph, class WeightMap, class CycleOutputIterator>
    typename boost::property_traits<WeightMap>::value_type mcb_sva_iso_trees(const Graph &g, WeightMap weight_map,
            CycleOutputIterator out) {
        return _mcb_sva_trees<Graph, WeightMap, CycleOutputIterator, parmcb::detail::ISOCyclesBuilder<Graph, WeightMap>,
                false>(g, weight_map, out);
    }

    template<class Graph, class WeightMap, class CycleOutputIterator>
    typename boost::property_traits<WeightMap>::value_type mcb_sva_iso_trees_tbb(const Graph &g, WeightMap weight_map,
            CycleOutputIterator out) {
        return _mcb_sva_trees<Graph, WeightMap, CycleOutputIterator, parmcb::detail::ISOCyclesBuilder<Graph, WeightMap>,
                true>(g, weight_map, out);
    }

} // namespace parmcb

#endif


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
/*
  Copyright (C) 2014, 2016 Peter Caspers

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

/*! \file lineartsrpricer.cpp
*/

#include <ql/cashflows/lineartsrpricer.hpp>
#include <ql/cashflows/fixedratecoupon.hpp>
#include <ql/cashflows/iborcoupon.hpp>
#include <ql/cashflows/cmscoupon.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/indexes/iborindex.hpp>
#include <ql/time/schedule.hpp>
#include <ql/instruments/vanillaswap.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <ql/math/integrals/kronrodintegral.hpp>
#include <ql/pricingengines/blackformula.hpp>
#include <ql/termstructures/volatility/atmsmilesection.hpp>
#include <boost/make_shared.hpp>

namespace QuantLib {

   const Real LinearTsrPricer::defaultLowerBound = 0.0001,
             LinearTsrPricer::defaultUpperBound = 2.0000;

    LinearTsrPricer::LinearTsrPricer(
        const Handle<SwaptionVolatilityStructure> &swaptionVol,
        const Handle<Quote> &meanReversion,
        const Handle<YieldTermStructure> &couponDiscountCurve,
        const Settings &settings,
        const boost::shared_ptr<Integrator> &integrator)
        : CmsCouponPricer(swaptionVol), meanReversion_(meanReversion),
          couponDiscountCurve_(couponDiscountCurve), settings_(settings),
          volDayCounter_(swaptionVol->dayCounter()), integrator_(integrator) {

        if (!couponDiscountCurve_.empty())
            registerWith(couponDiscountCurve_);

        if (integrator_ == NULL)
            integrator_ =
                boost::make_shared<GaussKronrodNonAdaptive>(1E-10, 5000, 1E-10);
    }

    Real LinearTsrPricer::GsrG(const Date &d) const {

        Real yf = volDayCounter_.yearFraction(fixingDate_, d);
        if (std::fabs(meanReversion_->value()) < 1.0E-4)
            return yf;
        else
            return (1.0 - std::exp(-meanReversion_->value() * yf)) /
                   meanReversion_->value();
    }

    Real LinearTsrPricer::singularTerms(const Option::Type type,
                                        const Real strike) const {

        Real omega = (type == Option::Call ? 1.0 : -1.0);
        Real s1 = std::max(omega * (swapRateValue_ - strike), 0.0) *
                  (a_ * swapRateValue_ + b_);
        Real s2 = (a_ * strike + b_) *
                  smileSection_->optionPrice(strike, strike < swapRateValue_
                                                         ? Option::Put
                                                         : Option::Call);
        return s1 + s2;
    }

    Real LinearTsrPricer::integrand(const Real strike) const {
        return 2.0 * a_ * smileSection_->optionPrice(
                              strike, strike < swapRateValue_ ? Option::Put
                                                              : Option::Call);
    }

    void LinearTsrPricer::initialize(const FloatingRateCoupon &coupon) {

        coupon_ = dynamic_cast<const CmsCoupon *>(&coupon);
        QL_REQUIRE(coupon_, "CMS coupon needed");
        gearing_ = coupon_->gearing();
        spread_ = coupon_->spread();

        fixingDate_ = coupon_->fixingDate();
        paymentDate_ = coupon_->date();
        swapIndex_ = coupon_->swapIndex();

        forwardCurve_ = swapIndex_->forwardingTermStructure();
        if (swapIndex_->exogenousDiscount())
            discountCurve_ = swapIndex_->discountingTermStructure();
        else
            discountCurve_ = forwardCurve_;

        // if no coupon discount curve is given just use the discounting curve
        // from the swap index. for rate calculation this curve cancels out in
        // the computation, so e.g. the discounting swap engine will produce
        // correct results, even if the couponDiscountCurve is not set here.
        // only the price member function in this class will be dependent on the
        // coupon discount curve.

        today_ = QuantLib::Settings::instance().evaluationDate();

        if (paymentDate_ > today_ && !couponDiscountCurve_.empty())
            couponDiscountRatio_ =
                couponDiscountCurve_->discount(paymentDate_) /
                discountCurve_->discount(paymentDate_);
        else
            couponDiscountRatio_ = 1.;

        spreadLegValue_ = spread_ * coupon_->accrualPeriod() *
                          discountCurve_->discount(paymentDate_) *
                          couponDiscountRatio_;

        if (fixingDate_ > today_) {

            swapTenor_ = swapIndex_->tenor();
            swap_ = swapIndex_->underlyingSwap(fixingDate_);

            swapRateValue_ = swap_->fairRate();
            annuity_ = 1.0E4 * std::fabs(swap_->fixedLegBPS());

            boost::shared_ptr<SmileSection> sectionTmp =
                swaptionVolatility()->smileSection(fixingDate_, swapTenor_);

            adjustedLowerBound_ = settings_.lowerRateBound_;
            adjustedUpperBound_ = settings_.upperRateBound_;

            if(sectionTmp->volatilityType() == Normal) {
                // adjust lower bound if it was not set explicitly
                if(settings_.defaultBounds_)
                    adjustedLowerBound_ = std::min(adjustedLowerBound_, -adjustedUpperBound_);
            } else {
                // adjust bounds by section's shift
                adjustedLowerBound_ -= sectionTmp->shift();
                adjustedUpperBound_ -= sectionTmp->shift();
            }

            // if the section does not provide an atm level, we enhance it to
            // have one, no need to exit with an exception ...

            if (sectionTmp->atmLevel() == Null<Real>())
                smileSection_ = boost::make_shared<AtmSmileSection>(
                    sectionTmp, swapRateValue_);
            else
                smileSection_ = sectionTmp;

            // compute linear model's parameters

            Real gx = 0.0, gy = 0.0;
            for (Size i = 0; i < swap_->fixedLeg().size(); i++) {
                boost::shared_ptr<Coupon> c =
                    boost::dynamic_pointer_cast<Coupon>(swap_->fixedLeg()[i]);
                Real yf = c->accrualPeriod();
                Date d = c->date();
                Real pv = yf * discountCurve_->discount(d);
                gx += pv * GsrG(d);
                gy += pv;
            }

            Real gamma = gx / gy;
            Date lastd = swap_->fixedLeg().back()->date();

            a_ = discountCurve_->discount(paymentDate_) *
                 (gamma - GsrG(paymentDate_)) /
                 (discountCurve_->discount(lastd) * GsrG(lastd) +
                  swapRateValue_ * gy * gamma);

            b_ = discountCurve_->discount(paymentDate_) / gy -
                 a_ * swapRateValue_;
        }
    }

    Real LinearTsrPricer::strikeFromVegaRatio(Real ratio,
                                              Option::Type optionType,
                                              Real referenceStrike) const {

        Real a, b, min, max, k;
        if (optionType == Option::Call) {
            a = swapRateValue_;
            min = referenceStrike;
            b = max = k =
                std::min(smileSection_->maxStrike(), adjustedUpperBound_);
        } else {
            a = min = k =
                std::max(smileSection_->minStrike(), adjustedLowerBound_);
            b = swapRateValue_;
            max = referenceStrike;
        }

        VegaRatioHelper h(&*smileSection_,
                          smileSection_->vega(swapRateValue_) * ratio);
        Brent solver;

        try {
            k = solver.solve(h, 1.0E-5, (a + b) / 2.0, a, b);
        }
        catch (...) {
            // use default value set above
        }

        return std::min(std::max(k, min), max);
    }

    Real LinearTsrPricer::strikeFromPrice(Real price, Option::Type optionType,
                                          Real referenceStrike) const {

        Real a, b, min, max, k;
        if (optionType == Option::Call) {
            a = swapRateValue_;
            min = referenceStrike;
            b = max = k =
                std::min(smileSection_->maxStrike(), adjustedUpperBound_);
        } else {
            a = min = k =
                std::max(smileSection_->minStrike(), adjustedLowerBound_);
            b = swapRateValue_;
            max = referenceStrike;
        }

        PriceHelper h(&*smileSection_, optionType, price);
        Brent solver;

        try {
            k = solver.solve(h, 1.0E-5, swapRateValue_, a, b);
        }
        catch (...) {
            // use default value set above
        }

        return std::min(std::max(k, min), max);
    }

    Real LinearTsrPricer::optionletPrice(Option::Type optionType,
                                         Real strike) const {

        if (optionType == Option::Call && strike >= adjustedUpperBound_)
            return 0.0;
        if (optionType == Option::Put && strike <= adjustedLowerBound_)
            return 0.0;

        // determine lower or upper integration bound (depending on option type)

        Real lower = strike, upper = strike;

        switch (settings_.strategy_) {

        case Settings::RateBound: {
            if (optionType == Option::Call)
                upper = adjustedUpperBound_;
            else
                lower = adjustedLowerBound_;
            break;
        }

        case Settings::VegaRatio: {
            // strikeFromVegaRatio ensures that returned strike is on the
            // expected side of strike
            Real bound =
                strikeFromVegaRatio(settings_.vegaRatio_, optionType, strike);
            if (optionType == Option::Call)
                upper = std::min(bound, adjustedUpperBound_);
            else
                lower = std::max(bound, adjustedLowerBound_);
            break;
        }

        case Settings::PriceThreshold: {
            // strikeFromPrice ensures that returned strike is on the expected
            // side of strike
            Real bound =
                strikeFromPrice(settings_.vegaRatio_, optionType, strike);
            if (optionType == Option::Call)
                upper = std::min(bound, adjustedUpperBound_);
            else
                lower = std::max(bound, adjustedLowerBound_);
            break;
        }

        case Settings::BSStdDevs : {
            Real atm = smileSection_->atmLevel();
            Real atmVol = smileSection_->volatility(atm);
            Real shift = smileSection_->shift();
            Real lowerTmp, upperTmp;
            if (smileSection_->volatilityType() == ShiftedLognormal) {
                upperTmp = (atm + shift) *
                               std::exp(settings_.stdDevs_ * atmVol -
                                        0.5 * atmVol * atmVol *
                                            smileSection_->exerciseTime()) -
                           shift;
                lowerTmp = (atm + shift) *
                               std::exp(-settings_.stdDevs_ * atmVol -
                                        0.5 * atmVol * atmVol *
                                            smileSection_->exerciseTime()) -
                           shift;
            } else {
                Real tmp = settings_.stdDevs_ * atmVol *
                           std::sqrt(smileSection_->exerciseTime());
                upperTmp = atm + tmp;
                lowerTmp = atm - tmp;
            }
            upper = std::min(upperTmp - shift, adjustedUpperBound_);
            lower = std::max(lowerTmp - shift, adjustedLowerBound_);
            break;
        }

        default:
            QL_FAIL("Unknown strategy (" << settings_.strategy_ << ")");
        }

        // compute the relevant integral

        Real result = 0.0;
        Real tmpBound;
        if (upper > lower) {
            tmpBound = std::min(upper, swapRateValue_);
            if (tmpBound > lower) {
                result += integrator_->operator()(
                    std::bind1st(std::mem_fun(&LinearTsrPricer::integrand),
                                 this),
                    lower, tmpBound);
            }
            tmpBound = std::max(lower, swapRateValue_);
            if (upper > tmpBound) {
                result += integrator_->operator()(
                    std::bind1st(std::mem_fun(&LinearTsrPricer::integrand),
                                 this),
                    tmpBound, upper);
            }
            result *= (optionType == Option::Call ? 1.0 : -1.0);
        }

        result += singularTerms(optionType, strike);

        return annuity_ * result * couponDiscountRatio_ *
               coupon_->accrualPeriod();
    }

    Real LinearTsrPricer::meanReversion() const { return meanReversion_->value(); }

    Rate LinearTsrPricer::swapletRate() const {
        return swapletPrice() /
               (coupon_->accrualPeriod() *
                discountCurve_->discount(paymentDate_) * couponDiscountRatio_);
    }

    Real LinearTsrPricer::capletPrice(Rate effectiveCap) const {
        // caplet is equivalent to call option on fixing
        if (fixingDate_ <= today_) {
            // the fixing is determined
            const Rate Rs = std::max(
                coupon_->swapIndex()->fixing(fixingDate_) - effectiveCap, 0.);
            Rate price =
                (gearing_ * Rs) *
                (coupon_->accrualPeriod() *
                 discountCurve_->discount(paymentDate_) * couponDiscountRatio_);
            return price;
        } else {
            Real capletPrice = optionletPrice(Option::Call, effectiveCap);
            return gearing_ * capletPrice;
        }
    }

    Rate LinearTsrPricer::capletRate(Rate effectiveCap) const {
        return capletPrice(effectiveCap) /
               (coupon_->accrualPeriod() *
                discountCurve_->discount(paymentDate_) * couponDiscountRatio_);
    }

    Real LinearTsrPricer::floorletPrice(Rate effectiveFloor) const {
        // floorlet is equivalent to put option on fixing
        if (fixingDate_ <= today_) {
            // the fixing is determined
            const Rate Rs = std::max(
                effectiveFloor - coupon_->swapIndex()->fixing(fixingDate_), 0.);
            Rate price =
                (gearing_ * Rs) *
                (coupon_->accrualPeriod() *
                 discountCurve_->discount(paymentDate_) * couponDiscountRatio_);
            return price;
        } else {
            Real floorletPrice = optionletPrice(Option::Put, effectiveFloor);
            return gearing_ * floorletPrice;
        }
    }

    Rate LinearTsrPricer::floorletRate(Rate effectiveFloor) const {
        return floorletPrice(effectiveFloor) /
               (coupon_->accrualPeriod() *
                discountCurve_->discount(paymentDate_) * couponDiscountRatio_);
    }

    Real LinearTsrPricer::swapletPrice() const {
        if (fixingDate_ <= today_) {
            // the fixing is determined
            const Rate Rs = coupon_->swapIndex()->fixing(fixingDate_);
            Rate price =
                (gearing_ * Rs + spread_) *
                (coupon_->accrualPeriod() *
                 discountCurve_->discount(paymentDate_) * couponDiscountRatio_);
            return price;
        } else {
            Real atmCapletPrice = optionletPrice(Option::Call, swapRateValue_);
            Real atmFloorletPrice = optionletPrice(Option::Put, swapRateValue_);
            return gearing_ * (coupon_->accrualPeriod() *
                                   discountCurve_->discount(paymentDate_) *
                                   swapRateValue_ * couponDiscountRatio_ +
                               atmCapletPrice - atmFloorletPrice) +
                   spreadLegValue_;
        }
    }
}