/* ================================================================
 *
 * PyCA Project
 *
 * Copyright (c) J. Samuel Preston, Linh K. Ha, Sarang C. Joshi. All
 * rights reserved.  See Copyright.txt or for details.
 *
 * This software is distributed WITHOUT ANY WARRANTY; without even the
 * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 * PURPOSE.  See the above copyright notice for more information.
 *
 * ================================================================ */

#include <FluidKernelFFTBase.h>

#include <FOpers.h>
#include <CudaUtils.h>
#include <boost_mem.h>

// MSVC lacks a few constants and math functions
#ifdef _MSC_VER
#define M_PI 3.141592653589793
#endif

namespace PyCA {

// ================================================================
// FFTLookupTable3D
// ================================================================

template<class T>
void
FFTLookupTable3D<T>::
InitTable(){

    //
    // Test that MAX_FFT_TABLE_LENGTH is big enough
    //
    if(mSize.x > MAX_FFT_TABLE_LENGTH ||
       mSize.y > MAX_FFT_TABLE_LENGTH ||
       mSize.z > MAX_FFT_TABLE_LENGTH)
    {
       throw PyCAException(__FILE__,__LINE__,"Error, not enough static memory allocated!");
    }

    //
    // precompute some values
    //
    double sX = 2.0 * M_PI / mSize.x;
    double sY = 2.0 * M_PI / mSize.y;
    double sZ = 2.0 * M_PI / mSize.z;

    double deltaXSq = mSp.x * mSp.x;
    double deltaYSq = mSp.y * mSp.y;
    double deltaZSq = mSp.z * mSp.z;

    //
    // fill in luts
    //
    for (size_t x = 0; x < (size_t)mSize.x; ++x) {
        mCosWX[x] = (2.0 * cos(sX * static_cast<float>(x)) - 2.0) / deltaXSq;
        mSinWX[x] = sin(sX * static_cast<float>(x)) / mSp.x;
    }

    for (size_t y = 0; y < (size_t)mSize.y; ++y) {
        mCosWY[y] = (2.0 * cos(sY * static_cast<float>(y)) - 2.0) / deltaYSq;
        mSinWY[y] = sin(sY * static_cast<float>(y)) / mSp.y;
    }

    for (size_t z = 0; z < (size_t)mSize.z; ++z) {
        mCosWZ[z] = (2.0 * cos(sZ * static_cast<float>(z)) - 2.0) / deltaZSq;
        mSinWZ[z] = sin(sZ * static_cast<float>(z)) / mSp.z;
    }
}

template<class T>
void
FFTLookupTable3D<T>::
Allocate(){
    mCosWX = new T [MAX_FFT_TABLE_LENGTH];
    mCosWY = new T [MAX_FFT_TABLE_LENGTH];
    mCosWZ = new T [MAX_FFT_TABLE_LENGTH];

    mSinWX = new T [MAX_FFT_TABLE_LENGTH];
    mSinWY = new T [MAX_FFT_TABLE_LENGTH];
    mSinWZ = new T [MAX_FFT_TABLE_LENGTH];
}

template<class T>
void
FFTLookupTable3D<T>::
setSize(const Vec3Di& size,
	const Vec3Df& spacing)
{
    mSize = size;
    mSp   = spacing;
    InitTable();
}

template<class T>
void
FFTLookupTable3D<T>::
Clear(){
    delete []mCosWX;
    delete []mCosWY;
    delete []mCosWZ;

    delete []mSinWX;
    delete []mSinWY;
    delete []mSinWZ;
}

// ================================================================
// FluidKernelFFTBase
// ================================================================

template<int mode, MemoryType mType, class T>
FluidKernelFFTBase<mode, mType, T>::
FluidKernelFFTBase()
  :mAlpha(1.0), mBeta(0.0), mGamma(0.0), mLPow(1.0),
   mDivergenceFree(false),
   mHasFFTPlan(false),
   mSize(0,0,0), mSpacing(0,0,0),
   mComplexSize(0),
   mAllocateSize(0),
   mFFTArrayX(NULL),
   mFFTArrayY(NULL),
   mFFTArrayZ(NULL)
{

}

template<int mode, MemoryType mType, class T>
FluidKernelFFTBase<mode, mType, T>::
~FluidKernelFFTBase()
{
}

template<int mode, MemoryType mType, class T>
void
FluidKernelFFTBase<mode, mType, T>::
setGrid(const GridInfo& grid)
{
    this->setSize(grid);
}

template<int mode, MemoryType mType, class T>
void
FluidKernelFFTBase<mode, mType, T>::setSize(const GridInfo& g)
{

    Vec3Di newSize   = g.size();
    Vec3Df newSpacing= g.spacing();

    bool changeSize    = (mSize    != newSize);
    bool changeSpacing = (mSpacing != newSpacing);

    // TEST
    if(mType != MEM_HOST){
	CudaUtils::CheckCUDAError(__FILE__, __LINE__);
    }
    // END TEST

    if (changeSize) {
        mSize = newSize;

        mComplexSize   = mSize;
        mComplexSize.x = mSize.x/2+1;

        size_t n = mComplexSize.prod();
        if ( n > mAllocateSize)
            Alloc(n);
    }

    if (changeSpacing) {
        mSpacing  = newSpacing;
    }

    // recompute the lookup table
    if (changeSize || changeSpacing)
        mLookupTable.setSize(mSize, mSpacing);

    // TEST
    if(mType != MEM_HOST){
	CudaUtils::CheckCUDAError(__FILE__, __LINE__);
    }
    // END TEST

    // create FFT plan
    if (changeSize) {
        if (mHasFFTPlan)
            DestroyFFTPlan();
        CreateFFTPlan();
    }
}

template<int mode, MemoryType mType, class T>
void
FluidKernelFFTBase<mode, mType, T>::
Alloc(size_t n){
    mFFTArray = CreateSharedArray<mType, ComplexT<T> >(3*n);
    mFFTArrayX = &(mFFTArray.get()[0]);
    mFFTArrayY = &(mFFTArray.get()[n]);
    mFFTArrayZ = &(mFFTArray.get()[2*n]);
    mAllocateSize = n;
}

template<int mode, MemoryType mType, class T>
void
FluidKernelFFTBase<mode, mType, T>::
ApplyImpl(T* dataX, T* dataY, T* dataZ, bool inverseOp, StreamT stream)
{
    //1. Compute FFT of the data, store result in mFFTArray buffers
    // convert the input from real to frequency(complex image)
    toFrequencyDomain(dataX, dataY, dataZ, stream);

    //2. Solve system
    if (mLPow == floor(mLPow)) {
        size_t nPow = (size_t) mLPow;
        for (size_t i=0; i < nPow; ++i)
            frequencyDomainApply(inverseOp, stream);
    }

    //3. convert the output back to time domain
    toSpaceDomain(dataX, dataY, dataZ, stream);

};

template<int mode, MemoryType mType, class T>
void
FluidKernelFFTBase<mode, mType, T>::
Apply(Field3D& out, const Field3D& in,
      bool inverseOp, StreamT stream)
{
    MK_CHECK_REFSIZE_2(mSize, out, in);
    //size_t nElems = mSize.prod();
    //FieldOpers<mode>::MulC(out, in, 1.f / nElems, stream);
    Opers::Copy(out, in, NULL);
    ApplyImpl(out.x, out.y, out.z, inverseOp, stream);
}

template<int mode, MemoryType mType, class T>
void
FluidKernelFFTBase<mode, mType, T>::
Apply(Field3D& out, bool inverseOp, StreamT stream){
    MK_CHECK_REFSIZE_1(mSize, out);
    //size_t nElems = mSize.prod();
    //FieldOpers<mode>::MulC_I(out, 1.f / nElems, stream);
    ApplyImpl(out.x, out.y, out.z, inverseOp, stream);
}

//--------------------------------------------------------------------------------
/**
 * f = Lv
 *
 * v field is overwritten in this operation
 */
template<int mode, MemoryType mType, class T>
void
FluidKernelFFTBase<mode, mType, T>::
applyOperator(Field3D& f, const Field3D& v, StreamT stream){
    Apply(f, v, false, stream);
}

/**
 * v = Kf
 *
 * f field is overwritten in this operation
 */
template<int mode, MemoryType mType, class T>
void
FluidKernelFFTBase<mode, mType, T>::
applyInverseOperator(Field3D& v, const Field3D& f, StreamT stream){
    Apply(v, f, true, stream);
}

/**
 * f = Lv
 *
 * v field is overwritten in this operation (holds f).
 */
template<int mode, MemoryType mType, class T>
void
FluidKernelFFTBase<mode, mType, T>::
applyOperator(Field3D& f, StreamT stream){
    Apply(f, false, stream);
}

/**
 * v = Kf
 *
 * f field is overwritten in this operation (holds v).
 */
template<int mode, MemoryType mType, class T>
void
FluidKernelFFTBase<mode, mType, T>::
applyInverseOperator(Field3D& v, StreamT stream){
    Apply(v, true, stream);
}

// template instantiations
template class FluidKernelFFTBase<EXEC_CPU, MEM_HOST, float>;
#ifdef CUDA_ENABLED
template class FluidKernelFFTBase<EXEC_GPU, MEM_DEVICE, float>;
#endif

}; // end namespace PyCA


