#include "optimizer.h"

#include <random>
#include <cassert>
#include <iomanip>
#include <iostream>
#include <sstream>

using namespace LiteMath;

struct OptComplex : public IOptimizer
{
  OptComplex(){}

  void         Init(const TriangleMesh& a_mesh, const Img& a_image) override;
  TriangleMesh Run (size_t a_numIters = 100) override;

  TriangleMesh g_mesh; ///<! global mesh optimized mesh
  Img          g_targetImage;
  size_t       g_iter = 0;
};

IOptimizer* CreateComplexOptimizer() { return new OptComplex; };

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include <Eigen/Dense>              // optimization methods
#define OPTIM_ENABLE_EIGEN_WRAPPERS // optimization methods
#include "optim.hpp"                // optimization methods

typedef Eigen::Matrix<double, Eigen::Dynamic, 1> EVector;

EVector VectorFromMesh(const TriangleMesh& a_mesh)
{
  EVector result( a_mesh.vertices.size()*2 + a_mesh.colors.size()*3 );
  size_t currPos = 0;
  for(size_t vertId=0; vertId< a_mesh.vertices.size(); vertId++, currPos+=2)
  {
    result[currPos+0] = a_mesh.vertices[vertId].x;
    result[currPos+1] = a_mesh.vertices[vertId].y;
  }
  for(size_t faceId=0; faceId < a_mesh.colors.size(); faceId++, currPos+=3)
  {
   result[currPos+0] = a_mesh.colors[faceId].x;
   result[currPos+1] = a_mesh.colors[faceId].y;
   result[currPos+2] = a_mesh.colors[faceId].z;
  }
  return result;
}

constexpr float alphaPos   = 0.1f;
constexpr float alphaColor = 0.00001f;

EVector VectorFromDMesh(const DTriangleMesh& a_mesh)
{
  EVector result(a_mesh.totalParams());
  size_t currPos = 0;
  for(int vertId=0; vertId< a_mesh.numVertices(); vertId++, currPos+=2)
  {
    result[currPos+0] = a_mesh.vertices()[vertId].x*alphaPos;
    result[currPos+1] = a_mesh.vertices()[vertId].y*alphaPos;
  }
  for(int faceId=0; faceId < a_mesh.numFaces(); faceId++, currPos+=3)
  {
    result[currPos+0] = a_mesh.faceColors()[faceId].x*alphaColor;
    result[currPos+1] = a_mesh.faceColors()[faceId].y*alphaColor;
    result[currPos+2] = a_mesh.faceColors()[faceId].z*alphaColor;
  }
  return result;
}

TriangleMesh MeshFromVector(const EVector& a_vec, const TriangleMesh& a_mesh)
{
  TriangleMesh result = a_mesh;
  size_t currPos = 0;
  for(size_t vertId=0; vertId< result.vertices.size(); vertId++, currPos+=2)
  {
    result.vertices[vertId].x = a_vec[currPos+0];
    result.vertices[vertId].y = a_vec[currPos+1];
  }
  for(size_t faceId=0; faceId < result.colors.size(); faceId++, currPos+=3)
  {
    result.colors[faceId].x = a_vec[currPos+0];
    result.colors[faceId].y = a_vec[currPos+1];
    result.colors[faceId].z = a_vec[currPos+2];
  }
  return result;
}

float EvalFunction(const EVector& vals_inp, EVector* grad_out, void* opt_data)
{
  OptComplex* pObj = (OptComplex*)opt_data;

  TriangleMesh mesh = MeshFromVector(vals_inp, pObj->g_mesh);

  constexpr int samples_per_pixel = 4;

  Img img(256, 256);
  std::mt19937 rng(1234);
  render(mesh, samples_per_pixel, rng, img);

  std::stringstream strOut;
  strOut  << "rendered_opt/render_" << std::setfill('0') << std::setw(4) << pObj->g_iter << ".bmp";
  save_img(img, strOut.str());

  Img adjoint(img.width, img.height, float3{1, 1, 1});
  float mse = MSEAndDiff(img, pObj->g_targetImage, adjoint);
  Img dx(img.width, img.height), dy(img.width, img.height); // actually not needed here

  DTriangleMesh d_mesh(mesh.vertices.size(), mesh.colors.size());
  d_render(mesh, adjoint, samples_per_pixel, img.width * img.height , rng, dx, dy, d_mesh);

  std::cout << "iter " << pObj->g_iter << ", error = " << mse << std::endl;
  (*grad_out) = VectorFromDMesh(d_mesh); // apply 2.0f*summ(I[x,y] - I_target[x,y]) to get correct gradient for target image
  pObj->g_iter++;
  return mse;
}

void OptComplex::Init(const TriangleMesh& a_mesh, const Img& a_image)
{
  g_mesh        = a_mesh;
  g_targetImage = a_image;
  g_iter        = 0;
}

TriangleMesh OptComplex::Run(size_t a_numIters)
{
  optim::algo_settings_t settings;
  settings.iter_max = a_numIters;
  settings.gd_settings.method = 0; // 0 for simple gradient descend, 6 ADAM
  settings.gd_settings.par_step_size      = 1.0; // initialization for ADAM
  settings.gd_settings.step_decay         = true;
  settings.gd_settings.step_decay_periods = a_numIters/10;
  settings.gd_settings.step_decay_val     = 0.75f;
  settings.opt_error_value                = 20.0f;

  EVector x = VectorFromMesh(g_mesh);
  bool success = optim::gd(x, &EvalFunction, this, settings);
  std::cout << "OptComplex, optimization is FINISHED!" << std::endl;

  return MeshFromVector(x, g_mesh);
}


/*
Copyright (c) 2015, Sigurd Storve
All rights reserved.

Licensed under the BSD license.

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 the <organization> 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 <COPYRIGHT HOLDER> 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.
*/

#pragma once
#include <cmath>
#include <boost/numeric/ublas/matrix.hpp>

// Create matrix for rotating around the x-axis.
template <typename T>
boost::numeric::ublas::matrix<T> rotation_matrix_x(T angle) {
    boost::numeric::ublas::matrix<T> m(3, 3);
    m(0,0) = static_cast<T>(1.0); m(0,1) = static_cast<T>(0.0);             m(0,2) = static_cast<T>(0.0);
    m(1,0) = static_cast<T>(0.0); m(1,1) = static_cast<T>(std::cos(angle)); m(1,2) = static_cast<T>(-std::sin(angle));
    m(2,0) = static_cast<T>(0.0); m(2,1) = static_cast<T>(std::sin(angle)); m(2,2) = static_cast<T>(std::cos(angle));
    return m;
}

// Create matrix for rotating around the y-axis
template <typename T>
boost::numeric::ublas::matrix<T> rotation_matrix_y(T angle) {
    boost::numeric::ublas::matrix<T> m(3, 3);
    m(0,0) = static_cast<T>(std::cos(angle));  m(0,1) = static_cast<T>(0.0); m(0,2) = static_cast<T>(std::sin(angle));
    m(1,0) = static_cast<T>(0.0);              m(1,1) = static_cast<T>(1.0); m(1,2) = static_cast<T>(0.0);
    m(2,0) = static_cast<T>(-std::sin(angle)); m(2,1) = static_cast<T>(0.0); m(2,2) = static_cast<T>(std::cos(angle));
    return m;
}

// Create matrix for rotating around the z-axis
template <typename T>
boost::numeric::ublas::matrix<T> rotation_matrix_z(T angle) {
    boost::numeric::ublas::matrix<T> m(3, 3);
    m(0,0) = static_cast<T>(std::cos(angle));  m(0,1) = static_cast<T>(-std::sin(angle)); m(0,2) = static_cast<T>(0.0);
    m(1,0) = static_cast<T>(std::sin(angle));  m(1,1) = static_cast<T>(std::cos(angle));  m(1,2) = static_cast<T>(0.0);
    m(2,0) = static_cast<T>(0.0);              m(2,1) = static_cast<T>(0.0);              m(2,2) = static_cast<T>(1.0);
    return m;
}

// Create matrix for rotation along x, y, z, in that order
template <typename T>
boost::numeric::ublas::matrix<T> rotation_matrix_xyz(T x_angle, T y_angle, T z_angle) {
    using namespace boost::numeric::ublas;
    const auto rot_x = rotation_matrix_x<double>(x_angle);
    const auto rot_y = rotation_matrix_y<double>(y_angle);
    const auto rot_z = rotation_matrix_z<double>(z_angle);
    boost::numeric::ublas::matrix<T> temp = prod(rot_z, rot_y);
    return prod(temp, rot_x);
}

// Return unit vector along the x-axis
template <typename T>
boost::numeric::ublas::vector<T> unit_x() {
    boost::numeric::ublas::vector<T> u(3);
    u(0) = static_cast<T>(1.0); u(1) = static_cast<T>(0.0); u(2) = static_cast<T>(0.0);
    return u;
}

// Return unit vector along the y-axis
template <typename T>
boost::numeric::ublas::vector<T> unit_y() {
    boost::numeric::ublas::vector<T> u(3);
    u(0) = static_cast<T>(0.0); u(1) = static_cast<T>(1.0); u(2) = static_cast<T>(0.0);
    return u;
}

// Return unit vector along the z-axis
template <typename T>
boost::numeric::ublas::vector<T> unit_z() {
    boost::numeric::ublas::vector<T> u(3);
    u(0) = static_cast<T>(0.0); u(1) = static_cast<T>(0.0); u(2) = static_cast<T>(1.0);
    return u;
}


/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Col;
using arma::Mat;
using arma::uword;
using arma::cross;
using arma::join_rows;
using arma::join_horiz;
using arma::join_cols;
using arma::join_vert;
using arma::kron;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedGenColVecGenMat : public Expected {
    public:
      ExpectedGenColVecGenMat() {
        cout << "Compute ExpectedGenColVecGenMat(): " << endl;

        vector<vector<pair<string, void*>>> inputs = Input::getTestParameters({
          InputClass::GenColVec,
          InputClass::GenMat
        });

        for (vector<pair<string, void*>> input : inputs) {
          _fileSuffix = "";

          int n = 0;
          for (pair<string, void*> value : input) {
            switch (n) {
              case 0:
                _fileSuffix += value.first;
                _genColVec = *static_cast<Col<double>*>(value.second);
                break;
              case 1:
                _fileSuffix += "," + value.first;
                _genMat = *static_cast<Mat<double>*>(value.second);
                break;
            }
            ++n;
          }

          cout << "Using input: " << _fileSuffix << endl;

          expectedArmaCross();
          expectedArmaJoin_rows();
          expectedArmaJoin_horiz();
          expectedArmaJoin_cols();
          expectedArmaJoin_vert();
          expectedArmaKron();
          expectedColPlus();
          expectedColMinus();
          expectedColTimes();
          expectedColElemTimes();
          expectedColElemDivide();
          expectedColEquals();
          expectedColNonEquals();
          expectedColGreaterThan();
          expectedColLessThan();
          expectedColStrictGreaterThan();
          expectedColStrictLessThan();
        }

        cout << "done." << endl;
      }

    protected:
      Col<double> _genColVec;
      Mat<double> _genMat;

      void expectedArmaCross() {
        cout << "- Compute expectedArmaCross() ... ";

        Col<double> tempGenColVec = Col<double>(_genColVec);
        tempGenColVec.resize(3);
        Mat<double> tempGenMat = Mat<double>(_genMat);
        tempGenMat.resize(3, 1);

        save<double>("Arma.cross", cross(tempGenColVec, tempGenMat));

        cout << "done." << endl;
      }

      void expectedArmaJoin_rows() {
        if(_genColVec.n_rows != _genMat.n_rows) {
          return;
        }

        cout << "- Compute expectedArmaJoin_rows() ... ";
        save<double>("Arma.join_rows", join_rows(_genColVec, _genMat));
        cout << "done." << endl;
      }

      void expectedArmaJoin_horiz() {
        if(_genColVec.n_rows != _genMat.n_rows) {
          return;
        }

        cout << "- Compute expectedArmaJoin_horiz() ... ";
        save<double>("Arma.join_horiz", join_horiz(_genColVec, _genMat));
        cout << "done." << endl;
      }

      void expectedArmaJoin_cols() {
        if(_genColVec.n_cols != _genMat.n_cols) {
          return;
        }

        cout << "- Compute expectedArmaJoin_cols() ... ";
        save<double>("Arma.join_cols", join_cols(_genColVec, _genMat));
        cout << "done." << endl;
      }

      void expectedArmaJoin_vert() {
        if(_genColVec.n_cols != _genMat.n_cols) {
          return;
        }

        cout << "- Compute expectedArmaJoin_vert() ... ";
        save<double>("Arma.join_vert", join_cols(_genColVec, _genMat));
        cout << "done." << endl;
      }

      void expectedArmaKron() {
        cout << "- Compute expectedArmaKron() ... ";
        save<double>("Arma.kron", kron(_genColVec, _genMat));
        cout << "done." << endl;
      }

      void expectedColPlus() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedColPlus() ... ";
        save<double>("Col.plus", _genColVec + _genMat);
        cout << "done." << endl;
      }

      void expectedColMinus() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedColMinus() ... ";
        save<double>("Col.minus", _genColVec - _genMat);
        cout << "done." << endl;
      }

      void expectedColTimes() {
        if(_genMat.n_rows != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedColTimes() ... ";
        save<double>("Col.times", _genColVec * _genMat);
        cout << "done." << endl;
      }

      void expectedColElemTimes() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedColElemTimes() ... ";
        save<double>("Col.elemTimes", _genColVec % _genMat);
        cout << "done." << endl;
      }

      void expectedColElemDivide() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedColElemDivide() ... ";
        save<double>("Col.elemDivide", _genColVec / _genMat);
        cout << "done." << endl;
      }

      void expectedColEquals() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedColEquals() ... ";

        Col<uword> expected = _genColVec == _genMat;
        save<uword>("Col.equals", expected);

        cout << "done." << endl;
      }

      void expectedColNonEquals() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedColVecNonEquals() ... ";

        Col<uword> expected = _genColVec != _genMat;
        save<uword>("Col.nonEquals", expected);

        cout << "done." << endl;
      }

      void expectedColGreaterThan() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedColGreaterThan() ... ";

        Col<uword> expected = _genColVec >= _genMat;
        save<uword>("Col.greaterThan", expected);

        cout << "done." << endl;
      }

      void expectedColLessThan() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedColLessThan() ... ";

        Col<uword> expected = _genColVec <= _genMat;
        save<uword>("Col.lessThan", expected);

        cout << "done." << endl;
      }

      void expectedColStrictGreaterThan() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedColStrictGreaterThan() ... ";

        Col<uword> expected = _genColVec > _genMat;
        save<uword>("Col.strictGreaterThan", expected);

        cout << "done." << endl;
      }

      void expectedColStrictLessThan() {
        if(_genMat.n_rows != _genColVec.n_rows) {
          return;
        }

        if(_genMat.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedColElemDivide() ... ";

        Col<uword> expected = _genColVec < _genMat;
        save<uword>("Col.strictLessThan", expected);

        cout << "done." << endl;
      }
  };
}