/**
 * @file sgd_test.cpp
 * @author Ryan Curtin
 *
 * Test file for SGD (stochastic gradient descent).
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/core/optimizers/sgd/sgd.hpp>
#include <mlpack/core/optimizers/problems/generalized_rosenbrock_function.hpp>
#include <mlpack/core/optimizers/problems/sgd_test_function.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace std;
using namespace arma;
using namespace mlpack;
using namespace mlpack::optimization;
using namespace mlpack::optimization::test;

BOOST_AUTO_TEST_SUITE(SGDTest);

BOOST_AUTO_TEST_CASE(SimpleSGDTestFunction)
{
  SGDTestFunction f;
  StandardSGD s(0.0003, 1, 5000000, 1e-9, true);

  arma::mat coordinates = f.GetInitialPoint();
  double result = s.Optimize(f, coordinates);

  BOOST_REQUIRE_CLOSE(result, -1.0, 0.05);
  BOOST_REQUIRE_SMALL(coordinates[0], 1e-3);
  BOOST_REQUIRE_SMALL(coordinates[1], 1e-7);
  BOOST_REQUIRE_SMALL(coordinates[2], 1e-7);
}

BOOST_AUTO_TEST_CASE(GeneralizedRosenbrockTest)
{
  // Loop over several variants.
  for (size_t i = 10; i < 50; i += 5)
  {
    // Create the generalized Rosenbrock function.
    GeneralizedRosenbrockFunction f(i);

    StandardSGD s(0.001, 1, 0, 1e-15, true);

    arma::mat coordinates = f.GetInitialPoint();
    double result = s.Optimize(f, coordinates);

    BOOST_REQUIRE_SMALL(result, 1e-10);
    for (size_t j = 0; j < i; ++j)
      BOOST_REQUIRE_CLOSE(coordinates[j], (double) 1.0, 1e-3);
  }
}

BOOST_AUTO_TEST_SUITE_END();


#include <algorithm>
#include <queue>
#include <boost/math/constants/constants.hpp>
#include <Eigen/Sparse>
#include <Euclid/Geometry/TriMeshGeometry.h>
#include <Euclid/Math/Numeric.h>
#include <Euclid/Topology/MeshTopology.h>

namespace Euclid
{

namespace _impl
{

template<typename Mesh, typename VertexCurvatureMap>
bool check_gauss_bonnet(const Mesh& mesh, VertexCurvatureMap vcm)
{
    using Scalar =
        typename boost::property_traits<VertexCurvatureMap>::value_type;
    Scalar sum = 0;
    for (auto v : vertices(mesh)) {
        sum += get(vcm, v);
    }
    auto chi = euler_characteristic(mesh);
    return eq_almost(sum, boost::math::constants::two_pi<Scalar>() * chi);
}

template<typename Mesh, typename VertexRadiusMap, typename EdgeWeightMap>
decltype(auto) elen_from_circle_packing_metric(
    const Mesh& mesh,
    typename boost::graph_traits<Mesh>::edge_descriptor e,
    VertexRadiusMap vrm,
    EdgeWeightMap ewm)
{
    auto h = halfedge(e, mesh);
    auto s = source(h, mesh);
    auto t = target(h, mesh);
    auto rs = get(vrm, s);
    auto rt = get(vrm, t);
    auto cosw = get(ewm, e);
    auto l2 = rs * rs + rt * rt + 2 * rs * rt * cosw;
    l2 = std::max(l2, static_cast<decltype(l2)>(0));
    return std::sqrt(l2);
}

template<typename T>
T cos12(T l1, T l2, T l3)
{
    auto cos = (l1 * l1 + l2 * l2 - l3 * l3) / (2 * l1 * l2);
    cos = std::min(cos, static_cast<T>(1));
    cos = std::max(cos, static_cast<T>(-1));
    return cos;
}

template<typename Mesh, typename T, typename EIMap>
T corner_angle(const Mesh& mesh,
               typename boost::graph_traits<Mesh>::halfedge_descriptor h1,
               const std::vector<T>& lengths,
               EIMap eimap)
{
    auto h2 = next(h1, mesh);
    auto h3 = prev(h1, mesh);
    auto e1 = edge(h1, mesh);
    auto e2 = edge(h2, mesh);
    auto e3 = edge(h3, mesh);
    auto l1 = lengths[get(eimap, e1)];
    auto l2 = lengths[get(eimap, e2)];
    auto l3 = lengths[get(eimap, e3)];
    auto c = _impl::cos12(l1, l2, l3);
    return std::acos(c);
}

template<typename Mesh, typename T, typename EIMap>
T gaussian(const Mesh& mesh,
           typename boost::graph_traits<Mesh>::vertex_descriptor v,
           const std::vector<T>& lengths,
           EIMap eimap)
{
    T k;
    if (CGAL::is_border(v, mesh)) {
        k = boost::math::constants::pi<T>();
    }
    else {
        k = boost::math::constants::two_pi<T>();
    }
    for (auto h : halfedges_around_target(v, mesh)) {
        if (!CGAL::is_border(h, mesh)) {
            k -= corner_angle(mesh, h, lengths, eimap);
        }
    }
    return k;
}

template<typename Mesh,
         typename VertexRadiusMap,
         typename EdgeIndexMap,
         typename T>
void solve_power_circle(const Mesh& mesh,
                        typename boost::graph_traits<Mesh>::face_descriptor f,
                        VertexRadiusMap vrm,
                        EdgeIndexMap eim,
                        const std::vector<T>& lengths,
                        std::vector<T>& weights)
{
    // construct local coordinates for a triangle
    auto h = *(halfedges_around_face(halfedge(f, mesh), mesh).first);
    auto v = target(h, mesh);
    T l1 = lengths[get(eim, edge(h, mesh))];
    T x1 = 0;
    T y1 = 0;
    T r1 = get(vrm, v);

    h = next(h, mesh);
    v = target(h, mesh);
    T l2 = lengths[get(eim, edge(h, mesh))];
    T x2 = l2;
    T y2 = 0;
    T r2 = get(vrm, v);

    h = next(h, mesh);
    v = target(h, mesh);
    T l3 = lengths[get(eim, edge(h, mesh))];
    T cos = cos12(l1, l2, l3);
    T sin = std::sqrt(1 - cos * cos);
    T x3 = cos * l1;
    T y3 = sin * l1;
    T r3 = get(vrm, v);

    // solve the power circle in local coordinates
    auto x1_2 = x1 * x1;
    auto y1_2 = y1 * y1;
    auto r1_2 = r1 * r1;
    auto x2_2 = x2 * x2;
    auto y2_2 = y2 * y2;
    auto r2_2 = r2 * r2;
    auto x3_2 = x3 * x3;
    auto y3_2 = y3 * y3;
    auto r3_2 = r3 * r3;
    auto a = -x1_2 + x2_2 - y1_2 + y2_2 + r1_2 - r2_2;
    auto b = -x1_2 + x3_2 - y1_2 + y3_2 + r1_2 - r3_2;
    auto c = 2 * ((y1 - y2) * (x3 - x1) - (y1 - y3) * (x2 - x1));

    auto y = ((x2 - x1) * b - (x3 - x1) * a) / c;
    auto x = (2 * (y1 - y3) * y + b) / (2 * (x3 - x1));

    // calculate the height from the center to each edge
    auto d1 = std::sqrt((x1 - x) * (x1 - x) + (y1 - y) * (y1 - y));
    auto d2 = std::sqrt((x2 - x) * (x2 - x) + (y2 - y) * (y2 - y));
    auto d3 = std::sqrt((x3 - x) * (x3 - x) + (y3 - y) * (y3 - y));

    auto cos1 = cos12(d3, l1, d1);
    auto sin1 = std::sqrt(1 - cos1 * cos1);
    auto height1 = d3 * sin1;
    h = next(h, mesh);
    weights[get(eim, edge(h, mesh))] += height1;

    auto cos2 = cos12(d1, l2, d2);
    auto sin2 = std::sqrt(1 - cos2 * cos2);
    auto height2 = d1 * sin2;
    h = next(h, mesh);
    weights[get(eim, edge(h, mesh))] += height2;

    auto cos3 = cos12(d2, l3, d3);
    auto sin3 = std::sqrt(1 - cos3 * cos3);
    auto height3 = d2 * sin3;
    h = next(h, mesh);
    weights[get(eim, edge(h, mesh))] += height3;
}

template<typename Mesh,
         typename VertexRadiusMap,
         typename EdgeWeightMap,
         typename VertexIndexMap,
         typename EdgeIndexMap,
         typename T>
void hessian(const Mesh& mesh,
             VertexRadiusMap vrm,
             EdgeWeightMap ewm,
             VertexIndexMap vim,
             EdgeIndexMap eim,
             const std::vector<T>& lengths,
             Eigen::SparseMatrix<T>& H)
{
    using Triplet = Eigen::Triplet<T>;
    using Triplets = std::vector<Triplet>;

    std::vector<T> weights(num_edges(mesh), 0);
    for (auto f : faces(mesh)) {
        solve_power_circle(mesh, f, vrm, eim, lengths, weights);
    }

    Triplets triplets;
    triplets.reserve(num_halfedges(mesh) + num_vertices(mesh));
    for (auto v : vertices(mesh)) {
        T sum = 0;
        auto vi_idx = get(vim, v);
        for (auto h : halfedges_around_target(v, mesh)) {
            auto vj_idx = get(vim, source(h, mesh));
            auto eidx = get(eim, edge(h, mesh));
            auto w = weights[eidx] / lengths[eidx];
            sum += w;
            triplets.emplace_back(vi_idx, vj_idx, -w);
        }
        triplets.emplace_back(vi_idx, vi_idx, sum);
    }

    H.resize(num_vertices(mesh), num_vertices(mesh));
    H.setFromTriplets(triplets.begin(), triplets.end());
    H.makeCompressed();
}

template<typename Mesh,
         typename VertexRadiusMap,
         typename EdgeWeightMap,
         typename SEM,
         typename SVM,
         typename VertexUVMap,
         typename VertexParameterizedMap>
void embed_circle_packing_metric(const Mesh& mesh,
                                 VertexRadiusMap vrm,
                                 EdgeWeightMap ewm,
                                 CGAL::Seam_mesh<Mesh, SEM, SVM>& seam_mesh,
                                 VertexUVMap uvm,
                                 VertexParameterizedMap vpm)
{
    using Seam_mesh = CGAL::Seam_mesh<Mesh, SEM, SVM>;
    using fd = typename boost::graph_traits<Seam_mesh>::face_descriptor;
    using Point_2 = typename boost::property_traits<VertexUVMap>::value_type;
    using Point_3 = typename CGAL::Kernel_traits<Point_2>::Kernel::Point_3;
    using FT = typename CGAL::Kernel_traits<Point_2>::Kernel::FT;

    // mesh and seam_mesh have the same number of faces
    // seam_mesh does not have face_index, use mesh's
    auto fimap = get(boost::face_index, mesh);
    std::vector<bool> visited(num_faces(seam_mesh), false);
    std::queue<fd> queue;

    // flatten the root face
    auto froot = *(faces(seam_mesh).first);
    auto hroot = halfedge(froot, seam_mesh);
    auto vroot = target(hroot, seam_mesh);
    auto l0 = _impl::elen_from_circle_packing_metric(
        mesh, edge(hroot.tmhd, mesh), vrm, ewm);
    put(uvm, vroot, Point_2(0, 0));
    put(vpm, vroot, true);
    hroot = next(hroot, seam_mesh);
    vroot = target(hroot, seam_mesh);
    auto l1 = _impl::elen_from_circle_packing_metric(
        mesh, edge(hroot.tmhd, mesh), vrm, ewm);
    put(uvm, vroot, Point_2(l1, 0));
    put(vpm, vroot, true);
    hroot = next(hroot, seam_mesh);
    vroot = target(hroot, seam_mesh);
    auto l2 = _impl::elen_from_circle_packing_metric(
        mesh, edge(hroot.tmhd, mesh), vrm, ewm);
    auto cos0 = _impl::cos12(l0, l1, l2);
    auto sin0 = std::sqrt(1 - cos0 * cos0);
    put(uvm, vroot, Point_2(cos0 * l0, sin0 * l0));
    put(vpm, vroot, true);
    visited[get(fimap, face(hroot.tmhd, mesh))] = true;
    queue.push(froot);
    size_t nv = 3;
    size_t nf = 1;

    // propogate the flattening
    while (!queue.empty()) {
        auto f = queue.front();
        queue.pop();
        for (auto h :
             halfedges_around_face(halfedge(f, seam_mesh), seam_mesh)) {
            auto hoppo = opposite(h, seam_mesh);
            if (!CGAL::is_border(hoppo, seam_mesh)) {
                auto foppo_idx = get(fimap, face(hoppo.tmhd, mesh));
                if (visited[foppo_idx]) {
                    continue;
                }
                visited[foppo_idx] = true;
                queue.push(face(hoppo, seam_mesh));
                ++nf;

                auto vk = target(next(hoppo, seam_mesh), seam_mesh);
                if (get(vpm, vk)) {
                    continue;
                }
                put(vpm, vk, true);
                ++nv;

                // use the intersection point of two circles as the next
                // flattened vertex's uv-coordinate
                auto vs = source(hoppo, seam_mesh);
                auto vt = target(hoppo, seam_mesh);
                auto ps = get(uvm, vs);
                auto pt = get(uvm, vt);
                auto ls = _impl::elen_from_circle_packing_metric(
                    mesh, edge(prev(hoppo.tmhd, mesh), mesh), vrm, ewm);
                auto lt = _impl::elen_from_circle_packing_metric(
                    mesh, edge(next(hoppo.tmhd, mesh), mesh), vrm, ewm);
                auto d = std::sqrt((pt - ps).squared_length());
                auto a = (ls * ls - lt * lt + d * d) / (2 * d);
                auto h =
                    std::sqrt(std::max(ls * ls - a * a, static_cast<FT>(0)));
                auto pm = ps + a * (pt - ps) / d;
                auto u = pm.x() + h * (pt.y() - ps.y()) / d;
                auto v = pm.y() - h * (pt.x() - ps.x()) / d;
                assert(!std::isnan(u));
                assert(!std::isnan(v));

                // check orientation
                Point_3 pi(ps.x(), ps.y(), 0);
                Point_3 pj(pt.x(), pt.y(), 0);
                Point_3 pk(u, v, 0);
                if (CGAL::cross_product(pj - pi, pk - pi).z() > 0) {
                    put(uvm, vk, Point_2(u, v));
                }
                else {
                    u = pm.x() - h * (pt.y() - ps.y()) / d;
                    v = pm.y() + h * (pt.x() - ps.x()) / d;
                    put(uvm, vk, Point_2(u, v));
                }
            }
        }
    }
    assert(nv == num_vertices(seam_mesh));
    assert(nf == num_faces(seam_mesh));
}

} // namespace _impl

template<typename Mesh, typename VertexRadiusMap, typename EdgeWeightMap>
void circle_packing_metric(Mesh& mesh, VertexRadiusMap vrm, EdgeWeightMap ewm)
{
    using Scalar = typename boost::property_traits<VertexRadiusMap>::value_type;
    auto eimap = get(boost::edge_index, mesh);

    // compute edge lengths
    std::vector<Scalar> lens;
    lens.reserve(num_edges(mesh));
    for (auto e : edges(mesh)) {
        auto l = edge_length(e, mesh);
        lens.push_back(l);
    }

    // compute vertex radii
    for (auto v : vertices(mesh)) {
        Scalar radius;
        bool initialized = false;
        for (auto h : halfedges_around_target(v, mesh)) {
            if (!CGAL::is_border(h, mesh)) {
                auto e1 = get(eimap, edge(h, mesh));
                auto e2 = get(eimap, edge(next(h, mesh), mesh));
                auto e3 = get(eimap, edge(prev(h, mesh), mesh));
                auto r = (lens[e1] + lens[e2] - lens[e3]) / 2;
                if (!initialized) {
                    radius = r;
                    initialized = true;
                }
                else {
                    radius = std::min(radius, r);
                }
            }
        }
        put(vrm, v, radius);
    }

    // compute edge weights
    for (auto e : edges(mesh)) {
        auto h = halfedge(e, mesh);
        auto s = source(h, mesh);
        auto t = target(h, mesh);
        auto rs = get(vrm, s);
        auto rt = get(vrm, t);
        auto l = lens[get(eimap, e)];
        // don't use cos12 since it's okay if cosw > 1
        auto cosw = (l * l - rs * rs - rt * rt) / (2 * rs * rt);
        put(ewm, e, cosw);
    }
}

template<typename Mesh,
         typename VertexRadiusMap,
         typename EdgeWeightMap,
         typename SEM,
         typename SVM,
         typename VertexUVMap>
void embed_circle_packing_metric(const Mesh& mesh,
                                 VertexRadiusMap vrm,
                                 EdgeWeightMap ewm,
                                 CGAL::Seam_mesh<Mesh, SEM, SVM>& seam_mesh,
                                 VertexUVMap uvm)
{
    using Seam_mesh = CGAL::Seam_mesh<Mesh, SEM, SVM>;
    using Vertex = typename boost::graph_traits<Seam_mesh>::vertex_descriptor;
    using VPM = std::map<Vertex, bool>;

    VPM vpm;
    _impl::embed_circle_packing_metric(
        mesh,
        vrm,
        ewm,
        seam_mesh,
        uvm,
        boost::associative_property_map<VPM>(vpm));
}

template<typename Mesh,
         typename VertexRadiusMap,
         typename EdgeWeightMap,
         typename VertexCurvatureMap>
RicciFlowSolverStatus ricci_flow(Mesh& mesh,
                                 VertexRadiusMap vrm,
                                 EdgeWeightMap ewm,
                                 VertexCurvatureMap vcm,
                                 const RicciFlowSolverSettings& settings)
{
    std::cout << "==========Ricci flow==========" << std::endl
              << "solver type: ";
    if (settings.type == RicciFlowSolverType::GradientDescent) {
        std::cout << "gradient descent" << std::endl;
    }
    else {
        std::cout << "Newton" << std::endl;
    }
    std::cout << "step: " << settings.step << std::endl
              << "eps: " << settings.eps << std::endl
              << "max iters: " << settings.max_iters << std::endl
              << "==============================" << std::endl;

    if (!_impl::check_gauss_bonnet(mesh, vcm)) {
        std::cerr << "Error: target curvatures are not admissible."
                  << std::endl;
        return RicciFlowSolverStatus::InvalidInput;
    }

    using Scalar = typename boost::property_traits<VertexRadiusMap>::value_type;
    using SpMat = Eigen::SparseMatrix<Scalar>;
    using Vec = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    auto vimap = get(boost::vertex_index, mesh);
    auto eimap = get(boost::edge_index, mesh);
    std::vector<Scalar> lengths(num_edges(mesh));
    auto type = settings.type;

    Eigen::SimplicialLDLT<SpMat> solver;
    if (settings.type == RicciFlowSolverType::Newton) {
        // prefactor the solver using graph Laplacian
        auto L = graph_laplacian_matrix(mesh);
        solver.analyzePattern(L);
    }

    for (auto iter = 0; iter < settings.max_iters; ++iter) {
        // update edge lengths from current vertex radii and edge weights
        for (auto e : edges(mesh)) {
            lengths[get(eimap, e)] =
                _impl::elen_from_circle_packing_metric(mesh, e, vrm, ewm);
        }

        // compute the gradients based on solver type
        Scalar kdiff_max = 0;
        Vec grads = Vec::Zero(num_vertices(mesh));
        if (type == RicciFlowSolverType::Newton) {
            // FIXME: Newton is not working
            assert(type == RicciFlowSolverType::GradientDescent);
            // SpMat H;
            // _impl::hessian(mesh, vrm, ewm, vimap, eimap, lengths, H);
            // solver.factorize(H);
            // Vec b(num_vertices(mesh));
            // for (auto v : vertices(mesh)) {
            //     auto kdiff =
            //         get(vcm, v) - _impl::gaussian(mesh, v, lengths, eimap);
            //     b(get(vimap, v)) = kdiff;
            //     kdiff_max = std::max(std::abs(kdiff), kdiff_max);
            // }
            // grads = solver.solve(b);
        }
        else { // GradientDescent
            for (auto v : vertices(mesh)) {
                auto kdiff =
                    get(vcm, v) - _impl::gaussian(mesh, v, lengths, eimap);
                grads(get(vimap, v)) = kdiff * settings.step;
                kdiff_max = std::max(std::abs(kdiff), kdiff_max);
            }
        }

        // optimize the Ricci energy
        for (auto v : vertices(mesh)) {
            auto u = std::log(get(vrm, v)); // convert radius to scaling factor
            u += grads(get(vimap, v));      // Ricci flow
            put(vrm, v, std::exp(u));       // convert back to vertex radius
        }

        // stopping criterion
        if (settings.verbose) {
            std::cout << "iter: " << iter << ", error: " << kdiff_max
                      << std::endl;
        }
        if (kdiff_max < settings.eps) {
            std::cout << "Optimization done." << std::endl;
            return RicciFlowSolverStatus::Optimal;
        }
    }
    std::cout << "Max iterations reached." << std::endl;
    return RicciFlowSolverStatus::Suboptimal;
}

template<typename Mesh, typename SEM, typename SVM>
Ricci_flow_parameterizer3<Mesh, SEM, SVM>::Ricci_flow_parameterizer3(
    const Mesh& mesh)
    : _underlying_mesh(mesh), _vrpm(_vrm), _ewpm(_ewm), _vcpm(_vcm)
{
    circle_packing_metric(_underlying_mesh, _vrpm, _ewpm);
    for (auto v : vertices(mesh)) {
        put(_vcpm, v, static_cast<Scalar>(0));
    }
}

template<typename Mesh, typename SEM, typename SVM>
void Ricci_flow_parameterizer3<Mesh, SEM, SVM>::set_solver_settings(
    const RicciFlowSolverSettings& settings)
{
    _settings = settings;
}

template<typename Mesh, typename SEM, typename SVM>
void Ricci_flow_parameterizer3<Mesh, SEM, SVM>::add_cone(
    typename boost::graph_traits<Mesh>::vertex_descriptor v,
    Scalar k)
{
    put(_vcpm, v, k * boost::math::constants::pi<Scalar>());
}

template<typename Mesh, typename SEM, typename SVM>
template<typename VertexUVMap,
         typename VertexIndexMap,
         typename VertexParameterizedMap>
CGAL::Surface_mesh_parameterization::Error_code
Ricci_flow_parameterizer3<Mesh, SEM, SVM>::parameterize(
    TriangleMesh& mesh,
    halfedge_descriptor,
    VertexUVMap uvmap,
    VertexIndexMap,
    VertexParameterizedMap vpmap)
{
    auto status = ricci_flow(_underlying_mesh, _vrpm, _ewpm, _vcpm, _settings);
    _impl::embed_circle_packing_metric(
        _underlying_mesh, _vrpm, _ewpm, mesh, uvmap, vpmap);
    if (status == RicciFlowSolverStatus::InvalidInput) {
        return CGAL::Surface_mesh_parameterization::ERROR_WRONG_PARAMETER;
    }
    else {
        return CGAL::Surface_mesh_parameterization::OK;
    }
}

} // namespace Euclid


#include "mtf/SSM//HomographyEstimator.h"
#include "mtf/Utilities/miscUtils.h"
#include "opencv2/core/core_c.h"
#include "opencv2/calib3d/calib3d.hpp"
#include <boost/random/uniform_int_distribution.hpp>

_MTF_BEGIN_NAMESPACE

HomographyEstimator::HomographyEstimator(int _modelPoints, bool _use_boost_rng)
	: SSMEstimator(_modelPoints, cvSize(3, 3), 1, _use_boost_rng) {
	assert(_modelPoints >= 4);
	checkPartialSubsets = false;
}

int HomographyEstimator::runKernel(const CvMat* m1, const CvMat* m2, CvMat* H) {
	int i, count = m1->rows * m1->cols;
	const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
	const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;

	double LtL[9][9], W[9][1], V[9][9];
	CvMat _LtL = cvMat(9, 9, CV_64F, LtL);
	CvMat matW = cvMat(9, 1, CV_64F, W);
	CvMat matV = cvMat(9, 9, CV_64F, V);
	CvMat _H0 = cvMat(3, 3, CV_64F, V[8]);
	CvMat _Htemp = cvMat(3, 3, CV_64F, V[7]);
	CvPoint2D64f cM = { 0, 0 }, cm = { 0, 0 }, sM = { 0, 0 }, sm = { 0, 0 };

	for(i = 0; i < count; i++) {
		cm.x += m[i].x;
		cm.y += m[i].y;
		cM.x += M[i].x;
		cM.y += M[i].y;
	}

	cm.x /= count;
	cm.y /= count;
	cM.x /= count;
	cM.y /= count;

	for(i = 0; i < count; i++) {
		sm.x += fabs(m[i].x - cm.x);
		sm.y += fabs(m[i].y - cm.y);
		sM.x += fabs(M[i].x - cM.x);
		sM.y += fabs(M[i].y - cM.y);
	}

	if(fabs(sm.x) < DBL_EPSILON || fabs(sm.y) < DBL_EPSILON ||
		fabs(sM.x) < DBL_EPSILON || fabs(sM.y) < DBL_EPSILON)
		return 0;
	sm.x = count / sm.x;
	sm.y = count / sm.y;
	sM.x = count / sM.x;
	sM.y = count / sM.y;

	double invHnorm[9] = { 1. / sm.x, 0, cm.x, 0, 1. / sm.y, cm.y, 0, 0, 1 };
	double Hnorm2[9] = { sM.x, 0, -cM.x * sM.x, 0, sM.y, -cM.y * sM.y, 0, 0, 1 };
	CvMat _invHnorm = cvMat(3, 3, CV_64FC1, invHnorm);
	CvMat _Hnorm2 = cvMat(3, 3, CV_64FC1, Hnorm2);

	cvZero(&_LtL);
	for(i = 0; i < count; i++) {
		double x = (m[i].x - cm.x) * sm.x, y = (m[i].y - cm.y) * sm.y;
		double X = (M[i].x - cM.x) * sM.x, Y = (M[i].y - cM.y) * sM.y;
		double Lx[] = { X, Y, 1, 0, 0, 0, -x * X, -x * Y, -x };
		double Ly[] = { 0, 0, 0, X, Y, 1, -y * X, -y * Y, -y };
		int j, k;
		for(j = 0; j < 9; j++)
			for(k = j; k < 9; k++)
				LtL[j][k] += Lx[j] * Lx[k] + Ly[j] * Ly[k];
	}
	cvCompleteSymm(&_LtL);

	//cvSVD( &_LtL, &matW, 0, &matV, CV_SVD_MODIFY_A + CV_SVD_V_T );
	cvEigenVV(&_LtL, &matV, &matW);
	cvMatMul(&_invHnorm, &_H0, &_Htemp);
	cvMatMul(&_Htemp, &_Hnorm2, &_H0);
	cvConvertScale(&_H0, H, 1. / _H0.data.db[8]);

	return 1;
}

void HomographyEstimator::computeReprojError(const CvMat* m1, const CvMat* m2,
	const CvMat* model, CvMat* _err) {
	int i, count = m1->rows * m1->cols;
	const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
	const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
	const double* H = model->data.db;
	float* err = _err->data.fl;

	for(i = 0; i < count; i++) {
		double ww = 1. / (H[6] * M[i].x + H[7] * M[i].y + 1.);
		double dx = (H[0] * M[i].x + H[1] * M[i].y + H[2]) * ww - m[i].x;
		double dy = (H[3] * M[i].x + H[4] * M[i].y + H[5]) * ww - m[i].y;
		err[i] = (float)(dx * dx + dy * dy);
	}
}

bool HomographyEstimator::refine(const CvMat* m1, const CvMat* m2,
	CvMat* model, int maxIters) {
	LevMarq solver(8, 0, cvTermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, maxIters, DBL_EPSILON));
	int i, j, k, count = m1->rows * m1->cols;
	const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
	const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
	CvMat modelPart = cvMat(solver.param->rows, solver.param->cols, model->type, model->data.ptr);
	cvCopy(&modelPart, solver.param);

	for(;;) {
		const CvMat* _param = 0;
		CvMat *_JtJ = 0, *_JtErr = 0;
		double* _errNorm = 0;

		if(!solver.updateAlt(_param, _JtJ, _JtErr, _errNorm))
			break;

		for(i = 0; i < count; i++) {
			const double* h = _param->data.db;
			double Mx = M[i].x, My = M[i].y;
			double ww = h[6] * Mx + h[7] * My + 1.;
			ww = fabs(ww) > DBL_EPSILON ? 1. / ww : 0;
			double _xi = (h[0] * Mx + h[1] * My + h[2]) * ww;
			double _yi = (h[3] * Mx + h[4] * My + h[5]) * ww;
			double err[] = { _xi - m[i].x, _yi - m[i].y };
			if(_JtJ || _JtErr) {
				double J[][8] = {
					{ Mx * ww, My * ww, ww, 0, 0, 0, -Mx*ww * _xi, -My*ww * _xi },
					{ 0, 0, 0, Mx * ww, My * ww, ww, -Mx*ww * _yi, -My*ww * _yi }
				};

				for(j = 0; j < 8; j++) {
					for(k = j; k < 8; k++)
						_JtJ->data.db[j * 8 + k] += J[0][j] * J[0][k] + J[1][j] * J[1][k];
					_JtErr->data.db[j] += J[0][j] * err[0] + J[1][j] * err[1];
				}
			}
			if(_errNorm)
				*_errNorm += err[0] * err[0] + err[1] * err[1];
		}
	}

	cvCopy(solver.param, &modelPart);
	return true;
}

cv::Mat estimateHomography(cv::InputArray _points1, cv::InputArray _points2,
	cv::OutputArray _mask, const SSMEstimatorParams &params){
	cv::Mat points1 = _points1.getMat(), points2 = _points2.getMat();
	int n_pts = points1.checkVector(2);
	CV_Assert(n_pts >= 0 && points2.checkVector(2) == n_pts &&
		points1.type() == points2.type());

	cv::Mat H(3, 3, CV_64F);
	CvMat _pt1 = points1, _pt2 = points2;
	CvMat matH = H, c_mask, *p_mask = 0;
	if(_mask.needed()){
		_mask.create(n_pts, 1, CV_8U, -1, true);
		p_mask = &(c_mask = _mask.getMat());
	}
	bool ok = estimateHomography(&_pt1, &_pt2, &matH, p_mask, params) > 0;
	if(!ok)
		H = cv::Scalar(0);
	return H;
}

int estimateHomography(const CvMat* objectPoints, const CvMat* imagePoints,
	CvMat* __H, CvMat* mask, const SSMEstimatorParams &params){
	bool result = false;
	cv::Ptr<CvMat> m, M, tempMask;

	double H[9];
	CvMat matH = cvMat(3, 3, CV_64FC1, H);

	CV_Assert(CV_IS_MAT(imagePoints) && CV_IS_MAT(objectPoints));

	int n_pts = MAX(imagePoints->cols, imagePoints->rows);
	CV_Assert(n_pts >= params.n_model_pts);

	m = cvCreateMat(1, n_pts, CV_64FC2);
	cvConvertPointsHomogeneous(imagePoints, m);

	M = cvCreateMat(1, n_pts, CV_64FC2);
	cvConvertPointsHomogeneous(objectPoints, M);

	if(mask){
		CV_Assert(CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&
			(mask->rows == 1 || mask->cols == 1) &&
			mask->rows*mask->cols == n_pts);
	}
	if(mask || n_pts > params.n_model_pts)
		tempMask = cvCreateMat(1, n_pts, CV_8U);
	if(!tempMask.empty())
		cvSet(tempMask, cvScalarAll(1.));

	HomographyEstimator estimator(params.n_model_pts, params.use_boost_rng);
	int method = n_pts == params.n_model_pts ? 0 : params.method_cv;
	if(method == CV_LMEDS)
		result = estimator.runLMeDS(M, m, &matH, tempMask, params.confidence,
		params.max_iters, params.max_subset_attempts);
	else if(method == CV_RANSAC)
		result = estimator.runRANSAC(M, m, &matH, tempMask, params.ransac_reproj_thresh,
		params.confidence, params.max_iters, params.max_subset_attempts);
	else
		result = estimator.runKernel(M, m, &matH) > 0;

	if(result && n_pts > params.n_model_pts){
		utils::icvCompressPoints((CvPoint2D64f*)M->data.ptr, tempMask->data.ptr, 1, n_pts);
		n_pts = utils::icvCompressPoints((CvPoint2D64f*)m->data.ptr, tempMask->data.ptr, 1, n_pts);
		M->cols = m->cols = n_pts;
		if(method == CV_RANSAC)
			estimator.runKernel(M, m, &matH);
		if(params.refine){
			estimator.refine(M, m, &matH, params.lm_max_iters);
		}
	}

	if(result)
		cvConvert(&matH, __H);

	if(mask && tempMask){
		if(CV_ARE_SIZES_EQ(mask, tempMask))
			cvCopy(tempMask, mask);
		else
			cvTranspose(tempMask, mask);
	}

	return (int)result;
}

//int	cvFindHomography(const PtsT &objectPoints, const PtsT &imagePoints,
//	ProjWarpT & __H, EstimatorMethod method, double ransacReprojThreshold,
//	VectorXb &mask){
//	int count = objectPoints.cols();
//	assert(count >= 4);
//	assert(mask.size() == count);

//	const double confidence = 0.995;
//	const int maxIters = 2000;
//	const double defaultRANSACReprojThreshold = 3;
//	bool result = false;

//	HomPtsT m, M;
//	ProjWarpT matH;
//	if(ransacReprojThreshold <= 0)
//		ransacReprojThreshold = defaultRANSACReprojThreshold;

//	homogenize(imagePoints, m);
//	homogenize(objectPoints, M);
//
//	mask.fill(1);

//	AffineEstimator estimator(4);
//	if(count == 4)
//		method = EstimatorMethod::LSTSQR;

//	if(method == EstimatorMethod::LMEDS)
//		result = estimator.runLMeDS(M, m, matH, mask, confidence, maxIters);
//	else if(method == EstimatorMethod::RANSAC)
//		result = estimator.runRANSAC(M, m, matH, mask, ransacReprojThreshold, confidence, maxIters);
//	else
//		result = estimator.runKernel(M, m, matH) > 0;

//	if(result && count > 4){
//		compressPoints(m, mask, 1, count);
//		count = compressPoints(M, mask, 1, count);
//		if(method == EstimatorMethod::RANSAC)
//			estimator.runKernel(M, m, matH);
//		estimator.refine(M, m, matH, 10);
//	}

//	if(result)
//		__H = matH;

//	return result;
//}

_MTF_END_NAMESPACE


#ifndef TRIUMF_NMR_NUCLEI_HPP
#define TRIUMF_NMR_NUCLEI_HPP

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

#include <triumf/nmr/utilities.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

// nuclear magnetic resonance (NMR)
namespace nmr {

// NMR probe nuclei and their properties
namespace nuclei {

// niobium-93
template <typename T = double> struct niobium_93 {
  // radioactive half-life (s)
  static inline constexpr T half_life() {
    return std::numeric_limits<T>::infinity();
  }
  // radioactive lifetime (s)
  static inline constexpr T lifetime() {
    return half_life() / boost::math::constants::ln_two<T>();
  }
  // spin quantum number
  static inline constexpr T spin() { return 9.0 / 2.0; }
  // magnetic dipole moment (nm)
  static inline constexpr T magnetic_dipole_moment() { return 6.163; }
  // electric dipole moment (b)
  static inline constexpr T electric_quadrupole_moment() { return -0.32; }
  // gyromagnetic ratio (s^-1 T^-1)
  static inline constexpr T gyromagnetic_ratio() {
    return triumf::nmr::utilities::calculate_gamma<T>(magnetic_dipole_moment(),
                                                      spin());
  }
  // gyromagnetic ratio (MHz / T)
  static inline constexpr T gyromagnetic_ratio_in_MHz_T() {
    return gyromagnetic_ratio() / 1e6 / boost::math::constants::two_pi<T>();
  }
};

} // namespace nuclei

} // namespace nmr

} // namespace triumf

#endif // TRIUMF_NMR_NUCLEI_HPP


#include "sv/llol/match.h"

#include <Eigen/Geometry>  // inverse

namespace sv {

void PointMatch::ResetGrid() {
  px_g = {kBadPx, kBadPx};
  mc_g.Reset();
}

void PointMatch::ResetPano() {
  px_p = {kBadPx, kBadPx};
  mc_p.Reset();
}

void PointMatch::Reset() {
  ResetGrid();
  ResetPano();
  U.setZero();
  scale = 0.0;
}

void PointMatch::CalcSqrtInfo(float lambda) {
  auto cov = mc_p.Covar();
  if (lambda > 0) cov.diagonal().array() += lambda;
  U = MatrixSqrtUtU(cov.inverse().eval());
}

void PointMatch::CalcSqrtInfo(const Eigen::Matrix3f& R_p_g, float lambda) {
  auto cov = mc_p.Covar();
  cov.noalias() += R_p_g * mc_g.Covar() * R_p_g.transpose();
  if (lambda > 0) cov.diagonal().array() += lambda;
  U = MatrixSqrtUtU(cov.inverse().eval());
}

}  // namespace sv


#include <cctbx/boost_python/flex_fwd.h>
#include <boost/python/enum.hpp>
#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/dict.hpp>
#include <boost/python/list.hpp>
#include <scitbx/array_family/flex_types.h>
#include <scitbx/array_family/shared.h>
#include <scitbx/math/mean_and_variance.h>
#include <scitbx/array_family/boost_python/shared_wrapper.h>
#include <cctbx/examples/merging/xscale_prototype_core.h>
#include <cctbx/examples/merging/postrefine_base.h>
#include <Eigen/Sparse>
#include <boost/python/return_internal_reference.hpp>
#include <boost/math/tools/precision.hpp>

using namespace boost::python;
namespace cctbx{ namespace merging {

double d_product (double const&A, double const&B, double const&dA, double const&dB){
  return dA * B + A * dB;
}

static const double logmax = boost::math::tools::log_max_value<double>()/2.;
static const double logmin = boost::math::tools::log_min_value<double>()/2.;

static boost::python::tuple
task6c_f_g(intensity_data::vecd const& x,
           const int& N_I, const int& N_G,
           const intensity_data & FSIM,
           bool compute_curv) {
  double f = 0.;
  const double* Iptr = x.begin(); // indices into the array of parameters
  const double* Gptr = Iptr + N_I;
  const double* Bptr = Gptr + N_G;

  intensity_data::vecd g(x.size(),0);
  intensity_data::vecd c(x.size(),0);
  double* Iptrg = g.begin(); // indices into the array of derivatives
  double* Gptrg = Iptrg + N_I;
  double* Bptrg = Gptrg + N_G;
  double* Iptrc = c.begin(); // indices into the array of curvatures
  double* Gptrc = Iptrc + N_I;
  double* Bptrc = Gptrc + N_G;

  for (size_t i = 0; i<FSIM.raw_obs.size(); ++i){
      double weight = 1./FSIM.exp_var[i];
      double Gitem  = Gptr[ FSIM.frame[i] ];
      double Bargument = -2.* Bptr[ FSIM.frame[i] ] * FSIM.stol_sq[i];

      if (logmax < Bargument){
        throw SCITBX_ERROR("exp argument greater than logmax");
      }
      if (logmin > Bargument){
        throw SCITBX_ERROR("exp argument less than logmin");
      }

      double Bitem  = std::exp(Bargument); // :=exp(beta)
      double Iitem  = Iptr[ FSIM.miller[i] ];

      double residual = - FSIM.raw_obs[i] +
                        Gitem *
                        Bitem *
                        Iitem;

      f += weight * residual * residual;

      Gptrg[ FSIM.frame[i] ] += weight * residual * Bitem * Iitem;
      Iptrg[ FSIM.miller[i] ]+= weight * residual * Bitem * Gitem;

      double d_Bitem_d_B =    Bitem * ( -2. * FSIM.stol_sq[i] );
      double d_residual_d_B = Gitem * Iitem * d_Bitem_d_B;

      Bptrg[ FSIM.frame[i] ] += weight * residual * d_residual_d_B;

      if (compute_curv) {
        Gptrc[ FSIM.frame[i] ] +=  weight * Bitem * Bitem * Iitem * Iitem;
        Iptrc[ FSIM.miller[i] ]+=  weight * Bitem * Bitem * Gitem * Gitem;
        Bptrc[ FSIM.frame[i] ] +=  weight * d_residual_d_B * d_residual_d_B;
      }
  }

  return (boost::python::make_tuple(f/2., g, c));
}

void f_g_error(std::string const& message){
  throw SCITBX_ERROR(message);
}

namespace boost_python { namespace {

  void
  large_scale_merging_init_module() {
    using namespace boost::python;

    def("task6c_f_g", &cctbx::merging::task6c_f_g);
    def("f_g_error", &cctbx::merging::f_g_error);

    typedef return_value_policy<return_by_value> rbv;
    typedef default_call_policies dcp;

    typedef cctbx::merging::intensity_data cmid;
    class_<cmid>("intensity_data", init<>())
      .add_property("frame",
                    make_getter(&cmid::frame, rbv()),
                    make_setter(&cmid::frame, dcp()))
      .add_property("miller",
                    make_getter(&cmid::miller, rbv()),
                    make_setter(&cmid::miller, dcp()))
      .add_property("raw_obs",
                    make_getter(&cmid::raw_obs, rbv()),
                    make_setter(&cmid::raw_obs, dcp()))
      .add_property("exp_var",
                    make_getter(&cmid::exp_var, rbv()),
                    make_setter(&cmid::exp_var, dcp()))
      .add_property("stol_sq",
                    make_getter(&cmid::stol_sq, rbv()),
                    make_setter(&cmid::stol_sq, dcp()))
      .add_property("origHKL",
                    make_getter(&cmid::origHKL, rbv()),
                    make_setter(&cmid::origHKL, dcp()))
      .def("estimate_G", &cmid::estimate_G,(arg("Nframes"),arg("inv_d_sq_max")=0.,arg("inv_d_sq_min")=0.))
      .def("estimate_I", &cmid::estimate_I,(arg("Gframes"),arg("inv_d_sq_max")=0.,arg("inv_d_sq_min")=0.))
      .def("reset_mem", &cmid::reset_mem)
    ;

    enum_<ParameterFlags>("ParameterFlags")
      .value("PartialityDeff", PartialityDeff)
      .value("PartialityEtaDeff", PartialityEtaDeff)
      .value("Bfactor", Bfactor)
      .value("Deff", Deff)
      .value("Eta", Eta)
      .value("Rxy", Rxy)
    ;

    typedef scaling_common_functions scf;
    class_<scf >(
      "scaling_common_functions", no_init)
      .def("set_cpp_data",&scf::set_cpp_data,
        (arg("fsim")))
      .def("set_parameter_flags", &scf::set_parameter_flags)
      .def("set_wavelength", &scf::set_wavelength)
      .def("set_domain_size", &scf::set_domain_size)
      .def("set_Astar_matrix", &scf::set_Astar_matrix)
      .def("get_rh_rs_ratio", &scf::get_rh_rs_ratio)
    ;

    typedef scitbx::example::non_linear_ls_eigen_wrapper nllsew;
    typedef xscale6e wt6e;
    class_<wt6e,
           bases<nllsew, scf  > >(
      "xscale6e", no_init)
      .def(init<int>(arg("n_parameters")))
      .def("access_cpp_build_up_directly_eigen_eqn",&wt6e::access_cpp_build_up_directly_eigen_eqn,
        (arg("objective_only"),arg("current_values")))
      .def("reset_mem", &wt6e::reset_mem)
    ;

    typedef postrefine_base prb;
    class_<prb,
           bases<wt6e  > >(
      "postrefine_base", no_init)
      .def(init<int>(arg("n_parameters")))
      .def("access_cpp_build_up_directly_eigen_eqn",&prb::access_cpp_build_up_directly_eigen_eqn,
        (arg("objective_only"),arg("current_values")))
    ;

  }

}}
}} // namespace xfel::boost_python::<anonymous>

BOOST_PYTHON_MODULE(cctbx_large_scale_merging_ext)
{
  cctbx::merging::boost_python::large_scale_merging_init_module();

}


#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <vector>
#include <sstream>
#include <math.h>
#include "testing.h"

class Suspension
{

	// variables
private:
	// suffix ref stands for ref, so reference/initial coordinates can be easily
	// differentiated from coordinates of the same points during wheel movement
	// hardpoints
	// LCA
	Eigen::Vector3f lca1ref;
	Eigen::Vector3f lca2ref;
	Eigen::Vector3f lca3ref;

	Eigen::Vector3f uca1ref;
	Eigen::Vector3f uca2ref;
	Eigen::Vector3f uca3ref;

	Eigen::Vector3f tr1ref;
	Eigen::Vector3f tr2ref;

	Eigen::Vector3f wcnref;
	Eigen::Vector3f spnref;

	// wheel radius
	float wRadius=210;
	// wheel vertical movement
	float wVert=30;
	// wheel steering movement
	float wSteer=30;
	// number of increments between reference position and
	// downmost/upmost/leftmost/rightmost position
	int vertIncr=30;
	int steerIncr=0;
	// precision- at what value has the iterator converged, in percentage- 0...1
	float precision;

	// derived values

	Eigen::Vector3f lca12;
	Eigen::Vector3f uca12;

	/* initializers */