#define DEBUG 1
/**
 * File    : F.cpp
 * Author  : Kazune Takahashi
 * Created : 2020/7/1 1:44:15
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <climits>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
// ----- boost -----
#include <boost/integer/common_factor_rt.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/rational.hpp>
// ----- using directives and manipulations -----
using namespace std;
using boost::rational;
using boost::integer::gcd; // for C++14 or for cpp_int
using boost::integer::lcm; // for C++14 or for cpp_int
using boost::multiprecision::cpp_int;
using ll = unsigned long long;
using ld = long double;
template <typename T>
using max_heap = priority_queue<T>;
template <typename T>
using min_heap = priority_queue<T, vector<T>, greater<T>>;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1'000'000'007LL};
// constexpr ll MOD{998'244'353LL}; // be careful
constexpr ll MAX_SIZE{3'000'010LL};
// constexpr ll MAX_SIZE{30'000'010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
bool ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
    return true;
  }
  return false;
}
template <typename T>
bool ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
    return true;
  }
  return false;
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{(x % MOD + MOD) % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(Mint const &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(Mint const &a) { return *this += -a; }
  Mint &operator++() { return *this += 1; }
  Mint operator++(int)
  {
    Mint tmp{*this};
    ++*this;
    return tmp;
  }
  Mint &operator--() { return *this -= 1; }
  Mint operator--(int)
  {
    Mint tmp{*this};
    --*this;
    return tmp;
  }
  Mint &operator*=(Mint const &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(Mint const &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(Mint const &a) const { return Mint(*this) += a; }
  Mint operator-(Mint const &a) const { return Mint(*this) -= a; }
  Mint operator*(Mint const &a) const { return Mint(*this) *= a; }
  Mint operator/(Mint const &a) const { return Mint(*this) /= a; }
  bool operator<(Mint const &a) const { return x < a.x; }
  bool operator<=(Mint const &a) const { return x <= a.x; }
  bool operator>(Mint const &a) const { return x > a.x; }
  bool operator>=(Mint const &a) const { return x >= a.x; }
  bool operator==(Mint const &a) const { return x == a.x; }
  bool operator!=(Mint const &a) const { return !(*this == a); }
  Mint power(ll N) const
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, Mint<MOD> const &rhs) { return rhs + lhs; }
template <ll MOD>
Mint<MOD> operator-(ll lhs, Mint<MOD> const &rhs) { return -rhs + lhs; }
template <ll MOD>
Mint<MOD> operator*(ll lhs, Mint<MOD> const &rhs) { return rhs * lhs; }
template <ll MOD>
Mint<MOD> operator/(ll lhs, Mint<MOD> const &rhs) { return Mint<MOD>{lhs} / rhs; }
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a) { return stream >> a.x; }
template <ll MOD>
ostream &operator<<(ostream &stream, Mint<MOD> const &a) { return stream << a.x; }
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i{2LL}; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i{1LL}; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
// ----- for C++17 -----
template <typename T, typename enable_if<is_integral<T>::value>::type * = nullptr>
size_t popcount(T x) { return bitset<64>(x).count(); }
size_t popcount(string const &S) { return bitset<200010>{S}.count(); }
// ----- Infty -----
template <typename T>
constexpr T Infty() { return numeric_limits<T>::max(); }
template <typename T>
constexpr T mInfty() { return numeric_limits<T>::min(); }
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1'000'000'000'000'010LL}; // or
// constexpr int infty{1'000'000'010};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "-1" << endl;
  exit(0);
}

// ----- Solve -----

template <typename T>
ostream &operator<<(ostream &os, vector<vector<T>> const &v)
{
  int n = v.size();
  for (auto i{0}; i < n; ++i)
  {
    for (auto j{0}; j < n; ++j)
    {
      os << v[i][j];
      if (j < n - 1)
      {
        os << " ";
      }
      else
      {
        os << endl;
      }
    }
  }
  return os;
}

class Solve
{
  int n;
  vector<bool> s, t, u, v;
  vector<vector<int>> res;

public:
  Solve(int n, vector<bool> s, vector<bool> t, vector<bool> u, vector<bool> v) : n{n}, s{s}, t{t}, u{u}, v{v}, res(n, vector<int>(n, -1))
  {
#if DEBUG == 1
    cerr << "problem: " << endl;
    for (auto i{0}; i < n; ++i)
    {
      cerr << "s[" << i << "] = " << s[i] << endl;
    }
    for (auto i{0}; i < n; ++i)
    {
      cerr << "t[" << i << "] = " << t[i] << endl;
    }
    for (auto i{0}; i < n; ++i)
    {
      cerr << "u[" << i << "] = " << u[i] << endl;
    }
    for (auto i{0}; i < n; ++i)
    {
      cerr << "v[" << i << "] = " << v[i] << endl;
    }
#endif
  }

  vector<vector<int>> answer()
  {
    prepare();
    fill_zero();
#if DEBUG == 1
    cerr << res;
#endif
    fixed_up();
#if DEBUG == 1
    cerr << res;
#endif
    final_check();
    return res;
  }

private:
  void ch(int i, int j, int v)
  {
    if (res[i][j] == -1)
    {
      res[i][j] = v;
    }
    else if (res[i][j] != v)
    {
      No();
    }
  }

  void final_check()
  {
    for (auto i{0}; i < n; ++i)
    {
      int tmp{res[i][0]};
      if (!s[i])
      {
        for (auto j{0}; j < n; ++j)
        {
          tmp &= res[i][j];
        }
      }
      else
      {
        for (auto j{0}; j < n; ++j)
        {
          tmp |= res[i][j];
        }
      }
      if (tmp != u[i])
      {
        No();
      }
    }
    for (auto j{0}; j < n; ++j)
    {
      int tmp{res[0][j]};
      if (!t[j])
      {
        for (auto i{0}; i < n; ++i)
        {
          tmp &= res[i][j];
        }
      }
      else
      {
        for (auto i{0}; i < n; ++i)
        {
          tmp |= res[i][j];
        }
      }
      if (tmp != v[j])
      {
        No();
      }
    }
  }

  void fixed_up()
  {
    for (auto i{0}; i < n; ++i)
    {
      if (!(s[i] && (u[i] & 1)))
      {
        continue;
      }
      bool ok{false};
      for (auto j{0}; j < n; ++j)
      {
        if (res[i][j] == 1)
        {
          ok = true;
          break;
        }
      }
      if (ok)
      {
        continue;
      }
      for (auto j{0}; j < n; ++j)
      {
        if (!(!t[j] && !(v[j] & 1)))
        {
          continue;
        }
        int cnt{0};
        for (auto k{0}; k < n; ++k)
        {
          if (res[k][j] == 0)
          {
            ++cnt;
          }
        }
        if (cnt <= 1)
        {
          continue;
        }
        res[i][j] = 1;
        ok = true;
        break;
      }
      if (!ok)
      {
        No();
      }
    }
    for (auto j{0}; j < n; ++j)
    {
      if (!(t[j] && (v[j] & 1)))
      {
        continue;
      }
#if DEBUG == 1
      cerr << "j = " << j << endl;
#endif
      bool ok{false};
      for (auto i{0}; i < n; ++i)
      {
        if (res[i][j] == 1)
        {
          ok = true;
          break;
        }
      }
      if (ok)
      {
        continue;
      }
#if DEBUG == 1
      cerr << "j = " << j << endl;
#endif
      for (auto i{0}; i < n; ++i)
      {
        if (!(!s[i] && !(u[i] & 1)))
        {
          continue;
        }
#if DEBUG == 1
        cerr << "i = " << i << endl;
#endif
        int cnt{0};
        for (auto k{0}; k < n; ++k)
        {
          if (res[i][k] == 0)
          {
            ++cnt;
          }
        }
        if (cnt <= 1)
        {
          continue;
        }
        res[i][j] = 1;
        ok = true;
        break;
      }
      if (!ok)
      {
        No();
      }
    }
  }

  void fill_zero()
  {
    for (auto i{0}; i < n; ++i)
    {
      for (auto j{0}; j < n; ++j)
      {
        if (res[i][j] == -1)
        {
          res[i][j] = 0;
        }
      }
    }
  }

  void prepare()
  {
    for (auto i{0}; i < n; ++i)
    {
      if (!s[i] && u[i])
      {
        for (auto j{0}; j < n; ++j)
        {
          ch(i, j, 1);
        }
      }
      else if (s[i] && !u[i])
      {
        for (auto j{0}; j < n; ++j)
        {
          ch(i, j, 0);
        }
      }
    }
    for (auto i{0}; i < n; ++i)
    {
      if (!t[i] && v[i])
      {
        for (auto j{0}; j < n; ++j)
        {
          ch(j, i, 1);
        }
      }
      else if (t[i] && !v[i])
      {
        for (auto j{0}; j < n; ++j)
        {
          ch(j, i, 0);
        }
      }
    }
    for (auto i{0}; i < n; ++i)
    {
      for (auto j{0}; j < n; ++j)
      {
        if (res[i][j] == -1)
        {
          if (u[i] && v[j])
          {
            res[i][j] = 1;
          }
          else if (!u[i] && !v[j])
          {
            res[i][j] = 0;
          }
        }
      }
    }
  }
};

// ----- main() -----

constexpr int C{64};

int main()
{
  int n;
  cin >> n;
  vector<bool> s(n), t(n);
  vector<ll> us(n), vs(n);
  for (auto i{0}; i < n; ++i)
  {
    int x;
    cin >> x;
    s[i] = x;
  }
  for (auto i{0}; i < n; ++i)
  {
    int x;
    cin >> x;
    t[i] = x;
  }
  for (auto i{0}; i < n; ++i)
  {
    cin >> us[i];
  }
  for (auto i{0}; i < n; ++i)
  {
    cin >> vs[i];
  }
  vector<vector<ll>> ans(n, vector<ll>(n, 0));
  for (auto k{0}; k < C; ++k)
  {
    vector<bool> u(n), v(n);
    for (auto i{0}; i < n; ++i)
    {
      u[i] = (us[i] >> k & 1);
    }
    for (auto i{0}; i < n; ++i)
    {
      v[i] = (vs[i] >> k & 1);
    }
    Solve solve(n, s, t, u, v);
    auto res{solve.answer()};
    for (auto i{0}; i < n; ++i)
    {
      for (auto j{0}; j < n; ++j)
      {
        ans[i][j] |= static_cast<ll>(res[i][j]) << k;
      }
    }
#if DEBUG == 1
    cerr << ans;
#endif
    for (auto i{0}; i < n; ++i)
    {
      auto tmp{ans[i][0]};
      if (!s[i])
      {
        for (auto j{0}; j < n; ++j)
        {
          tmp &= ans[i][j];
        }
      }
      else
      {
        for (auto j{0}; j < n; ++j)
        {
          tmp |= ans[i][j];
        }
      }
      auto cut{us[i]};
      if (k < C - 1)
      {
        cut &= (1ULL << (k + 1)) - 1;
      }
      if (tmp != cut)
      {
#if DEBUG == 1
        cerr << "failed: k = " << k << ", i = " << i << endl;
#endif
        No();
      }
    }
    for (auto j{0}; j < n; ++j)
    {
      auto tmp{ans[0][j]};
      if (!t[j])
      {
        for (auto i{0}; i < n; ++i)
        {
          tmp &= ans[i][j];
        }
      }
      else
      {
        for (auto i{0}; i < n; ++i)
        {
          tmp |= ans[i][j];
        }
      }
      auto cut{vs[j]};
      if (k < C - 1)
      {
        cut &= (1ULL << (k + 1)) - 1;
      }
      if (tmp != cut)
      {
#if DEBUG == 1
        cerr << "failed: k = " << k << ", j = " << j << endl;
#endif
        No();
      }
    }
  }
  for (auto i{0}; i < n; ++i)
  {
    for (auto j{0}; j < n; ++j)
    {
      cout << ans[i][j];
      if (j < n - 1)
      {
        cout << " ";
      }
      else
      {
        cout << endl;
      }
    }
  }
}


/*
 main.cxx

 Copyright (c) 2018 Guy Skinner

 This file is distributed under the terms of the MIT license.
 Please see the file 'LICENCE.txt' in the root directory
 or http://opensource.org/licenses/mit-license.php for information.
*/