public:

	Suspension()
	{
		std::vector<float> hps;
		std::string userArrayInput;
		std::cout << "enter numbers: ";
		std::cin >> userArrayInput;

		std::istringstream iss(userArrayInput);
		std::string item;

		while (std::getline(iss, item, ','))
		{
			hps.push_back(std::stof(item));
		}

		std::cout << "for loop vector output" << std::endl;

		for (auto i = hps.begin(); i != hps.end(); i++)
			std::cout << *i << std::endl;

		lca1ref << hps[0], hps[1], hps[2];
		lca2ref << hps[3], hps[4], hps[5];
		lca3ref << hps[6], hps[7], hps[8];

		uca1ref << hps[9], hps[10], hps[11];
		uca2ref << hps[12], hps[13], hps[14];
		uca3ref << hps[15], hps[16], hps[17];

		tr1ref << hps[18], hps[19], hps[20];
		tr2ref << hps[21], hps[22], hps[23];

		wcnref << hps[24], hps[25], hps[26];
		spnref << hps[27], hps[28], hps[29];

	}

	Suspension(float lca1xin, float lca1yin, float lca1zin, // in suffix stands for input
		float lca2xin, float lca2yin, float lca2zin,
		float lca3xin, float lca3yin, float lca3zin,
		float uca1xin, float uca1yin, float uca1zin,
		float uca2xin, float uca2yin, float uca2zin,
		float uca3xin, float uca3yin, float uca3zin,
		float tr1xin, float tr1yin, float tr1zin,
		float tr2xin, float tr2yin, float tr2zin,
		float wcnxin, float wcnyin, float wcnzin,
		float spnxin, float spnyin, float spnzin,
		float wRadiusin, float wVertin, float wSteerin,
		int vertIncrin, int steerIncrin, float precisionin)
	{
		lca1ref << lca1xin, lca1yin, lca1zin;
		lca2ref << lca2xin, lca2yin, lca2zin;
		lca3ref << lca3xin, lca3yin, lca3zin;

		uca1ref << uca1xin, uca1yin, uca1zin;
		uca2ref << uca2xin, uca2yin, uca2zin;
		uca3ref << uca3xin, uca3yin, uca3zin;

		tr1ref << tr1xin, tr1yin, tr1zin;
		tr2ref << tr2xin, tr2yin, tr2zin;

		wcnref << wcnxin, wcnyin, wcnzin;
		spnref << spnxin, spnyin, spnzin;

		wRadius = wRadiusin;
		wVert = wVertin;
		wSteer = wSteerin;
		vertIncr = vertIncrin;
		steerIncr = steerIncrin;
		precision = precisionin;

	}

	// FUNCTIONS
private:

	// place as inputs variables rLCA, rUCA, uca12, lca12, etc. so those can be only temporary
	// values and not use up stack memory

		void CalculateConstants(Eigen::Matrix3f& _rotLCA, Eigen::Matrix3f& _rotUCA, float& _rLCA, float& _rUCA, float& _rCA, Eigen::ArrayXf& _zLocLca, float& _rST, float& _t_param, float& _rTR, Eigen::Vector3f& _wcnlocTRk, Eigen::Vector3f& _spnlocTRk)
	{

		// create line connecting lca1 and lca2
		Eigen::ParametrizedLine<float, 3> lca1lca2 = Eigen::ParametrizedLine<float, 3>::Through(lca1ref, lca2ref);

		// local LCA plane for determining max z value of wheel parameters
		Eigen::Vector4f abcd;
		Eigen::Vector3f _tr2prref;

		// z local maximum value
		float zLocHi;
		// z local minimum value
		float zLocLo;

		lca12 = lca1lca2.projection(lca3ref);
		_rLCA = lca1lca2.distance(lca3ref);

		// create rotation matrix for LCA cs
		_rotLCA <<
			(lca1ref - lca2ref).normalized(),
			(lca12 - lca3ref).normalized(),
			((lca1ref - lca2ref).cross(lca12 - lca3ref)).normalized();

		// calculate parameters for plane in LCA cs
		abcd <<
			_rotLCA.row(2)(0),
			_rotLCA.row(2)(1),
			_rotLCA.row(2)(2),
			-_rotLCA.row(2) * (_rotLCA.transpose() * Eigen::Vector3f{ -lca12(0),-lca12(1),lca3ref(2) - wVert - lca12(2) });

		// calculates z value for upmost movement of wheel for intersection of plane and circle in LCA
		zLocHi =
			(-abcd(2) * abcd(3) + abcd(1) *
			sqrt(abcd(2) * abcd(2) * _rLCA * _rLCA +
				abcd(1) * abcd(1) * _rLCA * _rLCA -
				abcd(3) * abcd(3))) /
			(abcd(1) * abcd(1) + abcd(2) * abcd(2));

		// reuses previous parameters for LCA plane, only 4th parameter is changed
		abcd(3) =
			-_rotLCA.row(2) *
			_rotLCA.transpose() *
			Eigen::Vector3f{ -lca12(0),-lca12(1),lca3ref(2) + wVert - lca12(2) };

		// calculates z value for downmost movement of wheel for intersection of plane and circle in LCA
		zLocLo =
			(-abcd(2) * abcd(3) +
			abcd(1) * sqrt(abcd(2) * abcd(2) * _rLCA * _rLCA +
				abcd(1) * abcd(1) * _rLCA * _rLCA -
				abcd(3) * abcd(3))) /
			(abcd(1) * abcd(1) + abcd(2) * abcd(2));

		_zLocLca <<
			Eigen::VectorXf::LinSpaced(vertIncr, zLocLo, zLocLo / vertIncr),
			0,
			Eigen::VectorXf::LinSpaced(vertIncr, zLocHi / vertIncr, zLocHi);

		// create line connecting uca1 and uca2
		Eigen::ParametrizedLine<float, 3> uca1uca2 = Eigen::ParametrizedLine<float, 3>::Through(uca1ref, uca2ref);

		uca12 = uca1uca2.projection(uca3ref);
		_rUCA = uca1uca2.distance(uca3ref);

		_rCA = (uca3ref - lca3ref).norm();

		// create rotation matrix for UCA cs
		_rotUCA <<
			(uca1ref - uca2ref).normalized(),
			(uca12 - uca3ref).normalized(),
			((uca1ref - uca2ref).cross(uca12 - uca3ref)).normalized();

		// calculate TR2 projection point and rTR
		Eigen::ParametrizedLine<float, 3> lca3uca3 = Eigen::ParametrizedLine<float, 3>::Through(lca3ref, uca3ref);

		_tr2prref = lca3uca3.projection(tr2ref);

		_rST = (tr2ref - _tr2prref).norm();

		_rTR = (tr2ref - tr1ref).norm();

		_t_param = (_tr2prref(0) - lca3ref(0)) / (uca3ref(0) - lca3ref(0));

		Eigen::Matrix3f _rotTRk; // TR rotation matrix defined by TR2ref

		Eigen::Vector3f _xCol{ _tr2prref - tr2ref };
		Eigen::Vector3f _zCol{ lca3ref - uca3ref };
		Eigen::Vector3f _yCol{ _zCol.cross(_xCol) };

		_rotTRk.col(0) << _xCol / _xCol.norm();
		_rotTRk.col(1) << _yCol / _yCol.norm();
		_rotTRk.col(2) << _zCol / _zCol.norm();

		_wcnlocTRk << _rotTRk.transpose() * (wcnref - _tr2prref);
		_spnlocTRk << _rotTRk.transpose() * (spnref - _tr2prref);

		std::cout << "spnlocTRk" << std::endl;
		std::cout << _spnlocTRk << std::endl;
	}