#include <boost/format.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/range/algorithm.hpp>
#include <cmath>
#include <fstream>
#include <iostream>

#include "correlation.hxx"
#include "neighbour.hxx"
#include "supercell.hxx"
#include "utils.hxx"

namespace ublas = boost::numeric::ublas;

int main(void) {

  unsigned long nbas = 2;
  ublas::matrix<double> plat(3,3);
  ublas::matrix<double> basis(nbas,3);
  double rmax = 2.0;

  /* Set Basis Vectors */
  double isqrt2 = 1/std::sqrt(2);
  double isqrt3 = 1/std::sqrt(3);
  double isqrt6 = 1/std::sqrt(6);

  basis(0,0) =      0.0; basis(0,1) =          0.0; basis(0,2) =            0.0;
  basis(1,0) =   isqrt6; basis(1,1) =       isqrt2; basis(1,2) =         isqrt3;

  plat(0,0)  = 2*isqrt6; plat(0,1)  =          0.0; plat(0,2)  = std::sqrt(3)/6;
  plat(1,0)  =      0.0; plat(1,1)  = std::sqrt(2); plat(1,2)  =            0.0;
  plat(2,0)  =      0.0; plat(2,1)  =          0.0; plat(2,2)  = std::sqrt(3)/2;

  /* Primitive I/O */
  std::cout << boost::format("Primitive Number of Atoms : %8d\n") % nbas;
  std::cout << boost::format("Pair Cut Off Radius       : %16.7f\n") % rmax;
  std::cout << boost::format("Primitive Lattice Vectors : %16.7f %16.7f %16.7f\n")
    % plat(0,0) % plat(0,1) % plat(0,2);
  std::cout << boost::format("                            %16.7f %16.7f %16.7f\n")
    % plat(1,0) % plat(1,1) % plat(1,2);
  std::cout << boost::format("                            %16.7f %16.7f %16.7f\n")
    % plat(2,0) % plat(2,1) % plat(2,2);

  for (auto i = 0; i < nbas; i++) {
    ublas::matrix_row<ublas::matrix<double>> b(basis,i);
    if (i == 0) {
      std::cout << boost::format("Primitive Basis Vectors   : %16.7f %16.7f %16.7f\n")
	% b(0) % b(1) % b(2);
    } else {
      std::cout << boost::format("                          : %16.7f %16.7f %16.7f\n")
        % b(0) % b(1) % b(2);
    }
  }

  /* Supercell Creation */
  ublas::vector<long> sext(3);
  sext(0) = 100; sext(1) = 60; sext(2) = 3;

  double concentration = 0.05;

  Supercell supercell = Supercell(sext,nbas,plat,basis,concentration);

  /* Supercell Inverse */
  ublas::matrix<double> isplat = inv3(supercell.lattice_vectors);
  ublas::matrix<double> xsbasis(supercell.number_of_atoms,3);
  for (auto i = 0; i < supercell.number_of_atoms; i++) {
    ublas::matrix_row<ublas::matrix<double>> b(supercell.basis_vectors,i);
    auto xb = prod(b,isplat);
    for (auto a = 0; a < 3; a++) {
      xsbasis(i,a) = xb(a);
    }
  }

  /* Supercell I/O */
  std::cout << boost::format("Supercell Number of Atoms : %8d\n")
    % supercell.number_of_atoms;

  std::cout << boost::format("Supercell Lattice Vectors : %16.7f %16.7f %16.7f\n")
    % supercell.lattice_vectors(0,0)
    % supercell.lattice_vectors(0,1)
    % supercell.lattice_vectors(0,2);
  std::cout << boost::format("                            %16.7f %16.7f %16.7f\n")
    % supercell.lattice_vectors(1,0)
    % supercell.lattice_vectors(1,1)
    % supercell.lattice_vectors(1,2);
  std::cout << boost::format("                            %16.7f %16.7f %16.7f\n")
    % supercell.lattice_vectors(2,0)
    % supercell.lattice_vectors(2,1)
    % supercell.lattice_vectors(2,2);

  /* Random Number Generator */
  boost::mt19937 mt(time(0));
  boost::uniform_int<> uni_dist;
  boost::variate_generator<boost::mt19937&,boost::uniform_int<>>
    generator(mt,uni_dist);

  long nitf = 100000;
  double term = 0.0001;

  std::cout << boost::format("Number of Iterations      : %8d\n") % nitf;
  std::cout << boost::format("Concentration             : %16.7f\n") % concentration;
  std::cout << boost::format("Perfect Correlation       : %16.7f\n")
    % ((2*concentration-1)*(2*concentration-1));
  std::cout << boost::format("SQS Accept Tolerance      : %16.7f\n\n") % term;

  Neighbour neighbour = Neighbour(supercell.number_of_atoms,
				  supercell.lattice_vectors,
				  supercell.basis_vectors);

  neighbour.SetInverseLattice(isplat,xsbasis);
  neighbour.GetNhbrList(rmax);

  double best = 1.0;
  long snum = 0;
  long bnum = 0;

  for (long n = 0; n < nitf; n++) {

    boost::range::random_shuffle(supercell.pointers,generator);

    /* Correlation Function Calculation */
    Correlation correlation = Correlation();
    correlation.Calculate(neighbour,supercell);
    double result = correlation.ErrorFunction(concentration);

    bool bestq = (result < best);
    bool writeq = (result < term);
    if (bestq) {
      best = result;
      bnum = snum;
    }

    std::cout << boost::format("Iteration                 : %8d\n") % (n+1);
    std::cout << boost::format("Mean Error                : %16.7f\n")
      % result;
    std::cout << boost::format("Least Error      %8d : %16.7f\n") % bnum % best;
    if (writeq) {
      snum++;
      std::cout << "Accept\n";
    } else {
      std::cout << "Reject\n";
    }

    for (auto i = 0; i < correlation.number; i++) {
      std::cout << boost::format("  %16.7f  %5d  %16.7f  %16.7f\n")
	% correlation.pair_clusters[i]
	% (correlation.pair_count[i]/supercell.number_of_atoms)
	% correlation.pair_correlations[i]
	% correlation.errors[i];
    }
    std::cout << std::endl;

    /* File Writing */
    if (writeq) {
      std::ofstream outfile;
      boost::format fmt = boost::format("sqs.%d.out") % snum;
      outfile.open(fmt.str());
      outfile << boost::format("# special quasirandom structure\n");
      for (auto i = 0; i < supercell.number_of_atoms; i++) {
	long ptr = 1;
	if (supercell.pointers(i) == -1) ptr = 2;
	outfile << boost::format("%8d %22.15f %22.15f %22.15f\n")
	  % ptr
	  % supercell.basis_vectors(i,0)
	  % supercell.basis_vectors(i,1)
	  % supercell.basis_vectors(i,2);
      }
      outfile.close();
    }

  }

  return 0;

}


#include <algorithm>
#include <vector>
#include <fstream>

#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Surface_mesh.h>

#include <CGAL/box_intersection_d.h>
#include <CGAL/Timer.h>

#include <boost/bind.hpp>
#include <boost/functional/value_factory.hpp>
#include <boost/range/algorithm/transform.hpp>

#include <algorithm>
#include <vector>
#include <fstream>

typedef CGAL::Exact_predicates_exact_constructions_kernel K;

typedef K::Triangle_3 Triangle_3;
typedef K::Point_3 Point_3;
typedef CGAL::Surface_mesh<K::Point_3> Mesh;
typedef CGAL::Bbox_3 Bbox_3;
typedef CGAL::Timer Timer;

typedef Mesh::Face_index Face_descriptor;
typedef Mesh::Halfedge_index Halfedge_descriptor;

/// small helper to extract a triangle from a face
Triangle_3 triangle(const Mesh& sm, Face_descriptor f)
{
  Halfedge_descriptor hf = sm.halfedge(f);
  Point_3 a = sm.point(sm.target(hf));
  hf = sm.next(hf);
  Point_3 b = sm.point(sm.target(hf));
  hf = sm.next(hf);
  Point_3 c = sm.point(sm.target(hf));
  hf = sm.next(hf);
  return Triangle_3(a, b, c);
}

class Box
  : public CGAL::Box_intersection_d::Box_d<double, 3,  CGAL::Box_intersection_d::ID_NONE> {
private:
  typedef CGAL::Box_intersection_d::Box_d<
    double, 3,  CGAL::Box_intersection_d::ID_NONE> Base;
  Face_descriptor fd;
public:
  typedef double                                   NT;
  typedef std::size_t                              ID;

  Box(Face_descriptor f, const Mesh& sm) : Base(triangle(sm, f).bbox()), fd(f) {}
  Box(const Bbox_3& b, Face_descriptor fd) : Base(b), fd(fd) {}
  Face_descriptor f() const { return fd; }
  ID  id() const { return static_cast<ID>(fd); }
};

struct Callback {
  Callback(const Mesh& P, const Mesh& Q, unsigned int& i)
    : P(P), Q(Q), count(i)
  {}

  void operator()(const Box* bp, const Box* bq) {
    Face_descriptor fp = bp->f();
    Triangle_3 tp = triangle(P, fp);

    Face_descriptor fq = bq->f();
    Triangle_3 tq = triangle(Q, fq);

    if(do_intersect( tp, tq)) {
      ++(count);
    }
  }

  const Mesh& P;
  const Mesh& Q;
  unsigned int& count;
};

const Box*
address_of_box(const Box& b)
{
  return &b;
}

unsigned int intersect(const Mesh& P, const Mesh& Q) {
  std::vector<Box> P_boxes, Q_boxes;
  std::vector<const Box*> P_box_ptr, Q_box_ptr;
  P_boxes.reserve(P.number_of_faces());
  P_box_ptr.reserve(P.number_of_faces());
  Q_boxes.reserve(Q.number_of_faces());
  Q_box_ptr.reserve(Q.number_of_faces());

  // build boxes and pointers to boxes
  for(auto f : P.faces())
    P_boxes.push_back( Box(f, P) );
  std::transform(P_boxes.begin(), P_boxes.end(), std::back_inserter(P_box_ptr),
                 &address_of_box);
  for(auto f : Q.faces())
    Q_boxes.push_back( Box(f, Q) );
  std::transform(Q_boxes.begin(), Q_boxes.end(), std::back_inserter(Q_box_ptr),
                 &address_of_box);

  unsigned int i = 0;
  Callback c(P,Q, i);
  CGAL::box_intersection_d(P_box_ptr.begin(), P_box_ptr.end(),
                           Q_box_ptr.begin(), Q_box_ptr.end(),
                           c);
  return i;
}