public:

	int CalculateMovement()
	{
		Eigen::Matrix3f rotLCA;
		Eigen::Matrix3f rotUCA;
		Eigen::MatrixXf tr2prGlob;
		float rLCA;
		float rUCA;
		float rCA;   // distance between LCA3 and UCA3
		float rST;   // distance between TR2 and UCA3LCA3 axis
		float rTR;   // distance between TR2 and TR1 axis
		float t_param;   // parameter to determine position of TR2pr on uca3lca3 line
		// coordinates of wcn and spn in TR cs defined by tr2ref
		Eigen::Vector3f wcnlocTRk;
		Eigen::Vector3f spnlocTRk;

		Eigen::ArrayXf zLCA3LocLCA(vertIncr * 2 + 1);

		Suspension::CalculateConstants(rotLCA, rotUCA, rLCA, rUCA, rCA, zLCA3LocLCA, rST, t_param, rTR, wcnlocTRk, spnlocTRk);

		Eigen::MatrixXf lca3LocLCA(vertIncr * 2 + 1, 3);
		Eigen::MatrixXf lca3Glob(vertIncr * 2 + 1, 3);

		// populating positions of local LCA3 in a matrix
		lca3LocLCA.col(0) << Eigen::VectorXf::Zero(vertIncr * 2 + 1);
		lca3LocLCA.col(1) << -(rLCA*rLCA - zLCA3LocLCA * zLCA3LocLCA).sqrt();
		lca3LocLCA.col(2) << zLCA3LocLCA;

		// global positions of LCA3 for whole wheel movement
		lca3Glob = (lca3LocLCA * rotLCA.transpose()).array().rowwise() + lca12.array().transpose();

		// global position of UCA3 for whole wheel movement
		Eigen::MatrixXf uca3Glob(vertIncr * 2 + 1, 3);
		Eigen::MatrixXf uca3LocUCA(vertIncr * 2 + 1, 3);
		Eigen::MatrixXf lca3LocUCA(vertIncr * 2 + 1, 3);

		lca3LocUCA = lca3Glob.rowwise() - uca12.transpose();
		lca3LocUCA = lca3LocUCA * rotUCA;

		// temporary values for calculating UCA3 in UCA cs, correspond to chunks of expression in word
		Eigen::ArrayXf temp1UCA3 =
			-rCA * rCA + rUCA * rUCA +
			lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array() +
			lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() +
			lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array();

		Eigen::ArrayXf temp2UCA3 =
			2 * (lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() +
			lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array());

		Eigen::ArrayXf temp3UCA3 =
			-rCA * rCA * rCA * rCA + 2 * rCA * rCA * rUCA * rUCA +
			2 * rCA * rCA * lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array() +
			2 * rCA * rCA * lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() +
			2 * rCA * rCA * lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array();

		Eigen::ArrayXf temp4UCA3 =
			-rUCA * rUCA * rUCA * rUCA -
			2 * rUCA * rUCA * lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array() +
			2 * rUCA * rUCA * lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() +
			2 * rUCA * rUCA * lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array();

		Eigen::ArrayXf temp5UCA3 =
			-lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array()* lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array() -
			2 * lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array() * lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() -
			2 * lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array() * lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array();

		Eigen::ArrayXf temp6UCA3 =
			-lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array()* lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() -
			2 * lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() * lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array() -
			lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array();

		Eigen::ArrayXf temp7UCA3 = (temp3UCA3 + temp4UCA3 + temp5UCA3 + temp6UCA3).sqrt();

		uca3LocUCA.col(0) << Eigen::VectorXf::Zero(vertIncr * 2 + 1);
		uca3LocUCA.col(1) << (lca3LocUCA.col(1).array() * temp1UCA3 - lca3LocUCA.col(2).array() * temp7UCA3) / temp2UCA3;
		uca3LocUCA.col(2) << (lca3LocUCA.col(2).array() * temp1UCA3 + lca3LocUCA.col(1).array() * temp7UCA3) / temp2UCA3;

		uca3Glob = (uca3LocUCA * rotUCA.transpose()).array().rowwise() + uca12.array().transpose();

		// calculating TR2, WCN, SPN and CP
		// steering is not enabled, for optimization
		if (steerIncr!=0 || wSteer!=0)
		{

			std::cout << "no steer movement" << std::endl;
			// calculating TR2 position
			Eigen::MatrixXf tr2Glob(vertIncr * 2 + 1, 3);
			Eigen::MatrixXf wcnGlob(vertIncr * 2 + 1, 3);
			Eigen::MatrixXf spnGlob(vertIncr * 2 + 1, 3);

			tr2prGlob = (uca3Glob - lca3Glob) * t_param + lca3Glob;

			for (int i = 0; i < vertIncr * 2 + 1; i++)
			{
				Eigen::Matrix3f rotTR;
				Eigen::Vector3f tr2locTR;

				Eigen::Vector3f xCol{ 30, 0, -30 * (uca3Glob(i, 0) - lca3Glob(i, 0)) / (uca3Glob(i, 2) - lca3Glob(i, 2)) };
				Eigen::Vector3f zCol{ (lca3Glob.row(i) - uca3Glob.row(i)).transpose() };
				Eigen::Vector3f yCol{ zCol.cross(xCol) };

				rotTR.col(0) << xCol / xCol.norm();
				rotTR.col(1) << yCol / yCol.norm();
				rotTR.col(2) << zCol / zCol.norm();

				// calculate local position of TR1
				Eigen::Vector3f tr1locTR;

				//lca3LocUCA = lca3Glob.rowwise() - uca12.transpose();
				//lca3LocUCA = lca3LocUCA * rotUCA;

				tr1locTR = -tr2prGlob.row(i) + tr1ref.transpose();
				tr1locTR = rotTR.transpose() * tr1locTR;

				// calculate local position of TR2
				float temp1TR2 =
					rST * rST - rTR * rTR +
					tr1locTR(0) * tr1locTR(0) +
					tr1locTR(1) * tr1locTR(1) +
					tr1locTR(2) * tr1locTR(2);

				float temp2TR2 = 2 * (tr1locTR(0) * tr1locTR(0) + tr1locTR(1) * tr1locTR(1));

				float temp3TR2 =
					-rST * rST * rST * rST +
					2 * rTR * rTR * rST * rST +
					2 * rST * rST * tr1locTR(0) * tr1locTR(0) +
					2 * rST * rST * tr1locTR(1) * tr1locTR(1) -
					2 * rST * rST * tr1locTR(2) * tr1locTR(2);

				float temp4TR2 =
					-rTR * rTR * rTR * rTR +
					2 * rTR * rTR * tr1locTR(0) * tr1locTR(0) +
					2 * rTR * rTR * tr1locTR(1) * tr1locTR(1) +
					2 * rTR * rTR * tr1locTR(2) * tr1locTR(2);

				float temp5TR2 =
					-tr1locTR(0) * tr1locTR(0) * tr1locTR(0) * tr1locTR(0) -
					2 * tr1locTR(0) * tr1locTR(0) * tr1locTR(1) * tr1locTR(1) -
					2 * tr1locTR(0) * tr1locTR(0) * tr1locTR(2) * tr1locTR(2);

				float temp6TR2 =
					-2 * tr1locTR(1) * tr1locTR(1) * tr1locTR(2) * tr1locTR(2) -
					tr1locTR(1) * tr1locTR(1) * tr1locTR(1) * tr1locTR(1) -
					tr1locTR(2) * tr1locTR(2) * tr1locTR(2) * tr1locTR(2);

				float temp7TR2 = std::sqrt(temp3TR2 + temp4TR2 + temp5TR2 + temp6TR2);

				tr2locTR(0) = (tr1locTR(0) * temp1TR2 - tr1locTR(1) * temp7TR2) / temp2TR2;
				tr2locTR(1) = (tr1locTR(1) * temp1TR2 + tr1locTR(0) * temp7TR2) / temp2TR2;
				tr2locTR(2) = 0;

				tr2Glob.row(i) << (rotTR * tr2locTR).transpose() + tr2prGlob.row(i);

				// calculating WCN and SPN
				Eigen::Matrix3f _rotTRk; // TR rotation matrix defined by TR2ref

				Eigen::Vector3f _xCol{ tr2prGlob.row(i) - tr2Glob.row(i) };
				Eigen::Vector3f _zCol{ lca3Glob.row(i) - uca3Glob.row(i) };
				Eigen::Vector3f _yCol{ _zCol.cross(_xCol) };

				_rotTRk.col(0) << _xCol / _xCol.norm();
				_rotTRk.col(1) << _yCol / _yCol.norm();
				_rotTRk.col(2) << _zCol / _zCol.norm();

				wcnGlob.row(i) << (_rotTRk * wcnlocTRk).transpose() + tr2prGlob.row(i);
				spnGlob.row(i) << (_rotTRk * spnlocTRk).transpose() + tr2prGlob.row(i);

			}

			// CP calculation
			float temp1cp{ -20 }; // this is actually vector 0,0,-20
			Eigen::MatrixXf temp2cp(vertIncr * 2 + 1, 3);
			Eigen::MatrixXf temp3cp(vertIncr * 2 + 1, 3);

			temp2cp << spnGlob - wcnGlob;

			std::cout << "temp2cp  " << std::endl;
			std::cout << temp2cp << std::endl;

			temp3cp.col(0) << -temp2cp.col(0).array() * temp2cp.col(2).array() * temp1cp;
			temp3cp.col(1) << -temp2cp.col(1).array() * temp2cp.col(2).array() * temp1cp;
			temp3cp.col(2) <<
				temp2cp.col(1).array() * temp2cp.col(1).array() * temp1cp +
				temp2cp.col(0).array() * temp2cp.col(0).array() * temp1cp;

			temp3cp.rowwise().normalize();

			Eigen::MatrixXf cpGlob(vertIncr * 2 + 1, 3);

			cpGlob << -temp3cp * wRadius + wcnGlob;

			std::cout << "cp  " << std::endl;
			std::cout << cpGlob << std::endl;
			return 123;

		}

		// steering is enabled
		else
		{
		return 456;
		}

	}

};

int main()
{
	Suspension susp{
		-2038.666, -411.709, -132.316,
		-2241.147, -408.195, -126.205,
		-2135, -600, -140,
		-2040.563, -416.249, -275.203,
		-2241.481, -417.314, -270.739,
		-2153, -578, -315,
		-2234.8, -411.45, -194.6,
		-2225, -582, -220,
		-2143.6, -620.5, -220.07,
		-2143.6, -595.5, -219.34,
		210, 30, 30, 1,
		10, 0.01
	};

	int i = susp.CalculateMovement();
	std::cout << i << std::endl;

	std::cout << addition(15,20) << std::endl;

	Suspension susp2{};

	susp2.CalculateMovement();

	/*
	-2038.666,-411.709,-132.316,-2241.147,-408.195,-126.205,-2135,-600,-140,-2040.563,-416.249,-275.203,-2241.481,-417.314,-270.739,-2153,-578,-315,-2234.8,-411.45,-194.6,-2225,-582,-220,-2143.6,-620.5,-220.07,-2143.6,-595.5,-219.34

	*/

}


#include <cmath>
#include <functional>
#include <unordered_map>
#include <unordered_set>
#include <vector>

#include <boost/functional/hash.hpp>
#include <boost/math/constants/constants.hpp>
#include <CGAL/boost/graph/helpers.h>
#include <CGAL/boost/graph/Dual.h>
#include <Euclid/Math/Vector.h>
#include <Euclid/Util/Assert.h>

namespace Euclid
{

template<typename Mesh>
Vector_3_t<Mesh> vertex_normal(vertex_t<Mesh> v,
                               const Mesh& mesh,
                               const VertexNormal& weight)
{
    Vector_3_t<Mesh> normal(0.0, 0.0, 0.0);
    for (auto he : CGAL::halfedges_around_source(v, mesh)) {
        if (!CGAL::is_border(he, mesh)) {
            auto f = face(he, mesh);
            auto fn = face_normal(f, mesh);

            if (weight == VertexNormal::uniform) {
                normal += fn;
            }
            else if (weight == VertexNormal::face_area) {
                auto area = face_area(f, mesh);
                normal += area * fn;
            }
            else { // incident_angle
                auto angle = corner_angle(he, mesh);
                normal += angle * fn;
            }
        }
    }
    return Euclid::normalized(normal);
}

template<typename Mesh>
Vector_3_t<Mesh> vertex_normal(
    vertex_t<Mesh> v,
    const Mesh& mesh,
    const std::vector<Vector_3_t<Mesh>>& face_normals,
    const VertexNormal& weight)
{
    auto fimap = get(boost::face_index, mesh);
    Vector_3_t<Mesh> normal(0.0, 0.0, 0.0);
    for (auto he : CGAL::halfedges_around_source(v, mesh)) {
        if (!CGAL::is_border(he, mesh)) {
            auto f = face(he, mesh);
            auto fi = get(fimap, f);
            auto fn = face_normals[fi];

            if (weight == VertexNormal::uniform) {
                normal += fn;
            }
            else if (weight == VertexNormal::face_area) {
                auto area = face_area(f, mesh);
                normal += area * fn;
            }
            else { // incident_angle
                auto angle = corner_angle(he, mesh);
                normal += angle * fn;
            }
        }
    }
    return Euclid::normalized(normal);
}

template<typename Mesh>
std::vector<Vector_3_t<Mesh>> vertex_normals(
    const Mesh& mesh,
    const std::vector<Vector_3_t<Mesh>>& face_normals,
    const VertexNormal& weight)
{
    std::vector<Vector_3_t<Mesh>> vnormals;
    vnormals.reserve(num_vertices(mesh));
    for (auto v : vertices(mesh)) {
        vnormals.push_back(vertex_normal(v, mesh, face_normals, weight));
    }
    return vnormals;
}

template<typename Mesh>
FT_t<Mesh> vertex_area(vertex_t<Mesh> v,
                       const Mesh& mesh,
                       const VertexArea& method)
{
    using T = FT_t<Mesh>;
    auto vpmap = get(boost::vertex_point, mesh);
    auto va = T(0);
    if (method == VertexArea::barycentric) {
        const auto one_third = boost::math::constants::third<T>();
        for (auto he : CGAL::halfedges_around_target(v, mesh)) {
            if (!CGAL::is_border(he, mesh)) {
                auto p1 = get(vpmap, source(he, mesh));
                auto p2 = get(vpmap, target(he, mesh));
                auto p3 = get(vpmap, target(next(he, mesh), mesh));
                va += area(p1, p2, p3);
            }
        }
        va *= one_third;
    }
    else if (method == VertexArea::voronoi) {
        for (auto he : CGAL::halfedges_around_target(v, mesh)) {
            if (!CGAL::is_border(he, mesh)) {
                auto p1 = get(vpmap, source(he, mesh));
                auto p2 = get(vpmap, target(he, mesh));
                auto p3 = get(vpmap, target(next(he, mesh), mesh));
                auto mid1 = CGAL::midpoint(p2, p1);
                auto mid2 = CGAL::midpoint(p2, p3);
                auto center = CGAL::circumcenter(p1, p2, p3);
                if (CGAL::angle(p2, p1, p3) == CGAL::OBTUSE) {
                    va += area(mid1, p2, center) - area(mid2, center, p2);
                }
                else if (CGAL::angle(p2, p3, p1) == CGAL::OBTUSE) {
                    va += area(mid2, center, p2) - area(mid1, p2, center);
                }
                else {
                    va += area(mid1, p2, center) + area(mid2, center, p2);
                }
            }
        }
    }
    else { // method == VertexArea::mixed_voronoi
        for (auto he : CGAL::halfedges_around_target(v, mesh)) {
            if (!CGAL::is_border(he, mesh)) {
                auto p1 = get(vpmap, source(he, mesh));
                auto p2 = get(vpmap, target(he, mesh));
                auto p3 = get(vpmap, target(next(he, mesh), mesh));
                if (CGAL::angle(p1, p2, p3) == CGAL::OBTUSE) {
                    va += area(p1, p2, p3) * 0.5;
                }
                else if (CGAL::angle(p3, p1, p2) == CGAL::OBTUSE ||
                         CGAL::angle(p1, p3, p2) == CGAL::OBTUSE) {
                    va += area(p1, p2, p3) * 0.25;
                }
                else { // triangle is acute or right
                    auto mid1 = CGAL::midpoint(p2, p1);
                    auto mid2 = CGAL::midpoint(p2, p3);
                    auto center = CGAL::circumcenter(p1, p2, p3);
                    va += area(mid1, p2, center) + area(mid2, center, p2);
                }
            }
        }
    }
    return va;
}

template<typename Mesh>
std::vector<FT_t<Mesh>> vertex_areas(const Mesh& mesh, const VertexArea& method)
{
    std::vector<FT_t<Mesh>> vareas;
    vareas.reserve(num_vertices(mesh));
    for (auto v : vertices(mesh)) {
        vareas.push_back(vertex_area(v, mesh, method));
    }
    return vareas;
}

// partial specialization for dual mesh
template<typename Mesh>
FT_t<Mesh> edge_length(halfedge_t<Mesh> h, const CGAL::Dual<Mesh>& dual)
{
    auto primal = dual.primal();
    auto f0 = source(h, dual);
    auto f1 = target(h, dual);
    auto p0 = barycenter(f0, primal);
    auto p1 = barycenter(f1, primal);
    return length(p1 - p0);
}

template<typename Mesh>
FT_t<Mesh> edge_length(halfedge_t<Mesh> he, const Mesh& mesh)
{
    auto vpmap = get(boost::vertex_point, mesh);
    auto p1 = get(vpmap, source(he, mesh));
    auto p2 = get(vpmap, target(he, mesh));
    return length(p2 - p1);
}

template<typename Mesh>
FT_t<Mesh> edge_length(edge_t<Mesh> e, const Mesh& mesh)
{
    auto he = halfedge(e, mesh);
    return edge_length(he, mesh);
}

template<typename Mesh>
std::vector<FT_t<Mesh>> edge_lengths(const Mesh& mesh)
{
    std::vector<FT_t<Mesh>> elens;
    elens.reserve(num_edges(mesh));
    for (auto he : edges(mesh)) {
        elens.push_back(edge_length(he, mesh));
    }
    return elens;
}

// partial specialization for dual mesh
template<typename Mesh>
FT_t<Mesh> squared_edge_length(edge_t<Mesh> e, const CGAL::Dual<Mesh>& dual)
{
    auto primal = dual.primal();
    auto h = halfedge(e, dual);
    auto f0 = source(h, dual);
    auto f1 = target(h, dual);
    auto p0 = barycenter(f0, primal);
    auto p1 = barycenter(f1, primal);
    return (p1 - p0).squared_length();
}

template<typename Mesh>
FT_t<Mesh> squared_edge_length(halfedge_t<Mesh> he, const Mesh& mesh)
{
    auto vpmap = get(boost::vertex_point, mesh);
    auto p1 = get(vpmap, source(he, mesh));
    auto p2 = get(vpmap, target(he, mesh));
    return (p1 - p2).squared_length();
}

template<typename Mesh>
FT_t<Mesh> squared_edge_length(edge_t<Mesh> e, const Mesh& mesh)
{
    auto he = halfedge(e, mesh);
    return squared_edge_length(he, mesh);
}

template<typename Mesh>
std::vector<FT_t<Mesh>> squared_edge_lengths(const Mesh& mesh)
{
    std::vector<FT_t<Mesh>> elens;
    elens.reserve(num_edges(mesh));
    for (auto he : edges(mesh)) {
        elens.push_back(squared_edge_length(he, mesh));
    }
    return elens;
}

template<typename Mesh>
FT_t<Mesh> dihedral_angle(edge_t<Mesh> e, const Mesh& mesh)
{
    auto h1 = halfedge(e, mesh);
    auto h2 = opposite(h1, mesh);
    auto f1 = face(h1, mesh);
    auto f2 = face(h2, mesh);
    auto n1 = face_normal(f1, mesh);
    auto n2 = face_normal(f2, mesh);
    return boost::math::constants::pi<FT_t<Mesh>>() - std::acos(cosine(n1, n2));
}

template<typename Mesh>
FT_t<Mesh> corner_angle(halfedge_t<Mesh> h, const Mesh& mesh)
{
    auto vpmap = get(boost::vertex_point, mesh);
    auto p1 = get(vpmap, source(h, mesh));
    auto p2 = get(vpmap, target(h, mesh));
    auto p3 = get(vpmap, target(next(h, mesh), mesh));
    return CGAL::approximate_angle(p1, p2, p3);
}

template<typename Mesh>
Vector_3_t<Mesh> face_normal(face_t<Mesh> f, const Mesh& mesh)
{
    using Vector_3 = Vector_3_t<Mesh>;
    auto vpmap = get(boost::vertex_point, mesh);
    auto he = halfedge(f, mesh);
    auto p1 = get(vpmap, source(he, mesh));
    auto p2 = get(vpmap, target(he, mesh));
    auto p3 = get(vpmap, target(next(he, mesh), mesh));

    Vector_3 result;
    if (CGAL::collinear(p1, p2, p3)) {
        EWARNING("Degenerate face, normal is set to zero vector");
        result = Vector_3(0.0, 0.0, 0.0);
    }
    else {
        result = normalized(CGAL::normal(p1, p2, p3));
    }
    return result;
}

template<typename Mesh>
std::vector<Vector_3_t<Mesh>> face_normals(const Mesh& mesh)
{
    std::vector<Vector_3_t<Mesh>> fnormals;
    fnormals.reserve(num_faces(mesh));
    for (auto f : faces(mesh)) {
        fnormals.push_back(face_normal(f, mesh));
    }
    return fnormals;
}

template<typename Mesh>
FT_t<Mesh> face_area(face_t<Mesh> f, const Mesh& mesh)
{
    auto vpmap = get(boost::vertex_point, mesh);
    auto he = halfedge(f, mesh);
    auto p1 = get(vpmap, source(he, mesh));
    auto p2 = get(vpmap, target(he, mesh));
    auto p3 = get(vpmap, target(next(he, mesh), mesh));
    return area(p1, p2, p3);
}

template<typename Mesh>
std::vector<FT_t<Mesh>> face_areas(const Mesh& mesh)
{
    std::vector<FT_t<Mesh>> fareas;
    fareas.reserve(num_faces(mesh));
    for (auto f : faces(mesh)) {
        fareas.push_back(face_area(f, mesh));
    }
    return fareas;
}

template<typename Mesh>
Point_3_t<Mesh> barycenter(face_t<Mesh> f, const Mesh& mesh)
{
    auto vpmap = get(boost::vertex_point, mesh);
    auto [vbeg, vend] = CGAL::vertices_around_face(halfedge(f, mesh), mesh);
    auto p0 = get(vpmap, *vbeg++);
    auto p1 = get(vpmap, *vbeg++);
    auto p2 = get(vpmap, *vbeg);
    return CGAL::centroid(p0, p1, p2);
}

template<typename Mesh>
std::vector<Point_3_t<Mesh>> barycenters(const Mesh& mesh)
{
    std::vector<Point_3_t<Mesh>> centroids;
    centroids.reserve(num_faces(mesh));
    for (auto f : faces(mesh)) {
        centroids.push_back(barycenter(f, mesh));
    }
    return centroids;
}

template<typename Mesh>
FT_t<Mesh> gaussian_curvature(vertex_t<Mesh> v, const Mesh& mesh)
{
    auto angle_defect = boost::math::constants::two_pi<FT_t<Mesh>>();
    for (auto he : CGAL::halfedges_around_target(v, mesh)) {
        if (!CGAL::is_border(he, mesh)) {
            angle_defect -= corner_angle(he, mesh);
        }
    }
    return angle_defect / vertex_area(v, mesh);
}

template<typename Mesh>
std::vector<FT_t<Mesh>> gaussian_curvatures(const Mesh& mesh)
{
    std::vector<FT_t<Mesh>> curvatures;
    curvatures.reserve(num_vertices(mesh));
    for (auto v : vertices(mesh)) {
        curvatures.push_back(gaussian_curvature(v, mesh));
    }
    return curvatures;
}

template<typename Mesh>
std::tuple<Eigen::SparseMatrix<FT_t<Mesh>>, Eigen::SparseMatrix<FT_t<Mesh>>>
adjacency_matrix(const Mesh& mesh)
{
    using T = FT_t<Mesh>;
    using Triplet = Eigen::Triplet<T>;
    auto vimap = get(boost::vertex_index, mesh);
    const auto nv = num_vertices(mesh);

    std::vector<Triplet> adj, degree;
    for (auto vi : vertices(mesh)) {
        int i = get(vimap, vi);
        int d = 0;
        for (auto he : CGAL::halfedges_around_target(vi, mesh)) {
            auto vj = source(he, mesh);
            int j = get(vimap, vj);
            adj.emplace_back(i, j, 1);
            ++d;
        }
        degree.emplace_back(i, i, d);
    }

    Eigen::SparseMatrix<T> adj_mat(nv, nv), degree_mat(nv, nv);
    adj_mat.setFromTriplets(adj.begin(), adj.end());
    adj_mat.makeCompressed();
    degree_mat.setFromTriplets(degree.begin(), degree.end());
    degree_mat.makeCompressed();
    return std::make_tuple(adj_mat, degree_mat);
}

template<typename Mesh>
Eigen::SparseMatrix<FT_t<Mesh>> graph_laplacian_matrix(const Mesh& mesh)
{
    using T = FT_t<Mesh>;
    using Triplet = Eigen::Triplet<T>;
    auto vimap = get(boost::vertex_index, mesh);
    const auto nv = num_vertices(mesh);

    std::vector<Triplet> triplets;
    for (auto vi : vertices(mesh)) {
        int i = get(vimap, vi);
        int d = 0;
        for (auto he : CGAL::halfedges_around_target(vi, mesh)) {
            auto vj = source(he, mesh);
            int j = get(vimap, vj);
            triplets.emplace_back(i, j, -1);
            ++d;
        }
        triplets.emplace_back(i, i, d);
    }

    Eigen::SparseMatrix<T> L(nv, nv);
    L.setFromTriplets(triplets.begin(), triplets.end());
    L.makeCompressed();
    return L;
}

template<typename Mesh>
FT_t<Mesh> cotangent_weight(halfedge_t<Mesh> he, const Mesh& mesh)
{
    auto vpmap = get(boost::vertex_point, mesh);
    auto vi = source(he, mesh);
    auto vj = target(he, mesh);
    auto va = target(next(he, mesh), mesh);
    auto vb = target(next(opposite(he, mesh), mesh), mesh);
    auto pi = get(vpmap, vi);
    auto pj = get(vpmap, vj);
    auto pa = get(vpmap, va);
    auto pb = get(vpmap, vb);
    auto cota = cotangent(pi, pa, pj);
    auto cotb = cotangent(pi, pb, pj);
    return (cota + cotb) * static_cast<FT_t<Mesh>>(0.5);
}

template<typename Mesh>
FT_t<Mesh> cotangent_weight(edge_t<Mesh> e, const Mesh& mesh)
{
    auto he = halfedge(e, mesh);
    return cotangent_weight(he, mesh);
}

template<typename Mesh>
Eigen::SparseMatrix<FT_t<Mesh>> cotangent_matrix(const Mesh& mesh)
{
    using T = FT_t<Mesh>;
    using Triplet = Eigen::Triplet<T>;
    auto vimap = get(boost::vertex_index, mesh);
    const auto nv = num_vertices(mesh);

    auto hash_fcn = [](const Triplet& t) {
        size_t seed = 0;
        boost::hash_combine(seed, t.col());
        boost::hash_combine(seed, t.row());
        return seed;
    };
    auto eq_fcn = [](const Triplet& t1, const Triplet& t2) {
        return (t1.col() == t2.col()) && (t1.row() == t2.row());
    };
    std::unordered_set<Triplet, decltype(hash_fcn), decltype(eq_fcn)> values(
        nv, hash_fcn, eq_fcn);

    for (auto vi : vertices(mesh)) {
        int i = get(vimap, vi);
        T row_sum = 0.0;
        for (auto he : CGAL::halfedges_around_target(vi, mesh)) {
            auto vj = source(he, mesh);
            int j = get(vimap, vj);
            auto existing = values.find(Triplet(j, i, 0.0));
            if (existing != values.end()) {
                values.emplace(i, j, existing->value());
                row_sum -= existing->value();
            }
            else {
                auto value = cotangent_weight(he, mesh);
                values.emplace(i, j, -value);
                row_sum += value;
            }
        }
        values.emplace(i, i, row_sum);
    }

    Eigen::SparseMatrix<T> mat(nv, nv);
    mat.setFromTriplets(values.begin(), values.end());
    mat.makeCompressed();
    return mat;
}

template<typename Mesh>
Eigen::SparseMatrix<FT_t<Mesh>> mass_matrix(const Mesh& mesh,
                                            const VertexArea& method)
{
    using T = FT_t<Mesh>;
    const auto nv = num_vertices(mesh);
    Eigen::SparseMatrix<T> mass(nv, nv);
    std::vector<Eigen::Triplet<T>> values;

    int i = 0;
    for (auto v : vertices(mesh)) {
        auto area = vertex_area(v, mesh, method);
        values.emplace_back(i, i, area);
        ++i;
    }

    mass.setFromTriplets(values.begin(), values.end());
    mass.makeCompressed();
    return mass;
}

} // namespace Euclid