int main(int argc, char* argv[])
{
  if(argc < 3)
  {
    std::cerr << "Usage: do_intersect <mesh_1.off> <mesh_2.off>" << std::endl;
    return EXIT_FAILURE;
  }

  std::cout.precision(17);

  Mesh P, Q;
  if(!CGAL::read_polygon_mesh(argv[1], P) || !CGAL::read_polygon_mesh(argv[2], Q))
  {
    std::cerr << "Invalid input files." << std::endl;
    return EXIT_FAILURE;
  }

  Timer timer;
  timer.start();
  unsigned int num_intersections = intersect(P,Q);
  timer.stop();
  std::cout << "Counted " << num_intersections << " in "
            << timer.time() << " seconds." << std::endl;

  return 0;
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu> Licensed
 * under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <stdint.h>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <unsupported/Eigen/MatrixFunctions>

// CUDA runtime
#include <cuda_runtime.h>
// Utilities and system includes
//#include <helper_functions.h>
#include <nvidia/helper_cuda.h>

//#include <mmf/defines.h>
#include <mmf/sphereSimple.hpp>
#include <mmf/optimizationSO3.hpp>
//#include <mmf/timer.hpp>

using namespace Eigen;

extern void vMFCostFctAssignmentGPU(float *h_cost, float *d_cost,
  uint32_t *h_W, uint32_t *d_W, float *d_x, float* d_weights,
  uint32_t *d_z, float *d_mu, float* d_pi, int N);

namespace mmf{

// closed form solution for the vMF cost function.
class OptSO3vMFCF : public OptSO3
{
  public:
  OptSO3vMFCF(float *d_weights =NULL):
    OptSO3(1.,1.,0.1,d_weights), pi_(6), tauR_(1000) {
    Eigen::VectorXf pi = Eigen::VectorXf::Ones(6)/6.;
    pi_.set(pi);
  };

  virtual ~OptSO3vMFCF() { };

protected:
  jsc::GpuMatrix<float> pi_;
  float tauR_; // concentration of vMF on rotation from previous to current frame

  virtual float computeAssignment(Matrix3f& R, uint32_t& N);
  virtual float conjugateGradientCUDA_impl(Matrix3f& R, float res0,
    uint32_t N, uint32_t maxIter);
  virtual void conjugateGradientPostparation_impl(Matrix3f& R);
  virtual float conjugateGradientPreparation_impl(Matrix3f& R, uint32_t& N);
  /* evaluate cost function for a given assignment of npormals to axes */
  virtual float evalCostFunction(Matrix3f& R);
  /* compute Jacobian */
  virtual void computeJacobian(Matrix3f&J, Matrix3f& R, float N);
  virtual void init() {};
};

}


#include <iostream>
#include <Eigen/Dense>

/**
 * This is a highly distilled version of fcl::detail::boxBox2 from
 * https://github.com/flexible-collision-library/fcl/blob/master/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h
 * I have been unable to reduce it further and still reproduce the issue.
 */
double reproFunction(const Eigen::Matrix3d& R_in)
{
  const Eigen::Matrix3d R = R_in;

  Eigen::Matrix3d Q = R.cwiseAbs();

  if(R(1,2) < 2) {
    Eigen::Vector3d n{0, 1, R(1, 2)};
    double s2 = R(1,2);
    s2 /= n.norm();
  }
  return R(1, 2);
}

int main() {
  Eigen::Matrix3d R;
  R = Eigen::Matrix3d::Zero(3,3);

  // This fails - reproFunction(R) returns 0
  R(1, 2) = 0.7;
  double R12 = reproFunction(R);
  bool are_they_equal = (R12 == R(1,2));
  std::cout << "R12 == R(1,2): " << are_they_equal << std::endl;
  std::cout << "R12: " << R12 << std::endl;
  std::cout << "R(1, 2): " << R(1, 2) << std::endl;
}


/// @brief Constraint functions for use with constraint based collisions.

#pragma once

#include <Eigen/Core>

namespace Eigen {
typedef Matrix<double, 12, 1> Vector12d;
}

namespace IPC {

/// @brief Methods for computing the constraints of the optimization problem.
enum CollisionConstraintType {
    VOLUME, ///< @brief Volume of a tetrahedron formed by the vertices.
    GRAPHICS, ///< @brief Distance constraint from Harmon et al. [2008].
    NONSMOOTH_NEWMARK, ///< @brief Variation of volume constraints proposed by Kane et al. [1999].
    GAP_FUNCTION, ///< @brief Common method from computational mechanics [Wriggers 1995]
    CMR, ///< @brief Constraint manifold refinement from Otaduy et al. [2009].
    VERSCHOOR, ///< @brief Variant of standard graphics approach by Verschoor and Jalba [2019].
    STIV ///< @brief Space-Time Interference Volume of Harmon et al. [2011] and Lu et al. [2018].
};

/**
 * @brief Compute the collision constraint value.
 *
 * Triangle vertices expected in clockwise order.
 *
 * @param[in]  v0_t0           First vertex at the start of time-step
 * @param[in]  v1_t0           Second vertex at the start of time-step
 * @param[in]  v2_t0           Third vertex at the start of time-step
 * @param[in]  v3_t0           Fourth vertex at the start of time-step
 * @param[in]  v0_t1           First vertex at the end of time-step
 * @param[in]  v1_t1           Second vertex at the end of time-step
 * @param[in]  v2_t1           Third vertex at the end of time-step
 * @param[in]  v3_t1           Fourth vertex at the end of time-step
 * @param[in]  constraintType  Type of collision constraint to compute.
 * @param[in]  is_edge_edge    Are the vertices of two edge:
 *                             (v0, v1) and (v2, v3)?
 * @param[in]  toi             Normalized time of impact between the two
 *                             primitives.
 * @param[out] c               Computed constraint value.
 */
void compute_collision_constraint(
    const Eigen::Vector3d& v0_t0, const Eigen::Vector3d& v1_t0,
    const Eigen::Vector3d& v2_t0, const Eigen::Vector3d& v3_t0,
    const Eigen::Vector3d& v0_t1, const Eigen::Vector3d& v1_t1,
    const Eigen::Vector3d& v2_t1, const Eigen::Vector3d& v3_t1,
    const CollisionConstraintType constraintType, bool is_edge_edge,
    double toi, double& c);

/**
 * @brief Compute the collision constraint gradient.
 *
 * Triangle vertices expected in clockwise order.
 *
 * @param[in]  v0_t0           First vertex at the start of time-step
 * @param[in]  v1_t0           Second vertex at the start of time-step
 * @param[in]  v2_t0           Third vertex at the start of time-step
 * @param[in]  v3_t0           Fourth vertex at the start of time-step
 * @param[in]  v0_t1           First vertex at the end of time-step
 * @param[in]  v1_t1           Second vertex at the end of time-step
 * @param[in]  v2_t1           Third vertex at the end of time-step
 * @param[in]  v3_t1           Fourth vertex at the end of time-step
 * @param[in]  constraintType  Type of collision constraint to compute.
 * @param[in]  is_edge_edge    Are the vertices of two edge:
 *                             (v0, v1) and (v2, v3)?
 * @param[in]  toi             Normalized time of impact between the two
 *                             primitives.
 * @param[out] grad_c          Computed gradient of constraint value.
 */
void compute_collision_constraint_gradient(
    const Eigen::Vector3d& v0_t0, // First vertex at the start of time-step
    const Eigen::Vector3d& v1_t0, // Second vertex at the start of time-step
    const Eigen::Vector3d& v2_t0, // Third vertex at the start of time-step
    const Eigen::Vector3d& v3_t0, // Fourth vertex at the start of time-step
    const Eigen::Vector3d& v0_t1, // First vertex at the end of time-step
    const Eigen::Vector3d& v1_t1, // Second vertex at the end of time-step
    const Eigen::Vector3d& v2_t1, // Third vertex at the end of time-step
    const Eigen::Vector3d& v3_t1, // Fourth vertex at the end of time-step
    const CollisionConstraintType constraintType, bool is_edge_edge,
    const double toi, Eigen::Vector12d& grad_c);

///////////////////////////////////////////////////////////////////////////////
// Volume Constraint

/**
 * @brief Compute the collision constraint volume.
 *
 * Triangle vertices expected in clockwise order.
 */
void compute_collision_volume_constraint(
    const Eigen::Vector3d& v0,
    const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, const Eigen::Vector3d& v3,
    double& c);

/**
 * @brief Compute the collision constraint volume gradient.
 *
 * Triangle vertices expected in clockwise order.
 */
void compute_collision_volume_constraint_gradient(
    const Eigen::Vector3d& v0,
    const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, const Eigen::Vector3d& v3,
    Eigen::Vector12d& grad_c);

///////////////////////////////////////////////////////////////////////////////
// Graphics (Distance) Constraint

/**
* @brief Compute the barycentric coordinates of a point in triangle (a, b, c).
*
* Computes the barycentric coordinates of any point in the plane containing
* the triangle (a, b, c).
*
* @param[in]  p       Compute the barycentric coordinates of this point.
* @param[in]  a       First vertex in the triangle.
* @param[in]  b       Second vertex in the triangle.
* @param[in]  b       Third vertex in the triangle.
* @param[out] coords  Computed barycentric coordinates.
*/
void barycentric_coordinates(
    const Eigen::Vector3d& p,
    const Eigen::Vector3d& a,
    const Eigen::Vector3d& b,
    const Eigen::Vector3d& c,
    Eigen::Vector3d& coords);

/**
 * @brief Compute the standard graphics point-triangle constraint value.
 *
 * Triangle vertices expected in counter-clockwise order.
 */
void compute_graphics_point_triangle_constraint(
    const Eigen::Vector3d& v0, // point
    const Eigen::Vector3d& v1, // triangle point 0
    const Eigen::Vector3d& v2, // triangle point 2
    const Eigen::Vector3d& v3, // triangle point 1
    double& c);

/**
 * @brief Compute the standard graphics edge-edge collision constraint value.
 */
void compute_graphics_edge_edge_constraint(
    const Eigen::Vector3d& v0, // first edge's first vertex
    const Eigen::Vector3d& v1, // first edge's second vertex
    const Eigen::Vector3d& v2, // second edge's first vertex
    const Eigen::Vector3d& v3, // second edge's second vertex
    double& c);

/**
 * @brief Compute the standard graphics point-triangle constraint gradient.
 *
 * Triangle vertices expected in counter-clockwise order.
 */
void compute_graphics_point_triangle_constraint_gradient(
    const Eigen::Vector3d& v0, // point
    const Eigen::Vector3d& v1, // triangle point 0
    const Eigen::Vector3d& v2, // triangle point 2
    const Eigen::Vector3d& v3, // triangle point 1
    Eigen::Vector12d& grad_c);

/**
 * @brief Compute the standard graphics edge-edge collision constraint gradient.
 */
void compute_graphics_edge_edge_constraint_gradient(
    const Eigen::Vector3d& v0, // first edge's first vertex
    const Eigen::Vector3d& v1, // first edge's second vertex
    const Eigen::Vector3d& v2, // second edge's first vertex
    const Eigen::Vector3d& v3, // second edge's second vertex
    Eigen::Vector12d& grad_c);

///////////////////////////////////////////////////////////////////////////////
// Efficient and Accurate Collision Response for Elastically Deformable Models
// [Verschoor et al. 2019]

void compute_Verschoor_point_triangle_constraint(
    const Eigen::Vector3d& v0_t0, // point at start of the timestep
    const Eigen::Vector3d& v1_t0, // triangle point 0 at start of the timestep
    const Eigen::Vector3d& v2_t0, // triangle point 1 at start of the timestep
    const Eigen::Vector3d& v3_t0, // triangle point 2 at start of the timestep
    const Eigen::Vector3d& v0_t1, // point at end of the timestep
    const Eigen::Vector3d& v1_t1, // triangle point 0 at end of the timestep
    const Eigen::Vector3d& v2_t1, // triangle point 1 at end of the timestep
    const Eigen::Vector3d& v3_t1, // triangle point 2 at end of the timestep
    double toi, double& c);

void compute_Verschoor_edge_edge_constraint(
    const Eigen::Vector3d& v0_t0, // first edge's first vertex at t = 0
    const Eigen::Vector3d& v1_t0, // first edge's second vertex at t = 0
    const Eigen::Vector3d& v2_t0, // second edge's first vertex at t = 0
    const Eigen::Vector3d& v3_t0, // second edge's second vertex at t = 0
    const Eigen::Vector3d& v0_t1, // first edge's first vertex at t = 1
    const Eigen::Vector3d& v1_t1, // first edge's second vertex at t = 1
    const Eigen::Vector3d& v2_t1, // second edge's first vertex at t = 1
    const Eigen::Vector3d& v3_t1, // second edge's second vertex at t = 1
    double toi, double& c);

void compute_Verschoor_point_triangle_constraint_gradient(
    const Eigen::Vector3d& v0_t0, // point at start of the timestep
    const Eigen::Vector3d& v1_t0, // triangle point 0 at start of the timestep
    const Eigen::Vector3d& v2_t0, // triangle point 1 at start of the timestep
    const Eigen::Vector3d& v3_t0, // triangle point 2 at start of the timestep
    const Eigen::Vector3d& v0_t1, // point at end of the timestep
    const Eigen::Vector3d& v1_t1, // triangle point 0 at end of the timestep
    const Eigen::Vector3d& v2_t1, // triangle point 1 at end of the timestep
    const Eigen::Vector3d& v3_t1, // triangle point 2 at end of the timestep
    double toi, Eigen::Vector12d& grad_c);

void compute_Verschoor_edge_edge_constraint_gradient(
    const Eigen::Vector3d& v0_t0, // first edge's first vertex at t = 0
    const Eigen::Vector3d& v1_t0, // first edge's second vertex at t = 0
    const Eigen::Vector3d& v2_t0, // second edge's first vertex at t = 0
    const Eigen::Vector3d& v3_t0, // second edge's second vertex at t = 0
    const Eigen::Vector3d& v0_t1, // first edge's first vertex at t = 1
    const Eigen::Vector3d& v1_t1, // first edge's second vertex at t = 1
    const Eigen::Vector3d& v2_t1, // second edge's first vertex at t = 1
    const Eigen::Vector3d& v3_t1, // second edge's second vertex at t = 1
    double toi, Eigen::Vector12d& grad_c);

///////////////////////////////////////////////////////////////////////////////
// Parallel contact-aware simulations of deformable particles in 3D Stokes flow
// [Lu et al. 2018]

void compute_STIV_point_triangle_constraint(
    const Eigen::Vector3d& v0_t0, // point at start of the timestep
    const Eigen::Vector3d& v1_t0, // triangle point 0 at start of the timestep
    const Eigen::Vector3d& v2_t0, // triangle point 1 at start of the timestep
    const Eigen::Vector3d& v3_t0, // triangle point 2 at start of the timestep
    const Eigen::Vector3d& v0_t1, // point at end of the timestep
    const Eigen::Vector3d& v1_t1, // triangle point 0 at end of the timestep
    const Eigen::Vector3d& v2_t1, // triangle point 1 at end of the timestep
    const Eigen::Vector3d& v3_t1, // triangle point 2 at end of the timestep
    double toi, double& c);

void compute_STIV_edge_edge_constraint(
    const Eigen::Vector3d& v0_t0, // first edge's first vertex at t = 0
    const Eigen::Vector3d& v1_t0, // first edge's second vertex at t = 0
    const Eigen::Vector3d& v2_t0, // second edge's first vertex at t = 0
    const Eigen::Vector3d& v3_t0, // second edge's second vertex at t = 0
    const Eigen::Vector3d& v0_t1, // first edge's first vertex at t = 1
    const Eigen::Vector3d& v1_t1, // first edge's second vertex at t = 1
    const Eigen::Vector3d& v2_t1, // second edge's first vertex at t = 1
    const Eigen::Vector3d& v3_t1, // second edge's second vertex at t = 1
    double toi, double& c);

void compute_STIV_point_triangle_constraint_gradient(
    const Eigen::Vector3d& v0_t0, // point at start of the timestep
    const Eigen::Vector3d& v1_t0, // triangle point 0 at start of the timestep
    const Eigen::Vector3d& v2_t0, // triangle point 1 at start of the timestep
    const Eigen::Vector3d& v3_t0, // triangle point 2 at start of the timestep
    const Eigen::Vector3d& v0_t1, // point at end of the timestep
    const Eigen::Vector3d& v1_t1, // triangle point 0 at end of the timestep
    const Eigen::Vector3d& v2_t1, // triangle point 1 at end of the timestep
    const Eigen::Vector3d& v3_t1, // triangle point 2 at end of the timestep
    double toi, Eigen::Vector12d& grad_c);

void compute_STIV_edge_edge_constraint_gradient(
    const Eigen::Vector3d& v0_t0, // first edge's first vertex at t = 0
    const Eigen::Vector3d& v1_t0, // first edge's second vertex at t = 0
    const Eigen::Vector3d& v2_t0, // second edge's first vertex at t = 0
    const Eigen::Vector3d& v3_t0, // second edge's second vertex at t = 0
    const Eigen::Vector3d& v0_t1, // first edge's first vertex at t = 1
    const Eigen::Vector3d& v1_t1, // first edge's second vertex at t = 1
    const Eigen::Vector3d& v2_t1, // second edge's first vertex at t = 1
    const Eigen::Vector3d& v3_t1, // second edge's second vertex at t = 1
    double toi, Eigen::Vector12d& grad_c);

} // namespace IPC


#include <boost/numeric/ublas/vector.hpp>
#include <kv/interval.hpp>

namespace ub = boost::numeric::ublas;

struct Matsu1 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(2);

		y(0) = (x(0)-1.) * (x(0)-1.) + x(1) * x(1) - 1.;
		y(1) = x(0) - x(1);

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(2);
		for (i=0; i<2; i++) {
			x(i) = kv::interval<T>(-8., 8.);
		}
	}
};