#include "gru_cell_old.hpp"
#include "generic/activity.hpp"
#include "generic/utils.hpp"
#include <Eigen/SVD>

namespace rnn {
gru_cell_old::gru_cell_old(const int inputDim, const int hiddenDim) {
  this->Wxr = MatD(hiddenDim, inputDim);
  this->Whr = MatD(hiddenDim, hiddenDim);
  this->br = VecD::Zero(hiddenDim);

  this->Wxz = MatD(hiddenDim, inputDim);
  this->Whz = MatD(hiddenDim, hiddenDim);
  this->bz = VecD::Zero(hiddenDim);

  this->Wxu = MatD(hiddenDim, inputDim);
  this->Whu = MatD(hiddenDim, hiddenDim);
  this->bu = VecD::Zero(hiddenDim);
}

void gru_cell_old::init(rnn::generic::rand &rnd, const real scale) {
  rnd.uniform(this->Wxr, scale);
  rnd.uniform(this->Whr, scale);

  rnd.uniform(this->Wxz, scale);
  rnd.uniform(this->Whz, scale);

  rnd.uniform(this->Wxu, scale);
  rnd.uniform(this->Whu, scale);

  this->Whr = Eigen::JacobiSVD<MatD>(this->Whr, Eigen::ComputeFullV |
      Eigen::ComputeFullU).matrixU();
  this->Whz = Eigen::JacobiSVD<MatD>(this->Whz, Eigen::ComputeFullV |
      Eigen::ComputeFullU).matrixU();
  this->Whu = Eigen::JacobiSVD<MatD>(this->Whu, Eigen::ComputeFullV |
      Eigen::ComputeFullU).matrixU();
}

gru_cell_old::State gru_cell_old::forward(const VecD &xt, const gru_cell_old::State &prev) {
//U=Wx, W=Wh, h^=u
  gru_cell_old::State cur;
  cur.r = br + Wxr * xt + Whr * prev.h;
  cur.z = bz + Wxz * xt + Whz * prev.h;

  activity::logistic(cur.r);
  activity::logistic(cur.z);

  cur.rh = cur.r.array() * prev.h.array();
  cur.u = bu + Wxu * xt + Whu * cur.rh;
  activity::tanh(cur.u);
  cur.h = (1.0 - cur.z.array()) * cur.u.array() +
      cur.z.array() * prev.h.array();
  return cur;
}

void gru_cell_old::backward(gru_cell_old::State *prev, gru_cell_old::State *cur, gru_cell_old::Grad &grad,
                        const VecD &xt) {
  VecD delr, delz, delu, delrh;
  //d7=delz
  delz = activity::logisticPrime(cur->z).array() * cur->delh.array() *
      (cur->u - prev->h).array();
  delu =
      activity::tanhPrime(cur->u).array() * cur->delh.array() * cur->z.array();
  delrh = Whu.transpose() * delu;
  delr =
      activity::logisticPrime(cur->r).array() * delrh.array() * prev->h.array();

  cur->delx = Wxr.transpose() * delr
      + Wxz.transpose() * delz
      + Wxu.transpose() * delu;

  prev->delh.noalias() += Whr.transpose() * delr + Whz.transpose() * delz;
  prev->delh.array() += delrh.array() * cur->r.array() +
          cur->delh.array() * (1.0 - cur->z.array());

  grad.Wxr.noalias() += delr * xt.transpose();
  grad.Whr.noalias() += delr * prev->h.transpose();

  grad.Wxz.noalias() += delz * xt.transpose();
  grad.Whz.noalias() += delz * prev->h.transpose();

  grad.Wxu.noalias() += delu * xt.transpose();
  grad.Whu.noalias() += delu * cur->rh.transpose();

  grad.br += delr;
  grad.bz += delz;
  grad.bu += delu;
}

void gru_cell_old::sgd(const gru_cell_old::Grad &grad, const real learningRate) {
  Wxr -= learningRate * grad.Wxr;
  Whr -= learningRate * grad.Whr;
  br -= learningRate * grad.br;

  Wxz -= learningRate * grad.Wxz;
  Whz -= learningRate * grad.Whz;
  bz -= learningRate * grad.bz;

  Wxu -= learningRate * grad.Wxu;
  Whu -= learningRate * grad.Whu;
  bu -= learningRate * grad.bu;
}

void gru_cell_old::save(std::ofstream &ofs) {
  rnn::generic::save(ofs, Wxr);
  rnn::generic::save(ofs, Whr);
  rnn::generic::save(ofs, br);
  rnn::generic::save(ofs, Wxz);
  rnn::generic::save(ofs, Whz);
  rnn::generic::save(ofs, bz);
  rnn::generic::save(ofs, Wxu);
  rnn::generic::save(ofs, Whu);
  rnn::generic::save(ofs, bu);
}

void gru_cell_old::load(std::ifstream &ifs) {
  rnn::generic::load(ifs, Wxr);
  rnn::generic::load(ifs, Whr);
  rnn::generic::load(ifs, br);
  rnn::generic::load(ifs, Wxz);
  rnn::generic::load(ifs, Whz);
  rnn::generic::load(ifs, bz);
  rnn::generic::load(ifs, Wxu);
  rnn::generic::load(ifs, Whu);
  rnn::generic::load(ifs, bu);
}

void gru_cell_old::State::clear() {
  this->h = VecD();
  this->u = VecD();
  this->r = VecD();
  this->z = VecD();
  this->rh = VecD();
  this->delh = VecD();
  this->delx = VecD();
}

gru_cell_old::Grad::Grad(const gru_cell_old &gru) {
  this->Wxr = MatD::Zero(gru.Wxr.rows(), gru.Wxr.cols());
  this->Whr = MatD::Zero(gru.Whr.rows(), gru.Whr.cols());
  this->br = VecD::Zero(gru.br.rows());

  this->Wxz = MatD::Zero(gru.Wxz.rows(), gru.Wxz.cols());
  this->Whz = MatD::Zero(gru.Whz.rows(), gru.Whz.cols());
  this->bz = VecD::Zero(gru.bz.rows());

  this->Wxu = MatD::Zero(gru.Wxu.rows(), gru.Wxu.cols());
  this->Whu = MatD::Zero(gru.Whu.rows(), gru.Whu.cols());
  this->bu = VecD::Zero(gru.bu.rows());
};

void gru_cell_old::Grad::init() {
  this->Wxr.setZero();
  this->Whr.setZero();
  this->br.setZero();
  this->Wxz.setZero();
  this->Whz.setZero();
  this->bz.setZero();
  this->Wxu.setZero();
  this->Whu.setZero();
  this->bu.setZero();
}

real gru_cell_old::Grad::norm() {
  return
      this->Wxr.squaredNorm() + this->Whr.squaredNorm() +
          this->br.squaredNorm() +
          this->Wxz.squaredNorm() + this->Whz.squaredNorm() +
          this->bz.squaredNorm() +
          this->Wxu.squaredNorm() + this->Whu.squaredNorm() +
          this->bu.squaredNorm();
}

void gru_cell_old::Grad::operator+=(const gru_cell_old::Grad &grad) {
  this->Wxr += grad.Wxr;
  this->Whr += grad.Whr;
  this->br += grad.br;
  this->Wxz += grad.Wxz;
  this->Whz += grad.Whz;
  this->bz += grad.bz;
  this->Wxu += grad.Wxu;
  this->Whu += grad.Whu;
  this->bu += grad.bu;
}
}


#include <iostream>
#include <cstdlib>
#include <algorithm>
#include <chrono>

#include <boost/numeric/ublas/io.hpp>

#include "../src/FloatMatrix.hpp"
#include "../src/MatrixMultiply.hpp"

void initRandomMatrix(scottgs::FloatMatrix& m);

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

	// ---------------------------------------------
	// BEGIN: Timing Analysis
	// ---------------------------------------------
	std::cout << "Running Timing Analysis" << std::endl
		  << "-----------------------" << std::endl;
	srand(123456);	// use a constant number to seed the pseudo random number generator
			// this way your results at least have the same input on a given system

	const unsigned int ITR=100;

	// ---------------------------------------------
	// Build up a set of test matrix-pair sizes
	// ---------------------------------------------
	std::vector<std::pair<std::pair<unsigned short,unsigned short>, std::pair<unsigned short,unsigned short> > > testList;
	testList.push_back( std::make_pair(std::pair<unsigned short,unsigned short>(50,40),
					   std::pair<unsigned short,unsigned short>(40,60)) );

	testList.push_back( std::make_pair(std::pair<unsigned short,unsigned short>(60,50),
					   std::pair<unsigned short,unsigned short>(50,70)) );

	testList.push_back( std::make_pair(std::pair<unsigned short,unsigned short>(70,60),
					   std::pair<unsigned short,unsigned short>(60,80)) );

	testList.push_back( std::make_pair(std::pair<unsigned short,unsigned short>(80,70),
					   std::pair<unsigned short,unsigned short>(70,90)) );

// Change this to 1 to enable larger matrices
#if 0
	testList.push_back( std::make_pair(std::pair<unsigned short,unsigned short>(90,80),
					   std::pair<unsigned short,unsigned short>(80,100)) );

	testList.push_back( std::make_pair(std::pair<unsigned short,unsigned short>(100,90),
					   std::pair<unsigned short,unsigned short>(90,110)) );

	testList.push_back( std::make_pair(std::pair<unsigned short,unsigned short>(200,90),
					   std::pair<unsigned short,unsigned short>(90,220)) );

	testList.push_back( std::make_pair(std::pair<unsigned short,unsigned short>(200,180),
					   std::pair<unsigned short,unsigned short>(180,220)) );

	testList.push_back( std::make_pair(std::pair<unsigned short,unsigned short>(400,360),
					   std::pair<unsigned short,unsigned short>(360,240)) );

#endif
	// ***********************************
	// Test functor : Your implementation
	// ***********************************

	scottgs::MatrixMultiply mm;

	for (std::vector<std::pair<std::pair<unsigned short,unsigned short>, std::pair<unsigned short,unsigned short> > >::const_iterator t=testList.begin();
		t!=testList.end();++t)
	{
		// Get matrix size pairs from iterator
		// instantiate matrices, then randomize
		std::pair<unsigned short,unsigned short> lp = t->first;
		std::pair<unsigned short,unsigned short> rp = t->second;
		scottgs::FloatMatrix l(lp.first,lp.second);
		scottgs::FloatMatrix r(rp.first,rp.second);
		initRandomMatrix(l);
		initRandomMatrix(r);

		// Run Timing Experiment
                std::chrono::high_resolution_clock c;
                std::chrono::high_resolution_clock::time_point start = c.now();
		for (unsigned int i = 0; i < ITR; ++i)
		{
			// This is an assignemnt of a call to a object functor;
			scottgs::FloatMatrix p = mm(l,r);
		}
                std::chrono::high_resolution_clock::time_point stop = c.now();
		double avgMs = (double) std::chrono::duration_cast<std::chrono::microseconds>(stop - start).count() / (1000000 * ITR);

		// Compute Ops and Elements
		// Log timing statistics
		const unsigned long opsMaybe = l.size1() * r.size2() * l.size2() + l.size1() + r.size2();
		const unsigned long elements = l.size1() * r.size2();
		std::cout << "------------------------------------------------------------------" << std::endl
			  << ITR << " iterations of matrix multiplication (functor) ran using ("
			  << l.size1() <<","<< l.size2() <<")*("
			  << r.size1() <<","<< r.size2() <<") = ("
			  << l.size1() <<","<< r.size2() <<")" << std::endl
			  << "      :Method:Average Time (s):approximate ops:computed elements" << std::endl
			  << "Data  Point:f:" << avgMs << ":" << opsMaybe<< ":" << elements << std::endl;
	}