struct Matsu2 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(2);

		y(0) = x(0) * x(0) + x(1) * x(1) - 1.;
		y(1) = x(0) - x(1) + 1.;

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(2);
		for (i=0; i<2; i++) {
			x(i) = kv::interval<T>(-8., 8.);
		}
	}
};

struct NoSol {

	double param;

	NoSol(double param = 1e-5) : param(param) {
	}

	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(2);

		y(0) = x(0) * x(0) - x(1);
		y(1) = x(0) * x(0) - x(1) + param;

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(2);
		for (i=0; i<2; i++) {
			x(i) = kv::interval<T>(-10., 10.);
		}
	}
};

struct BadCond {

	double param;

	BadCond (double param = 1e-7) : param(param) {
	}

	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(2);

		y(0) = x(0) * x(0) - x(1);
		y(1) = (1. - param) * x(0) * x(0) - x(1) + param;

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(2);
		for (i=0; i<2; i++) {
			x(i) = kv::interval<T>(-10., 10.);
		}
	}
};

struct Hansen1 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(1);

		y(0) = x(0)*x(0)*x(0)*x(0) - 12.*x(0)*x(0)*x(0) + 47.*x(0)*x(0) - 60.*x(0);

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(1);
		for (i=0; i<1; i++) {
			x(i) = kv::interval<T>(-1e20, 1e20);
		}
	}
};

struct Burden {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(2);

		y(0) = x(0) * (4. - 0.0003 * x(0) - 0.0004 * x(1));
		y(1) = x(1) * (2. - 0.0002 * x(0) - 0.0001 * x(1));

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(2);
		for (i=0; i<2; i++) {
			x(i) = kv::interval<T>(0., 1e10);
		}
	}
};

struct GE1 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(2);

		y(0) = -1. / (1. + x(0)) + x(1) / (1. + x(1));
		y(1) = 1. / (1. + x(0)) - x(1) / (x(0) + x(1));

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(2);
		for (i=0; i<2; i++) {
			x(i) = kv::interval<T>(0.01, 100.);
		}
	}
};

// Yoshitane Shinohara: Suuchikaiseki no Kiso, q. 3.8.2
struct Shinohara1 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(2);
		T p, q;

		p = x(0);
		q = x(1);

		y(0) = p*p*p*p*p - 10.*p*p*p*q*q + 5.*p*q*q*q*q
		       - 2.*p*p*p*p + 12.*p*p*q*q - 2.*q*q*q*q
		       + 10.*p*p*p - 30.*p*q*q - 9.*p + 3.;
		y(1) = q*q*q*q*q - 10.*p*p*q*q*q + 5.*p*p*p*p*q
		       - 8.*p*p*p*q + 8.*p*q*q*q + 30.*p*p*q
		       -10.*q*q*q - 9.*q;

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(2);
		for (i=0; i<2; i++) {
			x(i) = kv::interval<T>(-5., 5.);
		}
	}
};

// Yoshitane Shinohara: Suuchikaiseki no Kiso, q. 3.8.3
struct Shinohara2 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(2);
		T p, q;

		p = x(0);
		q = x(1);

		y(0) = p*p*p*p*p - 10.*p*p*p*q*q + 5.*p*q*q*q*q
		       - 3.*p*p*p*p + 18.*p*p*q*q - 3.*q*q*q*q
		       - 2.*p*p*p + 6.*p*q*q + 3.*p*p*q - q*q*q
		       + 12.*p*p - 12.*q*q - 10.*p*q - 8.*p + 8.*q;
		y(1) = 5.*p*p*p*p*q - 10.*p*p*q*q*q + q*q*q*q*q
		       - 12.*p*p*p*q + 12.*p*q*q*q - p*p*p + 3.*p*q*q
		       - 6.*p*p*q + 2.*q*q*q + 5.*p*p - 5.*q*q
		       + 24.*p*q - 8.*p - 8.*q + 4.;

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(2);
		for (i=0; i<2; i++) {
			x(i) = kv::interval<T>(-3., 3.);
		}
	}
};

// Yoshitane Shinohara: Suuchikaiseki no Kiso, ex. 3.8
struct Shinohara3 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(5);
		T p, q, r, s, t;

		p = x(0);
		q = x(1);
		r = x(2);
		s = x(3);
		t = x(4);

		y(0) = p*p*p - 2.*p*q + r + 0.75*p + 1.;
		y(1) = p*p*q - q*q - p*r +s + 0.75*q + 0.25;
		y(2) = p*p*r - p*s - q*r + t + 0.75*r + 0.75;
		y(3) = p*p*s - p*t - q*s + 0.75*s;
		y(4) = p*p*t - q*t + 0.75*t - 0.25;

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(5);
		x(0) = kv::interval<T>(-1.5, 2.);
		x(1) = kv::interval<T>(-0.6, 3.);
		x(2) = kv::interval<T>(-1.5, 2.5);
		x(3) = kv::interval<T>(-0.5, 1.9);
		x(4) = kv::interval<T>(-1, 1.);
	}
};

/*
  Problem taken from
  http://nlab.ee.tokushima-u.ac.jp/nishio/Pub-Data/WORK/W153.pdf,
  http://200.13.98.241/~martin/syop/tareas3/kuno_seader_homotopy.pdf
  Computing All Real Solutions to Systems of Nonlinear Equations
  with a Global Fixed-Point Homotopy
  This equation has 7 solutions and difficult to find all solution
  by homotopy method.
 */

struct ModifiedHimmelblau {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(2);

		y(0) = 2.*x(0)*x(0)*x(0) + 2.*x(0)*x(1) - 22.*x(0) + x(1)*x(1) + 13.;
		y(1) = x(0)*x(0) + 2.*x(0)*x(1) + 2.*x(1)*x(1)*x(1) - 14.*x(1) + 9.;

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(2);
		for (i=0; i<2; i++) {
			x(i) = kv::interval<T>(-1e8, 1e8);
		}
	}
};

//
// made by Heihachiro Yoshii on 2013/07/25
//

struct Heihachiro {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(2);

		y(0) = 2. * x(0) * x(0) * x(1) - 1.;
		y(1) = x(0) + 0.5 * x(1) * x(1) - 2.;

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(2);
		for (i=0; i<2; i++) {
			x(i) = kv::interval<T>(-1000., 1000.);
		}
	}
};

struct Yamamura2 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int n = x.size();
		ub::vector<T> y(n);
		T s;
		int i;

		s = 0.;
		for (i=0; i<n; i++) {
			s += pow(x(i), 3);
		}

		for (i=0; i<n; i++) {
			y(i) = x(i) - (s + (i + 1.)) / (2. * n);
		}

		return y;
	}

	template<class T>
	void range(int n, ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(n);
		for (i=0; i<n; i++) {
			x(i) = kv::interval<T>(-2.5, 2.5);
		}
	}
};

//
// K. Meintjes and A. P. Morgan:
// Chemical Equilibrium Systems as Numerical Test Problmes,
// ACM Transactions on Mathematical Software, 16(2):143, 1990.
//

struct HydroCarbon {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(5);
		// phisical constants
		static T K5 = kv::constants<T>::str("1.930e-1");
		static T K6 = kv::constants<T>::str("2.597e-3");
		static T K7 = kv::constants<T>::str("3.448e-3");
		static T K8 = kv::constants<T>::str("1.799e-5");
		static T K9 = kv::constants<T>::str("2.155e-4");
		static T K10 = kv::constants<T>::str("3.846e-5");
		// parameters
		static T R = T(10);
		static T p = T(40);
		// constants
		static T R5 = K5;
		static T R6 = K6 * pow(p, -0.5);
		static T R7 = K7 * pow(p, -0.5);
		static T R8 = K8 * pow(p, -1);
		static T R9 = K9 * pow(p, -0.5);
		static T R10 = K10 * pow(p, -1);

		y(0) = x(0) * x(1) + x(0) - 3 * x(4);
		y(1) = 2 * x(0) * x(1) + x(0) + 2 * R10 * pow(x(1), 2) + x(1) * pow(x(2), 2) + R7 * x(1) * x(2) + R9 * x(1) * x(3) + R8 * x(1) - R * x(4);
		y(2) = 2 * x(1) * pow(x(2), 2) + R7 * x(1) * x(2) + 2 * R5 * pow(x(2), 2) + R6 * x(2) - 8 * x(4);
		y(3) = R9 * x(1) * x(3) + 2 * pow(x(3), 2) - 4 * R * x(4);
		y(4) = x(0) * x(1) + x(0) + R10 * pow(x(1), 2) + x(1) * pow(x(2), 2) + R7 * x(1) * x(2) + R9 * x(1) * x(3) + R8 * x(1) + R5 * pow(x(2), 2) + R6 * x(2) + pow(x(3), 2) - 1;

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		x.resize(5);
		int i;
		for (i=0; i<5; i++) {
			x(i) = kv::interval<T>(-100, 100);
		}
	}
};


#pragma once

#include <Eigen/Dense>
#include <vector>
#include "EM.hpp"

template<class Mixture>
class Generator {
public:
    typedef typename Mixture::float_type float_type;
    typedef typename Mixture::sample_type sample_type;
    typedef typename Mixture::variable_type variable_type;
private:
    std::vector<float_type> prior;
    std::vector<Generator<variable_type> > variableGenerator;
public:
    Generator(const Mixture& mixture) :
        prior{mixture.prior}
    {
        for(size_t k=0; k<mixture.size(); k++){
            variableGenerator.emplace_back( Generator<variable_type>(mixture[k]) );
        }
    }
    sample_type operator()(){
        float_type p = (float_type) rand()/RAND_MAX;
        float_type sum = 0.0;
        for(size_t k=0; k<prior.size(); k++){
            sum += prior[k];
            if( sum>=p ){
                return variableGenerator[k]();
            }
        }
        return variableGenerator.back()();
    }
};


#include "datatypes.hpp"
#include <Eigen/Core>
#include <Eigen/Eigen>
#include "parameters.hpp"

#include <nlopt.h>

namespace gd {

using namespace Eigen;
using namespace std;

template<class F>
double nlopt_wrapper(unsigned n, const double *x, double *grad, void *data);
template<typename F=std::function<double(unsigned n, const double *x, double *grad)>, typename T=double>
class MinimizerNLoptTest {
	typedef MinimizerNLoptTest<F, T> type;
public:
	F f;
	int N;
	MinimizerNLoptTest(F f, int N) : f(f), N(N) {
	}
	//template<int N, class... Ts>
	void optimize(int n_eval, double* x) {
	//const int N = sizeof...(Ts);
		nlopt_opt opt;
		opt = nlopt_create(NLOPT_LD_MMA, N);
		opt = nlopt_create(NLOPT_LD_LBFGS, N);
		//opt = nlopt_create(NLOPT_AUGLAG, N);
	//opt = nlopt_create(NLOPT_LD_VAR1, N);
	//opt = nlopt_create(NLOPT_LN_COBYLA, N);

	//nlopt_set_lower_bounds(opt, lb);
		nlopt_set_min_objective(opt, nlopt_wrapper<type>, this);
		//double lower_bounds[N] = { 0 };
		double* lower_bounds = new double[N];
		double* upper_bounds = new double[N];
//lower_bounds[0] = 0.1;
		//lower_bounds[1] = 1;
		for(int i = 0; i < N; i++) {
			lower_bounds[i] = -HUGE_VAL;
			upper_bounds[i] =  HUGE_VAL;
		}

		//double upper_bounds[N];
		//upper_bounds[0] = 10;
		//upper_bounds[1] = 40;
		//for(int i = 2; i < N; i++) {
		//	lower_bounds[i] = -1;
		//	upper_bounds[i] = 1;
		//}
		//nlopt_set_lower_bounds(opt, lower_bounds);
		//nlopt_set_upper_bounds(opt, upper_bounds);
		//for(int i = 0; i < N; i++)
		//	printf("lower_bounds[%d] = %f\n", i, lower_bounds[i]);
		//for(int i = 0; i < N; i++)
		//	printf("upper [%d] = %f\n", i, upper_bounds[i]);
	//double steps[N] = {1e-8, 1e-10};
	//nlopt_set_initial_step1(opt, 1e-5);

		nlopt_set_xtol_rel(opt, 1e-8);
		//nlopt_set_ftol_abs(opt, 1e-9);
		nlopt_set_maxeval(opt, n_eval);
		//double x[N] = { initial_values... };
		double minf;
		int ret = nlopt_optimize(opt, x, &minf);
		if (ret < 0) {
			printf("nlopt failed!: %d\n", ret);
		}
		else {
			printf("found minimum at f(...");
			//for(int i = 0; i < N; i++)
			//	printf("%g, ", x[i]);
			printf(") = %0.10g\n", minf);
		}
		nlopt_destroy(opt);
	}
};

template<class F>
MinimizerNLoptTest<F> nnlopt_optimize(F f, int N)
{
	MinimizerNLoptTest<F> opt(f, N);
	return opt;
}

namespace schw {

};

class OptimizationMatrixChiSquare {
public:
	OptimizationMatrixChiSquare(double_matrix model_matrix, double_vector observed) {
		this->model_matrix = MatrixXd::Map(model_matrix.data().begin(), model_matrix.size2(), model_matrix.size1());
		this->observed = VectorXd::Map(observed.data().begin(), observed.size());
	}
	double _logp(double_vector _x) {
		Map<VectorXd> x = VectorXd::Map(_x.data().begin(), _x.size());
		return logp(x);
	}
	template<class T>
	double logp(T& x) {
		//double total = x.sum();
		//VectorXd xn = x/total;
		//double norm = totalmass.dot(x);
		//int N = pmatrix.rows();
		//printf("[N=%d f=%f %f]\n", N, norm, log(norm));
		//double value = ((pmatrix * x).cwise().log()).sum() - N*log(norm);// - pmatrix.rows() * log(x.sum());
		double chisq = (model_matrix * x - observed).cwise().square().sum();
		return -0.5 * chisq;
		//return value;
	}
	void _dlogpdx(double_vector _x, double_vector _gradient) {
		VectorXd x = VectorXd::Map(_x.data().begin(), _x.size());
		Map<VectorXd> gradient = VectorXd::Map(_gradient.data().begin(), _gradient.size());
		dlogpdx(x, gradient);
	}

	template<class T1, class T2>
	void dlogpdx(T1& x, T2& gradient) {
		//double total = x.sum();
		//VectorXd xn = x/total;
		VectorXd model_values = model_matrix * x;
		//int N = model_matrix.rows();
		//VectorXd Pi_inv = Pi.cwise().inverse();
		//VectorXd t1 = -pmatrix.transpose() * Pi_inv;
		//VectorXd g = t1.cwise() + x.size()/(x.sum());
		//gradient.setZero();
		int Nx = x.size();
		//printf("matrix: %d %d observed: %d\n", model_matrix.cols(), model_matrix.rows(), observed.size());
		//double norm = totalmass.dot(x);
		//int N = pmatrix.rows();
		int Nj = model_matrix.rows();
		for(int k = 0; k < Nx; k++) {
			for(int j = 0; j < Nj; j++) {
				gradient(k) += -(model_values(j) - observed(j))*model_matrix(j,k);
			}
			//gradient(k) += -N/norm*totalmass(k);
		}
		//gradient += g;//.cwise();//*x;

	}
	MatrixXd model_matrix;
	VectorXd observed;
};

class OptimizationMatrix {
public:
	OptimizationMatrix(double_matrix pmatrix, double_vector totalmass) {
		this->pmatrix = MatrixXd::Map(pmatrix.data().begin(), pmatrix.size2(), pmatrix.size1());
		this->totalmass = VectorXd::Map(totalmass.data().begin(), totalmass.size());
	}
	double _logp(double_vector _x) {
		Map<VectorXd> x = VectorXd::Map(_x.data().begin(), _x.size());
		return logp(x);
	}
	template<class T>
	double logp(T& x) {
		//double total = x.sum();
		//VectorXd xn = x/total;
		double norm = totalmass.dot(x);
		int N = pmatrix.rows();
		//printf("[N=%d f=%f %f]\n", N, norm, log(norm));
		double value = ((pmatrix * x).cwise().log()).sum() - N*log(norm);// - pmatrix.rows() * log(x.sum());
		return value;
	}
	void _dlogpdx(double_vector _x, double_vector _gradient) {
		VectorXd x = VectorXd::Map(_x.data().begin(), _x.size());
		Map<VectorXd> gradient = VectorXd::Map(_gradient.data().begin(), _gradient.size());
		dlogpdx(x, gradient);
	}

	template<class T1, class T2>
	void dlogpdx(T1& x, T2& gradient) {
		//double total = x.sum();
		//VectorXd xn = x/total;
		VectorXd Pi = (pmatrix * x);
		int N = pmatrix.rows();
		//VectorXd Pi_inv = Pi.cwise().inverse();
		//VectorXd t1 = -pmatrix.transpose() * Pi_inv;
		//VectorXd g = t1.cwise() + x.size()/(x.sum());
		//gradient.setZero();
		int Nx = x.size();
		//printf("matrix: %d %d Pi: %d\n", pmatrix.cols(), pmatrix.rows(), Pi.size());
		double norm = totalmass.dot(x);
		//int N = pmatrix.rows();
		int Nj = pmatrix.rows();
		for(int k = 0; k < Nx; k++) {
			for(int j = 0; j < Nj; j++) {
				gradient(k) += pmatrix(j,k)/Pi(j);
			}
			gradient(k) += -N/norm*totalmass(k);
		}
		//gradient += g;//.cwise();//*x;

	}
	MatrixXd pmatrix;
	VectorXd totalmass;
};

class OptimizationMatrixForegroundConditional {
public:
	OptimizationMatrixForegroundConditional(double_matrix pmatrix, double_matrix pcmatrix, double_vector ratios, double_vector p_v_non_member) {
		this->pmatrix = MatrixXd::Map(pmatrix.data().begin(), pmatrix.size2(), pmatrix.size1());
		this->pcmatrix = MatrixXd::Map(pcmatrix.data().begin(), pcmatrix.size2(), pcmatrix.size1());
		//this->totalmass = VectorXd::Map(totalmass.data().begin(), totalmass.size());
		this->ratios = VectorXd::Map(ratios.data().begin(), ratios.size());
		this->p_v_non_member = VectorXd::Map(p_v_non_member.data().begin(), p_v_non_member.size());
		p_member = this->ratios.cwise() / (1 + this->ratios.cwise());
	}

	double _logp(double_vector _x) {
		Map<VectorXd> x = VectorXd::Map(_x.data().begin(), _x.size());
		return logp(x);
	}
	template<class T>
	double logp(T& x) {
		//double total = x.sum();
		//VectorXd xn = x/total;
		//double norm = totalmass.dot(x);
		//int N = pmatrix.rows();
		//printf("[N=%d f=%f %f]\n", N, norm, log(norm));
		// sum( p(v|R) = sum( p(v,R)/p(R) )
		VectorXd p_v_member = (pmatrix * x).cwise() / (pcmatrix * x);
		double value = (p_v_member.cwise()*p_member +p_v_non_member.cwise() * ((p_member*-1).cwise() + 1)).cwise().log().sum(); // - N*log(norm);// - pmatrix.rows() * log(x.sum());
		return value;
	}
	void _dlogpdx(double_vector _x, double_vector _gradient) {
		VectorXd x = VectorXd::Map(_x.data().begin(), _x.size());
		Map<VectorXd> gradient = VectorXd::Map(_gradient.data().begin(), _gradient.size());
		dlogpdx(x, gradient);
	}

	template<class T1, class T2>
	void dlogpdx(T1& x, T2& gradient) {
		//double total = x.sum();
		//VectorXd xn = x/total;
		VectorXd Pi = (pmatrix * x);
		VectorXd PCi = (pcmatrix * x);
		//VectorXd p = Pi.cwise() / PCi;

		VectorXd p_v_member = (pmatrix * x).cwise() / (pcmatrix * x);
		VectorXd p = (p_v_member.cwise()*p_member +p_v_non_member.cwise() * ((p_member*-1).cwise() + 1));

		//int N = pmatrix.rows();
		//VectorXd Pi_inv = Pi.cwise().inverse();
		//VectorXd t1 = -pmatrix.transpose() * Pi_inv;
		//VectorXd g = t1.cwise() + x.size()/(x.sum());
		//gradient.setZero();
		int Nx = x.size();
		//printf("matrix: %d %d Pi: %d\n", pmatrix.cols(), pmatrix.rows(), Pi.size());
		//double norm = totalmass.dot(x);
		//int N = pmatrix.rows();
		int Nj = pmatrix.rows();
		//printf(" %d %d %d %d %d %d \n", Nj, Nx, Pi.size(), PCi.size(), p.size(), pcmatrix.cols());
		for(int k = 0; k < Nx; k++) {
			for(int j = 0; j < Nj; j++) {
				//gradient(k) += pmatrix(j,k)/Pi(j);
				//gradient(k) += 1./p(j) * (pmatrix(j,k) / PCi(j)  - Pi(j)/pow(PCi(j), 2) * pcmatrix(j,k) );
				gradient(k) += 1./p(j) * (pmatrix(j,k) * p_member(j) / PCi(j)  - Pi(j) * p_member(j) /pow(PCi(j), 2) * pcmatrix(j,k) );
			}
			//gradient(k) += -N/norm*totalmass(k);
		}
		//exit(0);
		//gradient += g;//.cwise();//*x;

	}
	MatrixXd pmatrix, pcmatrix;
	VectorXd totalmass;
	VectorXd ratios;
	VectorXd p_member;
	VectorXd p_v_non_member;

};

class OptimizationQP {
public:
	MatrixXd P;
	VectorXd q;
	OptimizationQP(double_matrix P, double_vector q) {
		this->P = MatrixXd::Map(P.data().begin(), P.size2(), P.size1());
		this->q = VectorXd::Map(q.data().begin(), q.size());
	}
	double _logp(double_vector _x) {
		Map<VectorXd> x = VectorXd::Map(_x.data().begin(), _x.size());
		return logp(x);
	}
	template<class T>
	double logp(T& x) {
		//double total = x.sum();
		VectorXd Px = P * x;
		double value = x.dot(Px)/2 + q.dot(x);
		//double norm = totalmass.dot(x);
		//double value = ((pmatrix * x).cwise().log().cwise() * counts).sum() - this->N*log(norm);// - pmatrix.rows() * log(x.sum());
		return value;
	}
	void _dlogpdx(double_vector _x, double_vector _gradient) {
		VectorXd x = VectorXd::Map(_x.data().begin(), _x.size());
		Map<VectorXd> gradient = VectorXd::Map(_gradient.data().begin(), _gradient.size());
		dlogpdx(x, gradient);
	}

	template<class T1, class T2>
	void dlogpdx(T1& x, T2& gradient) {
		gradient = gradient + P * x + q;
	}
};

class OptimizationMatrixN {
public:
	OptimizationMatrixN(double_matrix pmatrix, double_vector counts, double_vector totalmass) {
		this->pmatrix = MatrixXd::Map(pmatrix.data().begin(), pmatrix.size2(), pmatrix.size1());
		this->counts = VectorXd::Map(counts.data().begin(), counts.size());
		this->totalmass = VectorXd::Map(totalmass.data().begin(), totalmass.size());
		this->N = this->counts.sum();
	}
	double _logp(double_vector _x) {
		Map<VectorXd> x = VectorXd::Map(_x.data().begin(), _x.size());
		return logp(x);
	}
	template<class T>
	double logp(T& x) {
		//double total = x.sum();
		//VectorXd xn = x/total;
		double norm = totalmass.dot(x);
		double value = ((pmatrix * x).cwise().log().cwise() * counts).sum() - this->N*log(norm);// - pmatrix.rows() * log(x.sum());
		return value;
	}
	void _dlogpdx(double_vector _x, double_vector _gradient) {
		VectorXd x = VectorXd::Map(_x.data().begin(), _x.size());
		Map<VectorXd> gradient = VectorXd::Map(_gradient.data().begin(), _gradient.size());
		dlogpdx(x, gradient);
	}