#if 1
	// ***********************************
	// Test built-in Boost prod()
	// ***********************************

	for (std::vector<std::pair<std::pair<unsigned short,unsigned short>, std::pair<unsigned short,unsigned short> > >::const_iterator t=testList.begin();
		t!=testList.end();++t)
	{
		// Get matrix size pairs from iterator
		// instantiate matrices, then randomize
		std::pair<unsigned short,unsigned short> lp = t->first;
		std::pair<unsigned short,unsigned short> rp = t->second;
		scottgs::FloatMatrix l(lp.first,lp.second);
		scottgs::FloatMatrix r(rp.first,rp.second);
		initRandomMatrix(l);
		initRandomMatrix(r);

		// Run Timing Experiment
                std::chrono::high_resolution_clock c;
                std::chrono::high_resolution_clock::time_point start = c.now();
                for (unsigned int i = 0; i < ITR; ++i)
		{
			scottgs::FloatMatrix p = mm.multiply(l,r);
		}
                std::chrono::high_resolution_clock::time_point stop = c.now();
		double avgMs = (double) std::chrono::duration_cast<std::chrono::microseconds>(stop - start).count() / (1000000 * ITR);

		// Compute Ops and Elements
		// Log timing statistics
		const unsigned long opsMaybe = l.size1() * r.size2() * l.size2() + l.size1() + r.size2();
		const unsigned long elements = l.size1() * r.size2();
		std::cout << "------------------------------------------------------------------" << std::endl
			  << ITR << " iterations of matrix multiplication boost (method) ran using ("
			  << l.size1() <<","<< l.size2() <<")*("
			  << r.size1() <<","<< r.size2() <<") = ("
			  << l.size1() <<","<< r.size2() <<")" << std::endl
			  << "      :Method:Average Time (s):approximate ops:computed elements" << std::endl
			  << "Data Point:m1:" << avgMs << ":" << opsMaybe<< ":" << elements << std::endl;

	}
#endif

	std::cout << "Timing Analysis Completed" << std::endl
		  << "=========================" << std::endl;
	// ---------------------------------------------
	// END: Timing Analysis
	// ---------------------------------------------
}

void initRandomMatrix(scottgs::FloatMatrix& m)
{
	// Initialize each element.
	// See discussion board for better way,
	// this was originally posted to be a
	// simple example of per-element access into the matric
	for (unsigned i = 0; i < m.size1(); ++ i)
	        for (unsigned j = 0; j < m.size2(); ++ j)
		            m (i, j) = (static_cast<float>(rand()) / RAND_MAX) * 100.0;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2016 Jakub Szuppe <j.szuppe@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestMergeSortOnGPU
#include <boost/test/unit_test.hpp>

#include <iostream>

#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/is_sorted.hpp>
#include <boost/compute/algorithm/detail/merge_sort_on_gpu.hpp>
#include <boost/compute/container/vector.hpp>

#include "quirks.hpp"
#include "check_macros.hpp"
#include "context_setup.hpp"

namespace bc = boost::compute;

BOOST_AUTO_TEST_CASE(sort_small_vector_char)
{
    if(is_apple_cpu_device(device)) {
        std::cerr
            << "skipping all merge_sort_on_gpu tests due to Apple platform"
            << " behavior when local memory is used on a CPU device"
            << std::endl;
        return;
    }

    using boost::compute::char_;
    ::boost::compute::greater<char_> greater;
    ::boost::compute::less<char_> less;

    char_ data[] = { 'c', 'a', '0', '7', 'B', 'F', '\0', '$' };
    boost::compute::vector<char_> vector(data, data + 8, queue);
    BOOST_CHECK_EQUAL(vector.size(), size_t(8));
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end(), queue) == false);

    // <
    boost::compute::detail::merge_sort_on_gpu(
        vector.begin(), vector.end(), less, queue
    );
    BOOST_CHECK(
        boost::compute::is_sorted(vector.begin(), vector.end(), less, queue)
    );
    CHECK_RANGE_EQUAL(char_, 8, vector, ('\0', '$', '0', '7', 'B', 'F', 'a', 'c'));

    // >
    boost::compute::detail::merge_sort_on_gpu(
        vector.begin(), vector.end(), greater, queue
    );
    BOOST_CHECK(
        boost::compute::is_sorted(vector.begin(), vector.end(), greater, queue)
    );
    CHECK_RANGE_EQUAL(char_, 8, vector, ('c', 'a', 'F', 'B', '7', '0', '$', '\0'));
}

BOOST_AUTO_TEST_CASE(sort_mid_vector_int)
{
    if(is_apple_cpu_device(device)) {
        return;
    }

    using boost::compute::int_;
    ::boost::compute::greater<int_> greater;
    ::boost::compute::less<int_> less;

    const int_ size = 748;
    std::vector<int_> data(size);
    for(int_ i = 0; i < size; i++){
        data[i] = i%2 ? i : -i;
    }

    boost::compute::vector<int_> vector(data.begin(), data.end(), queue);
    BOOST_CHECK_EQUAL(vector.size(), size);
    BOOST_CHECK(!boost::compute::is_sorted(vector.begin(), vector.end(), queue));

    // <
    boost::compute::detail::merge_sort_on_gpu(
        vector.begin(), vector.end(), less, queue
    );
    BOOST_CHECK(
        boost::compute::is_sorted(vector.begin(), vector.end(), less, queue)
    );

    // >
    boost::compute::detail::merge_sort_on_gpu(
        vector.begin(), vector.end(), greater, queue
    );
    BOOST_CHECK(
        boost::compute::is_sorted(vector.begin(), vector.end(), greater, queue)
    );
}

BOOST_AUTO_TEST_CASE(sort_mid_vector_ulong)
{
    if(is_apple_cpu_device(device)) {
        return;
    }

    using boost::compute::ulong_;
    ::boost::compute::greater<ulong_> greater;
    ::boost::compute::less<ulong_> less;

    const ulong_ size = 260;
    std::vector<ulong_> data(size);
    for(ulong_ i = 0; i < size; i++){
        data[i] = i%2 ? i : i * i;
    }

    boost::compute::vector<ulong_> vector(data.begin(), data.end(), queue);
    BOOST_CHECK_EQUAL(vector.size(), size);
    BOOST_CHECK(!boost::compute::is_sorted(vector.begin(), vector.end(), queue));

    // <
    boost::compute::detail::merge_sort_on_gpu(
        vector.begin(), vector.end(), less, queue
    );
    BOOST_CHECK(
        boost::compute::is_sorted(vector.begin(), vector.end(), less, queue)
    );

    // >
    boost::compute::detail::merge_sort_on_gpu(
        vector.begin(), vector.end(), greater, queue
    );
    BOOST_CHECK(
        boost::compute::is_sorted(vector.begin(), vector.end(), greater, queue)
    );
}

BOOST_AUTO_TEST_CASE(sort_mid_vector_float)
{
    if(is_apple_cpu_device(device)) {
        return;
    }

    using boost::compute::float_;
    ::boost::compute::greater<float_> greater;
    ::boost::compute::less<float_> less;

    const int size = 513;
    std::vector<float_> data(size);
    for(int i = 0; i < size; i++){
        data[i] = float_(i%2 ? i : -i);
    }

    boost::compute::vector<float_> vector(data.begin(), data.end(), queue);
    BOOST_CHECK_EQUAL(vector.size(), size);
    BOOST_CHECK(!boost::compute::is_sorted(vector.begin(), vector.end(), queue));

    // <
    boost::compute::detail::merge_sort_on_gpu(
        vector.begin(), vector.end(), less, queue
    );
    BOOST_CHECK(
        boost::compute::is_sorted(vector.begin(), vector.end(), less, queue)
    );

    // >
    boost::compute::detail::merge_sort_on_gpu(
        vector.begin(), vector.end(), greater, queue
    );
    queue.finish();

    BOOST_CHECK(
        boost::compute::is_sorted(vector.begin(), vector.end(), greater, queue)
    );
}

BOOST_AUTO_TEST_CASE(sort_mid_vector_double)
{
    if(is_apple_cpu_device(device)) {
        return;
    }

    if(!device.supports_extension("cl_khr_fp64")){
        std::cout << "skipping test: device does not support double" << std::endl;
        return;
    }

    using boost::compute::double_;
    ::boost::compute::greater<double_> greater;
    ::boost::compute::less<double_> less;

    const int size = 1023;
    std::vector<double_> data(size);
    for(int i = 0; i < size; i++){
        data[i] = double_(i%2 ? i : -i);
    }

    boost::compute::vector<double_> vector(data.begin(), data.end(), queue);
    BOOST_CHECK_EQUAL(vector.size(), size);
    BOOST_CHECK(!boost::compute::is_sorted(vector.begin(), vector.end(), queue));

    // <
    boost::compute::detail::merge_sort_on_gpu(
        vector.begin(), vector.end(), less, queue
    );
    BOOST_CHECK(
        boost::compute::is_sorted(vector.begin(), vector.end(), less, queue)
    );

    // >
    boost::compute::detail::merge_sort_on_gpu(
        vector.begin(), vector.end(), greater, queue
    );
    queue.finish();

    BOOST_CHECK(
        boost::compute::is_sorted(vector.begin(), vector.end(), greater, queue)
    );
}

BOOST_AUTO_TEST_CASE(sort_mid_vector_int_custom_comparison_func)
{
    if(is_apple_cpu_device(device)) {
        return;
    }

    using boost::compute::int_;
    ::boost::compute::greater<int_> greater;
    ::boost::compute::less<int_> less;

    const int_ size = 1024;
    std::vector<int_> data(size);
    for(int_ i = 0; i < size; i++){
        data[i] = i%2 ? size - i : i - size;
    }

    BOOST_COMPUTE_FUNCTION(bool, abs_sort, (int_ a, int_ b),
    {
        return abs(a) < abs(b);
    });

    boost::compute::vector<int_> vector(data.begin(), data.end(), queue);
    BOOST_CHECK_EQUAL(vector.size(), size);
    BOOST_CHECK(
        !boost::compute::is_sorted(vector.begin(), vector.end(), abs_sort, queue)
    );

    boost::compute::detail::merge_sort_on_gpu(
        vector.begin(), vector.end(), abs_sort, queue
    );
    BOOST_CHECK(
        boost::compute::is_sorted(vector.begin(), vector.end(), abs_sort, queue)
    );
}

BOOST_AUTO_TEST_CASE(sort_mid_vector_int2)
{
    if(is_apple_cpu_device(device)) {
        return;
    }

    using boost::compute::int2_;
    using boost::compute::int_;
    ::boost::compute::greater<int2_> greater;
    ::boost::compute::less<int2_> less;

    const int_ size = 1024;
    std::vector<int2_> data(size);
    for(int_ i = 0; i < size; i++){
        data[i] = i%2 ? int2_(i, i) : int2_(i - size, i - size);
    }

    BOOST_COMPUTE_FUNCTION(bool, abs_sort, (int2_ a, int2_ b),
    {
        return abs(a.x + a.y) < abs(b.x + b.y);
    });

    boost::compute::vector<int2_> vector(data.begin(), data.end(), queue);
    BOOST_CHECK_EQUAL(vector.size(), size);
    BOOST_CHECK(
        !boost::compute::is_sorted(vector.begin(), vector.end(), abs_sort, queue)
    );

    boost::compute::detail::merge_sort_on_gpu(
        vector.begin(), vector.end(), abs_sort, queue
    );
    BOOST_CHECK(
        boost::compute::is_sorted(vector.begin(), vector.end(), abs_sort, queue)
    );
}

BOOST_AUTO_TEST_CASE(sort_mid_vector_long8)
{
    if(is_apple_cpu_device(device)) {
        return;
    }

    using boost::compute::long8_;
    using boost::compute::long_;
    ::boost::compute::greater<long8_> greater;
    ::boost::compute::less<long8_> less;

    const long_ size = 256;
    std::vector<long8_> data(size);
    for(long_ i = 0; i < size; i++){
        data[i] = i%2 ? long8_(i) : long8_(i * i);
    }

    BOOST_COMPUTE_FUNCTION(bool, comp, (long8_ a, long8_ b),
    {
        return a.s0 < b.s3;
    });

    boost::compute::vector<long8_> vector(data.begin(), data.end(), queue);
    BOOST_CHECK_EQUAL(vector.size(), size);
    BOOST_CHECK(
        !boost::compute::is_sorted(vector.begin(), vector.end(), comp, queue)
    );

    boost::compute::detail::merge_sort_on_gpu(
        vector.begin(), vector.end(), comp, queue
    );
    BOOST_CHECK(
        boost::compute::is_sorted(vector.begin(), vector.end(), comp, queue)
    );
}

BOOST_AUTO_TEST_CASE(stable_sort_vector_int2)
{
    if(is_apple_cpu_device(device)) {
        return;
    }

    using boost::compute::int2_;

    int2_ data[] = {
        int2_(8, 3), int2_(5, 1),
        int2_(2, 1), int2_(6, 1),
        int2_(8, 1), int2_(7, 1),
        int2_(4, 1), int2_(8, 2)
    };

    BOOST_COMPUTE_FUNCTION(bool, comp, (int2_ a, int2_ b),
    {
        return a.x < b.x;
    });

    boost::compute::vector<int2_> vector(data, data + 8, queue);
    BOOST_CHECK_EQUAL(vector.size(), 8);
    BOOST_CHECK(
        !boost::compute::is_sorted(vector.begin(), vector.end(), comp, queue)
    );

    //
    boost::compute::detail::merge_sort_on_gpu(
        vector.begin(), vector.end(), comp, true /*stable*/, queue
    );
    BOOST_CHECK(
        boost::compute::is_sorted(vector.begin(), vector.end(), comp, queue)
    );
    CHECK_RANGE_EQUAL(
        int2_, 8, vector,
        (
            int2_(2, 1), int2_(4, 1),
            int2_(5, 1), int2_(6, 1),
            int2_(7, 1), int2_(8, 3),
            int2_(8, 1), int2_(8, 2)
        )
    );
}

BOOST_AUTO_TEST_SUITE_END()