	template<class T1, class T2>
	void dlogpdx(T1& x, T2& gradient) {
		/*double total = x.sum();
		VectorXd xn = x/total;
		VectorXd pi = (pmatrix * xn);
		VectorXd t = pi.cwise().inverse();
		MatrixXd m = pmatrix;
		for(int i = 0; i < pmatrix.cols(); i++) {
			m.col(i) = m.col(i).cwise() * counts;
		}
		//cout << counts;
		//printf("matrix: %d %d vector: %d\n", pmatrix.cols(), pmatrix.rows(), counts.size());
		//MatrixXd m = pmatrix.transpose().cwise() * counts;
		//VectorXd g1 = -(pmatrix.transpose().colwise() * counts) * t;
		//g1 = g1.cwise() + pmatrix.rows() * 1/(x.sum());
		//g1 = (g1.cwise() * counts)  + (counts*(1/(x.sum())));
		//g1 = (g1.cwise() * counts)  + (counts*(1/(x.sum())));
		//g1 = (g1) ;//  + (counts*(1/(x.sum())));
		//gradient.setZero();
		VectorXd ex = counts*(1./x.sum());
		double bla = counts.sum()/(x.sum());
		gradient += (-(m.transpose() * t)).cwise() + bla;
		//gradient = g1;//.cwise();//*x;*/
		//double total = x.sum();
		//VectorXd xn = x/total;
		VectorXd Pi = (pmatrix * x);
		//VectorXd Pi_inv = Pi.cwise().inverse();
		//VectorXd t1 = -pmatrix.transpose() * Pi_inv;
		//VectorXd g = t1.cwise() + x.size()/(x.sum());
		//gradient.setZero();
		int Nx = x.size();
		double norm = totalmass.dot(x);
		//printf("matrixN: %d %d Pi: %d\n", pmatrix.cols(), pmatrix.rows(), Pi.size());
		int Nj = pmatrix.rows();
		for(int k = 0; k < Nx; k++) {
			for(int j = 0; j < Nj; j++) {
				gradient(k) += counts(j) * pmatrix(j,k)/Pi(j) - counts(j)/norm*totalmass(k);
			}
		}
		//gradient += g;//.cwise();//*x;

	}
	MatrixXd pmatrix;
	VectorXd counts;
	VectorXd totalmass;
	double N;
};

class OptimizationNormalize {
public:
	double value, error;
	OptimizationNormalize(double value, double error) : value(value), error(error) {
	}
	double _logp(double_vector _x) {
		Map<VectorXd> x = VectorXd::Map(_x.data().begin(), _x.size());
		return logp(x);
	}
	template<class T>
	double logp(T& x) {
		double value = -pow((this->value-x.sum())/error, 2);
		return value;
	}
	void _dlogpdx(double_vector _x, double_vector _gradient) {
		VectorXd x = VectorXd::Map(_x.data().begin(), _x.size());
		Map<VectorXd> gradient = VectorXd::Map(_gradient.data().begin(), _gradient.size());
		dlogpdx(x, gradient);
	}

	template<class T1, class T2>
	void dlogpdx(T1& x, T2& gradient) {
		gradient = gradient.cwise() + 2*(value-x.sum())/pow(error,2);
	}
};

class OptimizationNormalizeMass {
public:
	VectorXd mass_vector;
	double totalmass, error;
	OptimizationNormalizeMass(double_vector mass_vector, double totalmass, double error) : totalmass(totalmass), error(error) {
		this->mass_vector = VectorXd::Map(mass_vector.data().begin(), mass_vector.size());
	}
	double _logp(double_vector _x) {
		Map<VectorXd> x = VectorXd::Map(_x.data().begin(), _x.size());
		return logp(x);
	}
	template<class T>
	double logp(T& x) {
		double mass = mass_vector.dot(x);
		double value = -pow((this->totalmass-mass)/error, 2);
		return value;
	}
	void _dlogpdx(double_vector _x, double_vector _gradient) {
		VectorXd x = VectorXd::Map(_x.data().begin(), _x.size());
		Map<VectorXd> gradient = VectorXd::Map(_gradient.data().begin(), _gradient.size());
		dlogpdx(x, gradient);
	}

	template<class T1, class T2>
	void dlogpdx(T1& x, T2& gradient) {
		//gradient = gradient.cwise() + 2*(((mass_vector.cwise() - this->totalmass).cwise()) /pow(error,2)).cwise();
		int Nx = x.size();
		//double norm = totalmass.dot(x);
		//printf("matrixN: %d %d Pi: %d\n", pmatrix.cols(), pmatrix.rows(), Pi.size());
		//int Nj = pmatrix.rows();
		double mass = mass_vector.dot(x);
		double value = (this->totalmass-mass);
		double err = pow(error, 2);
		for(int k = 0; k < Nx; k++) {
			//for(int j = 0; j < Nj; j++) {
			gradient(k) += 2 * value/err * mass_vector(k);
			//}
		}
	}
};

class OptimizationEntropy {
public:
	double scale;
	OptimizationEntropy(double scale) : scale(scale)  {
	}
	double _logp(double_vector _x) {
		Map<VectorXd> x = VectorXd::Map(_x.data().begin(), _x.size());
		return logp(x);
	}
	template<class T>
	double logp(T& x) {
		double value = -scale * (x.cwise() * x.cwise().log()).sum();
		return value;
	}
	void _dlogpdx(double_vector _x, double_vector _gradient) {
		VectorXd x = VectorXd::Map(_x.data().begin(), _x.size());
		Map<VectorXd> gradient = VectorXd::Map(_gradient.data().begin(), _gradient.size());
		dlogpdx(x, gradient);
	}

	template<class T1, class T2>
	void dlogpdx(T1& x, T2& gradient) {
		//gradient = gradient.cwise() + 2*(value-x.sum())/pow(error,2);
		gradient += -scale * ((x.cwise().log().cwise()+1));
	}
};

/*
class OptimizationMatrixN {
public:
	OptimizationMatrixN(double_matrix pmatrix, double_vector counts) : pmatrix(pmatrix), counts(counts) {
		this->pmatrix = MatrixXd::Map(pmatrix.data().begin(), pmatrix.size2(), pmatrix.size1());
		this->counts = VectorXd::Map(counts.data().begin(), counts.size());
	}
	double _logp(double_vector _x) {
		Map<VectorXd> x = VectorXd::Map(_x.data().begin(), _x.size());
		return logp(x);
	}
	template<class T>
	double logp(T& x) {
		VectorXd x = u.cwise().exp();
		double total = x.sum();
		VectorXd xn = x/total;
		//VectorXd rho2d = rho2dmatrix * x;
		//cout << "cols " << pmatrix.cols() << endl;
		//double logLkinematics = (pmatrix * x).cwise().log().sum() - pmatrix.rows() * log(x.sum());
		double value = ((pmatrix * xn).cwise().log().cwise() * count.cwise()).sum();// - pmatrix.rows() * log(x.sum());
		return value;
	}
	MatrixXd pmatrix;
	VectorXd counts;
};*/

class OptimizationProblemSchw {
public:
	OptimizationProblemSchw(double_matrix _pmatrix, double_matrix _rho2dmatrix, double_vector _orbitweights, double_vector _rho2d_target, double_vector _rho2d_error, double error_x, double entropy_scale, bool kin, bool light, bool norm) : error_x(error_x), entropy_scale(entropy_scale), kin(kin), light(light), norm(norm) {
		pmatrix = MatrixXd::Map(_pmatrix.data().begin(), _pmatrix.size2(), _pmatrix.size1());
		rho2dmatrix = MatrixXd::Map(_rho2dmatrix.data().begin(), _rho2dmatrix.size2(), _rho2dmatrix.size1());
		orbitweights = VectorXd::Map(_orbitweights.data().begin(), _orbitweights.size());
		rho2d_target = VectorXd::Map(_rho2d_target.data().begin(), _rho2d_target.size());
		rho2d_error = VectorXd::Map(_rho2d_error.data().begin(), _rho2d_error.size());
	}

	void optimize(int n_eval, int steps, double_vector _u) {
		int iteration = 0;
		//int steps = 1;
		auto g = [&](int n, const double *x, double *grad) -> double {
			//toy.scale = x[0];
			//toy.M = exp(x[1]);
			Map<VectorXd> u(x, n);
			double minlogL = -this->likelihood(u);
			if(grad) {
				Map<VectorXd> gradvector(grad, n);
				this->dfdx(u, gradvector);
			}
			if((iteration % steps) == 0) {
				printf("current point(...");
				//for(int i = 0; i < n; i++)
				//	printf("%g, ", x[i]);
				printf(") = %20f\n", minlogL);
			}
			/*
			double chisq = fit.optimize_function(grad);
			if((iteration % steps) == 0) {
				if(grad) {
					printf(" gradient(");
					for(int i = 0; i < n; i++)
						printf("%g, ", grad[i]);
				}
				printf(")");
				printf(" chisq = %f iteration = %d\n", chisq, iteration);
			}
		//grad[0] = fit.denergyChisqdb(J1, J2);
		//return fit.energyChisq(J1, J2);*/
			iteration++;
			return minlogL;
		};
		auto o = nnlopt_optimize(g, orbitweights.rows());
		//int n = atoi(argv[1]);
		o.optimize(n_eval, _u.data().begin());
		cout << "interations: " << iteration << endl;
	}

	void _hessian(double_matrix _h, double_vector _u, bool du=true) {
		Map<MatrixXd> h = MatrixXd::Map(_h.data().begin(), _h.size2(), _h.size1());
		VectorXd u = VectorXd::Map(_u.data().begin(), _u.size());
		hessian(h, u, du);
	}

	template<class T1, class T2>
	double hessian(T1& h, T2& u, bool du=true) {
		VectorXd x = u.cwise().exp();

		assert(h.rows() == h.cols());
		int N = h.rows();
		VectorXd gradient = VectorXd::Zero(N);
		this->dfdx(u, gradient);
		h.setZero();
		//cout << "g " << gradient(0) << endl;

		cout << "rows = " << pmatrix.rows() << endl;
		cout << "cols = " << pmatrix.cols() << endl;
		if(kin) {
			VectorXd w = (pmatrix * x).cwise().inverse().cwise().square();
			cout << "els = " << w.rows() << endl;
			for(int k = 0; k < N; k++) {
				for(int l = 0; l < N; l++) {
					for(int i = 0; i < pmatrix.rows(); i++) {
						if(du)
							h(l,k) += w(i) * pmatrix(i,l) * pmatrix(i,k) * x(k) * x(l);
						else
							h(l,k) += w(i) * pmatrix(i,l) * pmatrix(i,k);
					}
				}
			}

		}
		for(int k = 0; k < N; k++) {
			for(int l = 0; l < N; l++) {
				if((k == l) and du)
					h(k,l) += gradient(k);
				if(light) {
					for(int i = 0; i < rho2dmatrix.rows(); i++) {
						if(du)
							h(k,l) += 2 * rho2dmatrix(i,l) * rho2dmatrix(i,k) / pow(rho2d_error(i), 2) * x(k) * x(l);
						else
							h(k,l) += 2 * rho2dmatrix(i,l) * rho2dmatrix(i,k) / pow(rho2d_error(i), 2);
					}
				}
				if(norm) {
					if(du)
						h(k, l) += 2/pow(error_x, 2) * x(k) * x(l);
					else
						h(k, l) += 2/pow(error_x, 2);
				}
				//cout << "h " << h(k,l) << endl;
			}
		}
		//MatrixXd::Map(_h.data().begin(), _h.size2(), _h.size1()) = h;
	}

	double  _likelihood(double_vector _u) {
		Map<VectorXd> u = VectorXd::Map(_u.data().begin(), _u.size());
		return likelihood(u);
	}
	// return f(x) to optimize
	template<class T>
	double likelihood(T& u) {
		//VectorXd u = VectorXd::Map(_u.data().begin(), _u.size());
		VectorXd x = u.cwise().exp();
		double total = x.sum();
		VectorXd xn = x/total;
		VectorXd rho2d = rho2dmatrix * x;
		//cout << "cols " << pmatrix.cols() << endl;
		//double logLkinematics = (pmatrix * x).cwise().log().sum() - pmatrix.rows() * log(x.sum());
		double logLkinematics = (pmatrix * xn).cwise().log().sum();// - pmatrix.rows() * log(x.sum());
		// + 52082.8 + 1500;
		double logLnorm = -pow((1.000001-x.sum())/error_x, 2);
		double logLdensity = -((rho2d_target-rho2d).cwise()/rho2d_error).cwise().pow(2).sum();
		//cout << "u = " << u << endl;
		//cout << "x = " << x << endl;
		//cout << ">" << logLkinematics << " " << logLnorm << " " << logLdensity << endl;
		//assert(0);
		//double entropy = -k * (x.cwise() * x.cwise().log()).sum();
		//return logLkinematics + logLnorm + logLdensity;
		double logL = 0;
		logL += -entropy_scale * (x.cwise() * x.cwise().log()).sum();
		if(kin)
			logL += logLkinematics;
		if(light)
			logL += logLdensity;
		if(norm)
			logL += logLnorm ;
		return logL;

		//return logLdensity;// - entropy; // logL \propto -entropy.. ?
		//return 1; //logLnorm;// - entropy; // logL \propto -entropy.. ?
		//return logLdensity;
		//return 0;
		//return logLkinematics;
		//-sum((rho2d_true-rho2d)**2/rho2d_error**2)*fudgefactor
	}

	// gradient
	void _dfdx(double_vector _u, double_vector _gradient) {
		VectorXd u = VectorXd::Map(_u.data().begin(), _u.size());
		Map<VectorXd> gradient = VectorXd::Map(_gradient.data().begin(), _gradient.size());
		dfdx(u, gradient);
	}

	template<class T1, class T2>
	void dfdx(T1& u, T2& gradient) {
		VectorXd x = u.cwise().exp();
		VectorXd rho2d = rho2dmatrix * x;
		/*g1 =  -sum(dpidxk / pi, axis=1)
		gu = -(2*(1.001-sum(u))/error_x**2)
		g2 = -sum(2*(rho2d_true-rho2d)/rho2d_error**2 *rho2ds, axis=1) * fudgefactor
		g = g1*u+gu*u# -g2 #-(-g1+g2)
		g += g2*u*/
		VectorXd pi = (pmatrix * x);
		//pmatrix.cwise() / pi;
		VectorXd t = pi.cwise().inverse();
		VectorXd g1 = -pmatrix.transpose() * t;
		g1 = g1.cwise() + pmatrix.rows() * 1/(x.sum()); // * x;
		VectorXd gu = -2*(1.000001-x.sum())/pow(error_x,2) * x;
		t = (rho2d_target-rho2d).cwise()/rho2d_error.cwise().pow(2) * -2;
		VectorXd g2 = rho2dmatrix.transpose() * t;
		//VectorXd g(g2.size());
		gradient.setZero();
		gradient = entropy_scale * ((x.cwise().log().cwise()+1).cwise() * x);
		if(kin)
			gradient += g1.cwise()*x;
		if(light)
			gradient += g2.cwise()*x;
		if(norm)
			gradient += gu;

		//VectorXd::Map(_gradient.data().begin(), _gradient.size()) = g;
		//VectorXd::Map(_gradient.data().begin(), _gradient.size()) = g2.cwise()*x;

		/*VectorXd::Map(_g1.data().begin(), _g1.size()) = g1.cwise()*x;
		VectorXd::Map(_gu.data().begin(), _gu.size()) = gu;
		VectorXd::Map(_g2.data().begin(), _g2.size()) = g2.cwise()*x;*/
	}

	double f_and_gradient(double_vector x, double &fx, double_vector &gradientvector) {
		return 0.;
	}
	MatrixXd pmatrix;
	MatrixXd rho2dmatrix;
	VectorXd orbitweights;
	VectorXd rho2d_target;
	VectorXd rho2d_error;
	double error_x;
	double entropy_scale;
	double k;
	bool kin, light, norm;
};
template<class F>
double nlopt_wrapper(unsigned n, const double *x, double *grad, void *data) {
	F *opt = (F*)data;
	return opt->f(n, x, grad);
}

};


/*
 * chaotic_system.cpp
 *
 * This example demonstrates how one can use odeint to determine the Lyapunov
 * exponents of a chaotic system namely the well known Lorenz system. Furthermore,
 * it shows how odeint interacts with boost.range.
 *
 * Copyright 2011-2012 Karsten Ahnert
 * Copyright 2011-2013 Mario Mulansky
 *
 * 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 <boost/array.hpp>

#include <boost/numeric/odeint.hpp>

#include "gram_schmidt.hpp"

using namespace std;
using namespace boost::numeric::odeint;

const double sigma = 10.0;
const double R = 28.0;
const double b = 8.0 / 3.0;

//[ system_function_without_perturbations
struct lorenz
{
    template< class State , class Deriv >
    void operator()( const State &x_ , Deriv &dxdt_ , double t ) const
    {
        typename boost::range_iterator< const State >::type x = boost::begin( x_ );
        typename boost::range_iterator< Deriv >::type dxdt = boost::begin( dxdt_ );

        dxdt[0] = sigma * ( x[1] - x[0] );
        dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
        dxdt[2] = -b * x[2] + x[0] * x[1];
    }
};
//]

//[ system_function_with_perturbations
const size_t n = 3;
const size_t num_of_lyap = 3;
const size_t N = n + n*num_of_lyap;

typedef boost::array< double , N > state_type;
typedef boost::array< double , num_of_lyap > lyap_type;

void lorenz_with_lyap( const state_type &x , state_type &dxdt , double t )
{
    lorenz()( x , dxdt , t );

    for( size_t l=0 ; l<num_of_lyap ; ++l )
    {
        const double *pert = x.begin() + 3 + l * 3;
        double *dpert = dxdt.begin() + 3 + l * 3;
        dpert[0] = - sigma * pert[0] + 10.0 * pert[1];
        dpert[1] = ( R - x[2] ) * pert[0] - pert[1] - x[0] * pert[2];
        dpert[2] = x[1] * pert[0] + x[0] * pert[1] - b * pert[2];
    }
}
//]

int main( int argc , char **argv )
{
    state_type x;
    lyap_type lyap;

    fill( x.begin() , x.end() , 0.0 );
    x[0] = 10.0 ; x[1] = 10.0 ; x[2] = 5.0;

    const double dt = 0.01;

    //[ integrate_transients_with_range
    // explicitly choose range_algebra to override default choice of array_algebra
    runge_kutta4< state_type , double , state_type , double , range_algebra > rk4;

    // perform 10000 transient steps
    integrate_n_steps( rk4 , lorenz() , std::make_pair( x.begin() , x.begin() + n ) , 0.0 , dt , 10000 );
    //]

    //[ lyapunov_full_code
    fill( x.begin()+n , x.end() , 0.0 );
    for( size_t i=0 ; i<num_of_lyap ; ++i ) x[n+n*i+i] = 1.0;
    fill( lyap.begin() , lyap.end() , 0.0 );

    double t = 0.0;
    size_t count = 0;
    while( true )
    {

        t = integrate_n_steps( rk4 , lorenz_with_lyap , x , t , dt , 100 );
        gram_schmidt< num_of_lyap >( x , lyap , n );
        ++count;

        if( !(count % 100000) )
        {
            cout << t;
            for( size_t i=0 ; i<num_of_lyap ; ++i ) cout << "\t" << lyap[i] / t ;
            cout << endl;
        }
    }
    //]

    return 0;
}


#include <VolViz/VolViz.h>

#include <Eigen/Core>
#include <igl/readOBJ.h>
#include <igl/readOFF.h>
#include <igl/readPLY.h>
#include <igl/readSTL.h>

#include <chrono>
#include <fstream>
#include <iostream>
#include <thread>

auto generateVolume();

auto generateVolume() {
  using namespace VolViz;
  using namespace VolViz::literals;
  Size3 const size(256, 256, 128);
  auto const nVoxels = size(0) * size(1) * size(2);

  std::vector<Color> data;
  data.reserve(3 * nVoxels);
  Color c = Colors::Black();
  for (unsigned int z = 0; z < size(2); ++z) {
    c(2) = (static_cast<float>(z) / static_cast<float>(size(2) - 1));
    for (unsigned int y = 0; y < size(1); ++y) {
      c(1) = (static_cast<float>(y) / static_cast<float>(size(1) - 1));
      for (unsigned int x = 0; x < size(0); ++x) {
        c(0) = (static_cast<float>(x) / static_cast<float>(size(0) - 1));
        data.push_back(c);
      }
    }
  }

  VolumeDescriptor v;
  v.size = size;
  v.voxelSize = {{100_um, 100_um, 200_um}};
  v.type = VolumeType::ColorRGB;

  return std::make_pair(v, data);
}

int main(int argc, char **argv) {
  using namespace VolViz;
  using namespace VolViz::literals;
  using Eigen::Vector3d;
  using Vertices = Eigen::MatrixXd;
  using Triangles = Eigen::MatrixXi;

  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " meshFile" << std::endl;
    return EXIT_FAILURE;
  }

  auto f = std::ifstream(argv[1]);
  auto const filename = std::string(argv[1]);
  auto const ext = filename.substr(filename.size() - 3, 3);
  std::cout << "Loading mesh " << argv[1] << "... " << std::flush;
  Vertices V;
  Triangles T;
  if (ext == "off")
    igl::readOFF(filename, V, T);
  else if (ext == "ply")
    igl::readPLY(filename, V, T);
  else if (ext == "stl") {
    Vertices N;
    igl::readSTL(filename, V, T, N);
  } else if (ext == "obj")
    igl::readOBJ(filename, V, T);
  else {
    std::cerr << std::endl << "Unrecognized mesh format: " << ext << std::endl;
    return EXIT_FAILURE;
  }
  std::cout << "done." << std::endl;
  std::cout << V.rows() << " vertices, " << T.rows() << " triangles."
            << std::endl;

  Eigen::Vector3d const min = V.colwise().minCoeff();
  Eigen::Vector3d const max = V.colwise().maxCoeff();

  Length meshScale = 5_cm;

  std::cout << "bbox: " << min.transpose() << " - " << max.transpose()
            << std::endl;
  std::cout << "bbox size: "
            << ((max - min).transpose() * static_cast<double>(meshScale / 1_mm))
            << " mm" << std::endl;

  //  Eigen::MatrixXd V(3, 3);
  //  V << 0, 1, 0, -1, -1, 0, 1, -1, 0;
  //  Eigen::MatrixXi T(1, 3);
  //  T << 0, 1, 2;

  auto viewer = Visualizer{};

  // Add mesh
  MeshDescriptor mesh;
  mesh.vertices = V.cast<float>();
  mesh.indices = T.cast<std::uint32_t>();
  mesh.movable = true;
  mesh.scale = 50_mm;
  mesh.color = Colors::White();
  viewer.addGeometry("Mesh", mesh);

  viewer.showGrid = true;
  viewer.backgroundColor = (Colors::Magenta() + Colors::Cyan()) / 2.0;

  Light light;
  light.ambientFactor = 1.0f;
  light.color = Colors::White();
  light.position = PositionH(1, 1, 1, 0);

  viewer.addLight(0, light);

  light.position = PositionH(2, 1, 1, 0);
  viewer.addLight(1, light);

  light.position = PositionH(1, 2, 1, 0);
  viewer.addLight(2, light);

  viewer.scale = 1_mm;

  AxisAlignedPlaneDescriptor plane;
  plane.axis = Axis::X;
  // plane.color = Colors::Green();
  plane.color = Colors::White();
  plane.intercept = 0_mm;

  viewer.addGeometry("X-Plane", plane);

  plane.axis = Axis::Y;
  // plane.color = Colors::Blue();
  plane.color = Colors::White();
  plane.intercept = 0_mm;
  viewer.addGeometry("Y-Plane", plane);

  plane.axis = Axis::Z;
  // plane.color = Colors::Red();
  plane.color = Colors::White();
  plane.intercept = 0_mm;
  viewer.addGeometry("Z-Plane", plane);

  // cube
  CubeDescriptor cube;
  cube.color = Colors::Magenta();
  cube.position = {11.5f, 11.5f, 11.5f};
  viewer.addGeometry("Cube", cube);

  std::cout << "Generating volume... " << std::flush;
  auto const vol = generateVolume();
  std::cout << "done." << std::endl;

  viewer.setVolume(vol.first, as_span(vol.second));

  viewer.enableMultithreading();
  viewer.start();

  std::thread workerThread([&viewer, &mesh, &cube]() {
    using Clock = std::chrono::steady_clock;
    using namespace std::chrono_literals;
    auto constexpr updateIntervall = 1s / 30.f;
    int count{0};
    while (viewer) {
      auto const t0 = Clock::now();
      mesh.vertices *= count < 10 ? 1.01f : 0.99f;
      cube.position[0] = static_cast<float>(count - 10) * 12.8f / 10.f;
      viewer.updateGeometry("Cube", cube);
      viewer.updateGeometry("Mesh", mesh);
      if (++count > 20) count = 0;
      auto const timeLeft = -(Clock::now() - t0) + updateIntervall;
      if (timeLeft > 0s) std::this_thread::sleep_for(timeLeft);
    }
  });

  viewer.renderAtFPS(60);

  workerThread.join();

  return EXIT_SUCCESS;
}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    int n, m; cin >> n >> m;
    vector<pair<long long int, long long int>> v(n);
    for (int i = 0; i < n; i++) cin >> v[i].first >> v[i].second;
    sort(v.begin(), v.end());
    cpp_int sum = 0;
    long long int cnt = 0;
    for (int i = 0; i < n; i++) {
        if (cnt + v[i].second > m) {
            sum += (m - cnt) * v[i].first;
            break;
        }
        else sum += v[i].second * v[i].first, cnt += v[i].second;
    }
    cout << sum << endl;

}