#pragma once

#include <cmath>
#include <map>

#include <Eigen/SparseLU>

#include "OpenABF/Exceptions.hpp"
#include "OpenABF/HalfEdgeMesh.hpp"
#include "OpenABF/Math.hpp"

namespace OpenABF
{

/**
 * @brief Compute parameterized mesh using Angle-based LSCM
 *
 * Computes a least-squares conformal parameterization of a mesh. Unlike the
 * original LSCM algorithm, this class ignores the 3D vertex positions and
 * instead uses the angle associated with the mesh's edge trait
 * (MeshType::EdgeTraits::alpha) to calculate the initial per-triangle edge
 * lengths. Without previously modifying the angles of the provided mesh, this
 * class produces the same result as a vertex-based LSCM implementation.
 * However, by first processing the mesh with a parameterized angle optimizer,
 * such as ABFPlusPlus, the parameterization can be improved, sometimes
 * significantly.
 *
 * Implements the angle-based variant of "Least squares conformal maps for
 * automatic texture atlas generation" by Lévy _et al._ (2002)
 * \cite levy2002lscm.
 *
 * @tparam T Floating-point type
 * @tparam MeshType HalfEdgeMesh type which implements the default mesh traits
 * @tparam Solver A solver implementing the
 * [Eigen Sparse solver
 * concept](https://eigen.tuxfamily.org/dox-devel/group__TopicSparseSystems.html)
 * and templated on Eigen::SparseMatrix<T>
 */
template <
    typename T,
    class MeshType = HalfEdgeMesh<T>,
    class Solver =
        Eigen::SparseLU<Eigen::SparseMatrix<T>, Eigen::COLAMDOrdering<int>>,
    std::enable_if_t<std::is_floating_point<T>::value, bool> = true>
class AngleBasedLSCM
{
public:
    /** @brief Mesh type alias */
    using Mesh = MeshType;

    /** @copydoc AngleBasedLSCM::Compute */
    void compute(typename Mesh::Pointer& mesh) const { Compute(mesh); }

    /**
     * @brief Compute the parameterized mesh
     *
     * @throws MeshException If pinned vertex is not on boundary.
     * @throws SolverException If matrix cannot be decomposed or if solver fails
     * to find a solution.
     */
    static void Compute(typename Mesh::Pointer& mesh)
    {
        using Triplet = Eigen::Triplet<T>;
        using SparseMatrix = Eigen::SparseMatrix<T>;
        using DenseMatrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;

        // Pinned vertex selection
        // Get the end points of a boundary edge
        auto p0 = mesh->vertices_boundary()[0];
        auto e = p0->edge;
        do {
            if (not e->pair) {
                break;
            }
            e = e->pair->next;
        } while (e != p0->edge);
        if (e == p0->edge and e->pair) {
            throw MeshException("Pinned vertex not on boundary");
        }
        auto p1 = e->next->vertex;

        // Map selected edge to closest XY axis
        // Use sign to select direction
        auto pinVec = p1->pos - p0->pos;
        auto dist = norm(pinVec);
        pinVec /= dist;
        p0->pos = {T(0), T(0), T(0)};
        auto maxElem = std::max_element(pinVec.begin(), pinVec.end());
        auto maxAxis = std::distance(pinVec.begin(), maxElem);
        dist = std::copysign(dist, *maxElem);
        if (maxAxis == 0) {
            p1->pos = {dist, T(0), T(0)};
        } else {
            p1->pos = {T(0), dist, T(0)};
        }

        // For convenience
        auto numFaces = mesh->num_faces();
        auto numVerts = mesh->num_vertices();
        auto numFixed = 2;
        auto numFree = numVerts - numFixed;

        // Permutation for free vertices
        // This helps us find a vert's row in the solution matrix
        std::map<std::size_t, std::size_t> freeIdxTable;
        for (const auto& v : mesh->vertices()) {
            if (v == p0 or v == p1) {
                continue;
            }
            auto newIdx = freeIdxTable.size();
            freeIdxTable[v->idx] = newIdx;
        }

        // Setup pinned bFixed
        std::vector<Triplet> tripletsB;
        tripletsB.emplace_back(0, 0, p0->pos[0]);
        tripletsB.emplace_back(1, 0, p0->pos[1]);
        tripletsB.emplace_back(2, 0, p1->pos[0]);
        tripletsB.emplace_back(3, 0, p1->pos[1]);
        SparseMatrix bFixed(2 * numFixed, 1);
        bFixed.reserve(tripletsB.size());
        bFixed.setFromTriplets(tripletsB.begin(), tripletsB.end());

        // Setup variables matrix
        // Are only solving for free vertices, so push pins in special matrix
        std::vector<Triplet> tripletsA;
        tripletsB.clear();
        for (const auto& f : mesh->faces()) {
            auto e0 = f->head;
            auto e1 = e0->next;
            auto e2 = e1->next;
            auto sin0 = std::sin(e0->alpha);
            auto sin1 = std::sin(e1->alpha);
            auto sin2 = std::sin(e2->alpha);

            // Find the max sin idx
            std::vector<T> sins{sin0, sin1, sin2};
            auto sinMaxElem = std::max_element(sins.begin(), sins.end());
            auto sinMaxIdx = std::distance(sins.begin(), sinMaxElem);

            // Rotate the edge order of the face so last angle is largest
            if (sinMaxIdx == 0) {
                auto temp = e0;
                e0 = e1;
                e1 = e2;
                e2 = temp;
                sin0 = sins[1];
                sin1 = sins[2];
                sin2 = sins[0];
            } else if (sinMaxIdx == 1) {
                auto temp = e2;
                e2 = e1;
                e1 = e0;
                e0 = temp;
                sin0 = sins[2];
                sin1 = sins[0];
                sin2 = sins[1];
            }

            auto ratio = (sin2 == T(0)) ? T(1) : sin1 / sin2;
            auto cosine = std::cos(e0->alpha) * ratio;
            auto sine = sin0 * ratio;

            // If pin0 or pin1, put in fixedB matrix, else put in A
            auto row = 2 * f->idx;
            if (e0->vertex == p0) {
                tripletsB.emplace_back(row, 0, cosine - T(1));
                tripletsB.emplace_back(row, 1, -sine);
                tripletsB.emplace_back(row + 1, 0, sine);
                tripletsB.emplace_back(row + 1, 1, cosine - T(1));
            } else if (e0->vertex == p1) {
                tripletsB.emplace_back(row, 2, cosine - T(1));
                tripletsB.emplace_back(row, 3, -sine);
                tripletsB.emplace_back(row + 1, 2, sine);
                tripletsB.emplace_back(row + 1, 3, cosine - T(1));
            } else {
                auto freeIdx = freeIdxTable.at(e0->vertex->idx);
                tripletsA.emplace_back(row, 2 * freeIdx, cosine - T(1));
                tripletsA.emplace_back(row, 2 * freeIdx + 1, -sine);
                tripletsA.emplace_back(row + 1, 2 * freeIdx, sine);
                tripletsA.emplace_back(row + 1, 2 * freeIdx + 1, cosine - T(1));
            }

            if (e1->vertex == p0) {
                tripletsB.emplace_back(row, 0, -cosine);
                tripletsB.emplace_back(row, 1, sine);
                tripletsB.emplace_back(row + 1, 0, -sine);
                tripletsB.emplace_back(row + 1, 1, -cosine);
            } else if (e1->vertex == p1) {
                tripletsB.emplace_back(row, 2, -cosine);
                tripletsB.emplace_back(row, 3, sine);
                tripletsB.emplace_back(row + 1, 2, -sine);
                tripletsB.emplace_back(row + 1, 3, -cosine);
            } else {
                auto freeIdx = freeIdxTable.at(e1->vertex->idx);
                tripletsA.emplace_back(row, 2 * freeIdx, -cosine);
                tripletsA.emplace_back(row, 2 * freeIdx + 1, sine);
                tripletsA.emplace_back(row + 1, 2 * freeIdx, -sine);
                tripletsA.emplace_back(row + 1, 2 * freeIdx + 1, -cosine);
            }

            if (e2->vertex == p0) {
                tripletsB.emplace_back(row, 0, T(1));
                tripletsB.emplace_back(row + 1, 1, T(1));
            } else if (e2->vertex == p1) {
                tripletsB.emplace_back(row, 2, T(1));
                tripletsB.emplace_back(row + 1, 3, T(1));
            } else {
                auto freeIdx = freeIdxTable.at(e2->vertex->idx);
                tripletsA.emplace_back(row, 2 * freeIdx, T(1));
                tripletsA.emplace_back(row + 1, 2 * freeIdx + 1, T(1));
            }
        }
        SparseMatrix A(2 * numFaces, 2 * numFree);
        A.reserve(tripletsA.size());
        A.setFromTriplets(tripletsA.begin(), tripletsA.end());

        SparseMatrix bFree(2 * numFaces, 2 * numFixed);
        bFree.reserve(tripletsB.size());
        bFree.setFromTriplets(tripletsB.begin(), tripletsB.end());

        // Calculate rhs from free and fixed matrices
        SparseMatrix b = bFree * bFixed * -1;

        // Setup AtA and solver
        SparseMatrix AtA = A.transpose() * A;
        AtA.makeCompressed();
        Solver solver;
        solver.compute(AtA);
        if (solver.info() != Eigen::ComputationInfo::Success) {
            throw SolverException(solver.lastErrorMessage());
        }

        // Setup Atb
        SparseMatrix Atb = A.transpose() * b;

        // Solve AtAx = AtAb
        DenseMatrix x = solver.solve(Atb);

        // Assign solution to UV coordinates
        // Pins are already updated, so these are free vertices
        for (const auto& v : mesh->vertices()) {
            if (v == p0 or v == p1) {
                continue;
            }
            auto newIdx = 2 * freeIdxTable.at(v->idx);
            v->pos[0] = x(newIdx, 0);
            v->pos[1] = x(newIdx + 1, 0);
            v->pos[2] = T(0);
        }
    }
};

}  // namespace OpenABF


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

*/

#ifndef __OPTIMISATION2_BA_LDLT_HPP__
#define __OPTIMISATION2_BA_LDLT_HPP__

#include "../container/container.hpp"
#include "tuple_to_mat.hpp"
#include "../omp/omp.hpp"
#include "isdiagonal1f.hpp"
#include "mat.hpp"
#include "make_type.hpp"
#include <libv/lma/version.hpp>
#include <boost/mpl/for_each.hpp>
#include <boost/fusion/include/front.hpp>
#include <libv/lma/time/tictoc.hpp>

namespace lma
{
  template<class Delta, class X> struct AssignDelta
  {
    Delta& delta;
    const X& x;
    int& cpt;

    AssignDelta(Delta& delta_, const X& x_, int& cpt_):delta(delta_),x(x_),cpt(cpt_){}

    template<class Key> void operator()(ttt::wrap<Key>)
    {
      auto& refa = boost::fusion::at_key<Key>(delta);
//       #pragma omp parallel for if(use_omp())
      for(auto i = refa.first() ; i < refa.size() ; ++i)
      {
        for(size_t k = 0 ; k < refa.I ; ++k)
          refa(i)[k] = x[cpt++];
      }
    }
  };

  template<class Delta, class X> AssignDelta<Delta,X> assign_delta(Delta& delta, const X& x, int& cpt)
  {
    return AssignDelta<Delta,X>(delta,x,cpt);
  }

  namespace internal
  {
    template<bool IsDiagonal1F> struct LDLT
    {
      template<class Tag, class Container, class Delta>
      static void compute(const Container& container, Delta& delta)
      {
        auto a = to_mat<Tag,typename Container::OptimizeKeys,typename Container::Hessian>(container.A(),size_tuple<mpl::size<typename Container::OptimizeKeys>::value>(delta));
//         auto a = to_sparse<typename Container::OptimizeKeys,typename Container::Hessian>(container.A(),size_tuple<mpl::size<typename Container::OptimizeKeys>::value>(delta));
        auto b = to_matv<Tag>(container.B());

        typename ContainerOption<Tag,0,0>::MatrixD1 x(b.size());

        ldlt_solve(x,a,b);

#ifndef NDEBUG
        if (is_invalid(x)) throw NAN_ERROR("Ldlt : delta contains nan");
#endif
        int cpt = 0;
        mpl::for_each<typename Container::OptimizeKeys,ttt::wrap<mpl::_1>>(assign_delta(delta,x,cpt));
      }
    };

    template<> struct LDLT<true>
    {
      template<class Float, class Container, class Delta>
      static void compute(const Container& container, Delta& delta)
      {
        auto& h = bf::front(container.A()).second;
        auto& jte = bf::front(container.B()).second;
        auto& d = bf::front(delta).second;

        if (h.size() == h.v.size()) // matrice diagonale par block
      	{
      	  for(auto i = h.first() ; i < h.size() ; ++i)
      	    d(i) = h(i).llt().solve(jte(i));
      	}
      	else
      	{
      	  LDLT<false>::compute(container,delta);
      	}
      }
    };
  }

  struct LDLT
  {
    template<class Config>
    LDLT(Config){}

    template<class MatrixTag, class Container, class Delta, size_t TotalFParams = 1>
    void operator()(const Container& container, Delta& delta, const MatrixTag&) const
    {
      internal::LDLT< IsDiagonal<Container>::value >::template compute<MatrixTag>(container,delta);
    }
  };

}

namespace ttt
{
  template<> struct Name<lma::LDLT> { static std::string name(){ return "LDLT"; } };
}
#endif


/**	\file TestPerpTangentPoints.cpp
*	\brief
*/

/****************************************************************************/
/*	TestPerpTangentPoints.cpp												*/
/****************************************************************************/
/*                                                                          */
/*  Copyright 2008 - 2010 Paul Kohut                                        */
/*  Licensed under the Apache License, Version 2.0 (the "License"); you may */
/*  not use this file except in compliance with the License. You may obtain */
/*  a copy of the License at                                                */
/*                                                                          */
/*  http://www.apache.org/licenses/LICENSE-2.0                              */
/*                                                                          */
/*  Unless required by applicable law or agreed to in writing, software     */
/*  distributed under the License is distributed on an "AS IS" BASIS,       */
/*  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or         */
/*  implied. See the License for the specific language governing            */
/*  permissions and limitations under the License.                          */
/*                                                                          */
/****************************************************************************/

//#include "stdafx.h"
#include <string>
#include <fstream>
#include "LatLongConversions.h"
#include "..\GeoFormulas\Conversions.h"
#include "..\GeoFormulas\GeoFormulas.h"
#include <boost/regex.hpp>

using namespace boost;
using namespace GeoCalcs;
using namespace std;

bool ParseTestPerpTangentPoints(string sString)
{
	bool bPassed = false;
	TrimWhitespace(sString);
	string sTestId, sGeoStartLat, sGeoStartLon, sGeoAzimuth;
	string sArcCenterLat, sArcCenterLon, sArcRadius;
	string sIntPt1Lat, sIntPt1Lon, sIntPt2Lat, sIntPt2Lon;
	string sTanPt1Lat, sTanPt1Lon, sTanPt2Lat, sTanPt2Lon;
	try
	{
		regex_constants::syntax_option_type flags =  regex_constants::icase | regex_constants::perl;

		string sRxPat = "([a-z]+|[A-Z]+\\d+)[,]";
		sRxPat += "([0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[NS])[,]([0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[WE])[,]";
		sRxPat += "([-+]?[0-9]*[.]?[0-9]+)[,]";
		sRxPat += "([0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[NS])[,]([0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[WE])[,]";
		sRxPat += "([-+]?[0-9]*[.]?[0-9]+)[,]";
		sRxPat += "([N/A]+|[0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[NS])[,]([N/A]+|[0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[WE])[,]";
		sRxPat += "([N/A]+|[0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[NS])[,]([N/A]+|[0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[WE])[,]";
		sRxPat += "([N/A]+|[0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[NS])[,]([N/A]+|[0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[WE])[,]";
		sRxPat += "([N/A]+|[0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[NS])[,]([N/A]+|[0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[WE])";

		regex pat(sRxPat, flags);

		int const sub_matches[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, };
		sregex_token_iterator it(sString.begin(), sString.end(), pat, sub_matches);
		if(it != sregex_token_iterator())
		{
			sTestId = *it++;
			sGeoStartLat = *it++;
			sGeoStartLon = *it++;
			sGeoAzimuth = *it++;
			sArcCenterLat = *it++;
			sArcCenterLon = *it++;
			sArcRadius = *it++;
			sIntPt1Lat = *it++;
			sIntPt1Lon = *it++;
			sIntPt2Lat = *it++;
			sIntPt2Lon = *it++;
			sTanPt1Lat = *it++;
			sTanPt1Lon = *it++;
			sTanPt2Lat = *it++;
			sTanPt2Lon = *it++;
			bPassed = true;
		}
	}
	catch(regex_error & e)
	{
		cout << "\n" << e.what();
		return false;
	}

	LLPoint geoPt(Deg2Rad(ParseLatitude(sGeoStartLat)), Deg2Rad(ParseLongitude(sGeoStartLon)));
	double geoAzimuth = Deg2Rad(atof(sGeoAzimuth.c_str()));

	LLPoint arc(Deg2Rad(ParseLatitude(sArcCenterLat)), Deg2Rad(ParseLongitude(sArcCenterLon)));
	double radius = NmToMeters(atof(sArcRadius.c_str()));

	LLPoint llIntPt1(Deg2Rad(ParseLatitude(sIntPt1Lat)), Deg2Rad(ParseLongitude(sIntPt1Lon)));
	LLPoint llIntPt2(Deg2Rad(ParseLatitude(sIntPt2Lat)), Deg2Rad(ParseLongitude(sIntPt2Lon)));

	LLPoint tangentPt1(Deg2Rad(ParseLatitude(sTanPt1Lat)), Deg2Rad(ParseLongitude(sTanPt1Lon)));
	LLPoint tangentPt2(Deg2Rad(ParseLatitude(sTanPt2Lat)), Deg2Rad(ParseLongitude(sTanPt2Lon)));

	LLPoint linePts[2];
	LLPoint tanPts[2];
	PerpTangentPoints(geoPt, geoAzimuth, arc, radius, linePts, tanPts, 1e-9);

		string sInt1Lat = ConvertLatitudeDdToDms(Rad2Deg(linePts[0].latitude));
		string sInt1Lon = ConvertLongitudeDdToDms(Rad2Deg(linePts[0].longitude));
		string sInt2Lat = ConvertLatitudeDdToDms(Rad2Deg(linePts[1].latitude));
		string sInt2Lon = ConvertLongitudeDdToDms(Rad2Deg(linePts[1].longitude));
		string sTan1Lat = ConvertLatitudeDdToDms(Rad2Deg(tanPts[0].latitude));
		string sTan1Lon = ConvertLongitudeDdToDms(Rad2Deg(tanPts[0].longitude));
		string sTan2Lat = ConvertLatitudeDdToDms(Rad2Deg(tanPts[1].latitude));
		string sTan2Lon = ConvertLongitudeDdToDms(Rad2Deg(tanPts[1].longitude));

		double dTol = 1e-10;

		if(sInt1Lat.compare(sIntPt1Lat) != 0)
		{
			double dLat = Deg2Rad(ParseLatitude(sIntPt1Lat));
			if(IsApprox(dLat, linePts[0].latitude, dTol))
			{
				cout << "\n" << sTestId << " within rounding tolerance of " << dTol << ": Input Intercept pt 1 latitude: " << sIntPt1Lat << "  calced: " << sInt1Lat;
			} else {
				cout << "\n" << sTestId << " failed: Expected Intercept pt 1 latitude: " << sIntPt1Lat << "  calced: " << sInt1Lat;
				bPassed = false;
			}
		}

		if(sInt1Lon.compare(sIntPt1Lon) != 0)
		{
			double dLon = Deg2Rad(ParseLongitude(sIntPt1Lon));
			if(IsApprox(dLon, linePts[0].longitude, dTol))
			{
				cout << "\n" << sTestId << " within rounding tolerance of " << dTol << ": Input Intercept pt 1 longitude: " << sIntPt1Lon << "  calced: " << sInt1Lon;
			} else {
				cout << "\n" << sTestId << " failed: Expected Intercept pt 1 longitude: " << sIntPt1Lon << "  calced: " << sInt1Lon;
				bPassed = false;
			}
		}

		if(sInt2Lat.compare(sIntPt2Lat) != 0)
		{
			double dLat = Deg2Rad(ParseLatitude(sIntPt2Lat));
			if(IsApprox(dLat, linePts[1].latitude, dTol))
			{
				cout << "\n" << sTestId << " within rounding tolerance of " << dTol << ": Input Intercept pt 2 latitude: " << sIntPt2Lat << "  calced: " << sInt2Lat;
			} else {
				cout << "\n" << sTestId << " failed: Expected Intercept pt 2 latitude: " << sIntPt2Lat << "  calced: " << sInt2Lat;
				bPassed = false;
			}
		}

		if(sInt2Lon.compare(sIntPt2Lon) != 0)
		{
			double dLon = Deg2Rad(ParseLongitude(sIntPt2Lon));
			if(IsApprox(dLon, linePts[1].longitude, dTol))
			{
				cout << "\n" << sTestId << " within rounding tolerance of " << dTol << ": Input Intercept pt 2 longitude: " << sIntPt2Lon << "  calced: " << sInt2Lon;
			} else {
				cout << "\n" << sTestId << " failed: Expected Intercept pt 2 longitude: " << sIntPt2Lon << "  calced: " << sInt2Lon;
				bPassed = false;
			}
		}

		if(sTan1Lat.compare(sTanPt1Lat) != 0)
		{
			double dLat = Deg2Rad(ParseLatitude(sTanPt1Lat));
			if(IsApprox(dLat, tanPts[0].latitude, dTol))
			{
				cout << "\n" << sTestId << " within rounding tolerance of " << dTol << ": Input Tangent pt 1 latitude: " << sTanPt1Lat << "  calced: " << sTan1Lat;
			} else {
				cout << "\n" << sTestId << " failed: Expected Tangent pt 1 latitude: " << sTanPt1Lat << "  calced: " << sTan1Lat;
				bPassed = false;
			}
		}

		if(sTan1Lon.compare(sTanPt1Lon) != 0)
		{
			double dLon = Deg2Rad(ParseLongitude(sTanPt1Lon));
			if(IsApprox(dLon, tanPts[0].longitude, dTol))
			{
				cout << "\n" << sTestId << " within rounding tolerance of " << dTol << ": Input Tangent pt 1 longitude: " << sTanPt1Lon << "  calced: " << sTan1Lon;
			} else {
				cout << "\n" << sTestId << " failed: Expected Tangent pt 1 longitude: " << sTanPt1Lon << "  calced: " << sTan1Lon;
				bPassed = false;
			}
		}

		if(sTan2Lat.compare(sTanPt2Lat) != 0)
		{
			double dLat = Deg2Rad(ParseLatitude(sTanPt2Lat));
			if(IsApprox(dLat, tanPts[1].latitude, dTol))
			{
				cout << "\n" << sTestId << " within rounding tolerance of " << dTol << ": Input Tangent pt 2 latitude: " << sTanPt2Lat << "  calced: " << sTan2Lat;
			} else {
				cout << "\n" << sTestId << " failed: Expected Tangent pt 2 latitude: " << sTanPt2Lat << "  calced: " << sTan2Lat;
				bPassed = false;
			}
		}

		if(sTan2Lon.compare(sTanPt2Lon) != 0)
		{
			double dLon = Deg2Rad(ParseLongitude(sTanPt2Lon));
			if(IsApprox(dLon, tanPts[1].longitude, dTol))
			{
				cout << "\n" << sTestId << " within rounding tolerance of " << dTol << ": Input Tangent pt 2 longitude: " << sTanPt2Lon << "  calced: " << sTan2Lon;
			} else {
				cout << "\n" << sTestId << " failed: Expected Tangent pt 2 longitude: " << sTanPt2Lon << "  calced: " << sTan2Lon;
				bPassed = false;
			}
		}

	return bPassed;
}

int TestPerpTangentPoints(const string & sFilePath)
{
	ifstream infile;
	infile.exceptions(ifstream::eofbit | ifstream::failbit | ifstream::badbit);
	int nCount = 0;
	int nCommentCount = 0;
	bool bPassed = true;
	try
	{
		string sLine;
		infile.open(sFilePath.c_str(), ifstream::in);

		while(!infile.eof())
		{
			getline(infile, sLine);
			if(sLine.at(0) == '#')
			{
				nCommentCount++;
			}
			else
			{
				if(!ParseTestPerpTangentPoints(sLine))
					bPassed = false;
				nCount++;
			}
		}
		infile.close();
		return bPassed;
	}

	catch(ifstream::failure e)
	{
		int nError = -99;
		// Per C++ standards for ifstream::failbit with global function getline
		// No characters were extracted because the end was prematurely found.Notice
		// that some eofbit cases will also set failbit.
		// In this case the end of the file is read and causes both flags to be raised,
		// so this presumably means all the data has been read correctly.
		if((infile.rdstate() & ifstream::failbit) && (infile.rdstate() & ifstream::eofbit) != 0)
			nError = bPassed;
		else if((infile.rdstate() & ifstream::failbit) != 0)
			nError = -1;
		else if((infile.rdstate() & ifstream::badbit) != 0)
			nError = -2;
		else if((infile.rdstate() & ifstream::eofbit) != 0)
			nError = -3;
		if(infile.is_open())
			infile.close();
		return nError;
	}
}


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file boost/numeric/ublasx/operation/realmax.hpp
 *
 * \brief Largest positive normalized floating-point number.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * 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)
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_REALMAX_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_REALMAX_HPP

#include <boost/numeric/ublas/traits.hpp>
#include <limits>

namespace boost { namespace numeric { namespace ublasx {

using namespace boost::numeric::ublas;

/// Return the largest positive normalized floating-point number.
template <typename RealT>
BOOST_UBLAS_INLINE
typename type_traits<RealT>::real_type realmax()
{
    return ::std::numeric_limits<typename type_traits<RealT>::real_type>::max();
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_REALMAX_HPP


#include "stdafx.h"
#include <boost/test/unit_test.hpp>
#include "BirthdayChocolate.h"

using namespace HackerRank;

BirthdayChocolate* bc;

void assertCountPartitions(vector<int> s, int day, int month, int expectedCount) {
	BOOST_TEST(expectedCount == bc->CountPossiblePartitions(s, day, month));
}

BOOST_AUTO_TEST_SUITE(BirthdayChocolateTests)

BOOST_AUTO_TEST_CASE(GivenReqdInputs_WhenCountPossiblePartitions_ThenReturnPartitionsCount) {
	bc = new BirthdayChocolate;

	assertCountPartitions({ 1 }, 1, 1, 1);
	assertCountPartitions({ 1 }, 2, 1, 0);
	assertCountPartitions({ 2 }, 2, 1, 1);
	assertCountPartitions({ 1 }, 1, 2, 0);
	assertCountPartitions({ 1, 1 }, 1, 1, 2);
	assertCountPartitions({ 1, 1 }, 1, 3, 0);
	assertCountPartitions({ 1, 1 }, 2, 1, 0);
	assertCountPartitions({ 1, 1 }, 2, 2, 1);
	assertCountPartitions({ 1, 2, 1, 3, 2 }, 3, 2, 2);
	assertCountPartitions({ 1, 1, 1, 1, 1, 1 }, 3, 2, 0);
	assertCountPartitions({ 4, 1 }, 4, 1, 1);
}

BOOST_AUTO_TEST_SUITE_END()


#ifdef STAND_ALONE
#   define BOOST_TEST_MODULE DemoTests
#endif

#include <boost/test/unit_test.hpp>
#include <stdexcept>
#include "../src/shared/Calculator.hpp"

// Scenariusze danych:

// add
// 1,1
// 3,1
// 1,3
// -1,-3
// -3,-1
// 'a','b'
// 1.5,2.5
// min int i max int

// min

// mult

// div

BOOST_AUTO_TEST_SUITE(calculator_suite)

    BOOST_AUTO_TEST_CASE(add_test_1_1)
    {
        Calculator calculator(1,1);
        BOOST_CHECK_EQUAL(calculator.add(), 2);
    }

    BOOST_AUTO_TEST_CASE(add_test_3_1)
    {
        Calculator calculator(3,1);
        BOOST_CHECK_EQUAL(calculator.add(), 4);
    }

    BOOST_AUTO_TEST_CASE(add_test_1_3)
    {
        Calculator calculator(1,3);
        BOOST_CHECK_EQUAL(calculator.add(), 4);
    }

    BOOST_AUTO_TEST_CASE(add_test_minus_1_minus_3)
    {
        Calculator calculator(-1,-3);
        BOOST_CHECK_EQUAL(calculator.add(), -4);
    }

    BOOST_AUTO_TEST_CASE(add_test_minus_3_minus_1)
    {
        Calculator calculator(-3,-1);
        BOOST_CHECK_EQUAL(calculator.add(), -4);
    }

    BOOST_AUTO_TEST_CASE(add_test_a_b)
    {
        Calculator calculator('a','b');  // 'a' in ascii table: 97, 'b' in ascii table: 98 !!!
        BOOST_CHECK_EQUAL(calculator.add(), 195);
    }

    BOOST_AUTO_TEST_CASE(add_test_1_5_2_5)
    {
        Calculator calculator(1.5,2.5);  // !!!
        BOOST_CHECK_EQUAL(calculator.add(), 4.0);
    }

    BOOST_AUTO_TEST_CASE(add_test_min_value)
    {
        Calculator calculator(5,std::numeric_limits<int>::min()); // min value for int is -2147483648
        BOOST_CHECK_EQUAL(calculator.add(), -2147483643);
    }

    BOOST_AUTO_TEST_CASE(add_test_max_value)
    {
        Calculator calculator(5,std::numeric_limits<int>::max()); // max value for int is 2147483647
        BOOST_CHECK_EQUAL(calculator.add(), -2147483644);  // !!!
    }

    BOOST_AUTO_TEST_CASE(div_test_1_0)
    {
        Calculator calculator(1,0);
        BOOST_CHECK_THROW(calculator.div(), std::exception);
    }

BOOST_AUTO_TEST_SUITE_END()


// Copyright 2015-2018 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/core/lightweight_test.hpp>
#include <boost/histogram/accumulators/mean.hpp>
#include <boost/histogram/accumulators/ostream.hpp>
#include <boost/histogram/accumulators/sum.hpp>
#include <boost/histogram/accumulators/thread_safe.hpp>
#include <boost/histogram/accumulators/weighted_mean.hpp>
#include <boost/histogram/accumulators/weighted_sum.hpp>
#include <boost/histogram/detail/throw_exception.hpp>
#include <sstream>
#include "is_close.hpp"

using namespace boost::histogram;
using namespace std::literals;

template <class T>
auto str(const T& t) {
  std::ostringstream os;
  os << t;
  return os.str();
}

int main() {
  {
    using w_t = accumulators::weighted_sum<double>;
    w_t w;
    BOOST_TEST_EQ(str(w), "weighted_sum(0, 0)"s);

    BOOST_TEST_EQ(w, w_t(0));
    BOOST_TEST_NE(w, w_t(1));
    w = w_t(1);
    BOOST_TEST_EQ(w.value(), 1);
    BOOST_TEST_EQ(w.variance(), 1);
    BOOST_TEST_EQ(w, 1);
    BOOST_TEST_NE(w, 2);

    w += 2;
    BOOST_TEST_EQ(w.value(), 3);
    BOOST_TEST_EQ(w.variance(), 5);
    BOOST_TEST_EQ(w, w_t(3, 5));
    BOOST_TEST_NE(w, w_t(3));

    w += w_t(1, 2);
    BOOST_TEST_EQ(w.value(), 4);
    BOOST_TEST_EQ(w.variance(), 7);

    // consistency: a weighted counter increased by weight 1 multiplied
    // by 2 must be the same as a weighted counter increased by weight 2
    w_t u(0);
    ++u;
    u *= 2;
    BOOST_TEST_EQ(u, w_t(2, 4));

    w_t v(0);
    v += 2;
    BOOST_TEST_EQ(u, v);

    // conversion to RealType
    w_t y(1, 2);
    BOOST_TEST_NE(y, 1);
    BOOST_TEST_EQ(static_cast<double>(y), 1);
  }

  {
    using m_t = accumulators::mean<double>;
    m_t a;
    BOOST_TEST_EQ(a.count(), 0);

    a(4);
    a(7);
    a(13);
    a(16);

    BOOST_TEST_EQ(a.count(), 4);
    BOOST_TEST_EQ(a.value(), 10);
    BOOST_TEST_EQ(a.variance(), 30);

    BOOST_TEST_EQ(str(a), "mean(4, 10, 30)"s);

    m_t b;
    b(1e8 + 4);
    b(1e8 + 7);
    b(1e8 + 13);
    b(1e8 + 16);

    BOOST_TEST_EQ(b.count(), 4);
    BOOST_TEST_EQ(b.value(), 1e8 + 10);
    BOOST_TEST_EQ(b.variance(), 30);

    auto c = a;
    c += a; // same as feeding all samples twice

    BOOST_TEST_EQ(c.count(), 8);
    BOOST_TEST_EQ(c.value(), 10);
    BOOST_TEST_IS_CLOSE(c.variance(), 25.714, 1e-3);
  }

  {
    using m_t = accumulators::weighted_mean<double>;
    m_t a;
    BOOST_TEST_EQ(a.sum_of_weights(), 0);

    a(0.5, 1);
    a(1.0, 2);
    a(0.5, 3);

    BOOST_TEST_EQ(a.sum_of_weights(), 2);
    BOOST_TEST_EQ(a.value(), 2);
    BOOST_TEST_IS_CLOSE(a.variance(), 0.8, 1e-3);

    BOOST_TEST_EQ(str(a), "weighted_mean(2, 2, 0.8)"s);

    auto b = a;
    b += a; // same as feeding all samples twice

    BOOST_TEST_EQ(b.sum_of_weights(), 4);
    BOOST_TEST_EQ(b.value(), 2);
    BOOST_TEST_IS_CLOSE(b.variance(), 0.615, 1e-3);
  }

  {
    double bad_sum = 0;
    bad_sum += 1;
    bad_sum += 1e100;
    bad_sum += 1;
    bad_sum += -1e100;
    BOOST_TEST_EQ(bad_sum, 0); // instead of 2

    accumulators::sum<double> sum;
    ++sum;
    BOOST_TEST_EQ(sum.large(), 1);
    BOOST_TEST_EQ(sum.small(), 0);
    BOOST_TEST_EQ(str(sum), "sum(1 + 0)"s);
    sum += 1e100;
    BOOST_TEST_EQ(str(sum), "sum(1e+100 + 1)"s);
    ++sum;
    BOOST_TEST_EQ(str(sum), "sum(1e+100 + 2)"s);
    sum += -1e100;
    BOOST_TEST_EQ(str(sum), "sum(0 + 2)"s);
    BOOST_TEST_EQ(sum, 2); // correct answer
    BOOST_TEST_EQ(sum.large(), 0);
    BOOST_TEST_EQ(sum.small(), 2);

    accumulators::sum<double> a(3), b(2), c(3);
    BOOST_TEST_LT(b, c);
    BOOST_TEST_LE(b, c);
    BOOST_TEST_LE(a, c);
    BOOST_TEST_GT(a, b);
    BOOST_TEST_GE(a, b);
    BOOST_TEST_GE(a, c);
  }

  {
    accumulators::weighted_sum<accumulators::sum<double>> w;

    ++w;
    w += 1e100;
    ++w;
    w += -1e100;

    BOOST_TEST_EQ(w.value(), 2);
    BOOST_TEST_EQ(w.variance(), 2e200);
  }

  {
    accumulators::thread_safe<int> i;
    ++i;
    i += 1000;

    BOOST_TEST_EQ(i, 1001);
    BOOST_TEST_EQ(str(i), "1001"s);
  }

  return boost::report_errors();
}


#ifndef MHTLP_HPP_
#define MHTLP_HPP_

#include <NTL/ZZ.h>
#include <assert.h>
#include <openssl/sha.h>
#include <vector>
#include <sstream>

#include "Puzzle.hpp"
#include "HTLP.hpp"

#ifndef RSA_
#define RSA_
typedef struct RSA
{
    NTL::ZZ p;
    NTL::ZZ q;
} RSA;
#endif

class MHTLP : public HTLP
{

private:
    NTL::ZZ chi_;

public:
    MHTLP(const long modulus_len, const long T, const long kappa);
    MHTLP(const NTL::ZZ &n, const NTL::ZZ &g, const NTL::ZZ &h, const NTL::ZZ &chi, const long T, const long kappa);
    MHTLP(const long modulus_len, const long T, const long kappa, bool cheeting_mode);

    NTL::ZZ chi()
    {
        return chi_;
    }

    MPuzzle GeneratePuzzle(const NTL::ZZ &s);
    MPuzzle GeneratePuzzle(const NTL::ZZ &s, const NTL::ZZ &r, const NTL::ZZ &r_prime);
    NTL::ZZ SolvePuzzle(const MPuzzle &Z);
    NTL::ZZ QuickSolvePuzzle(const MPuzzle &Z);

    std::tuple<std::vector<NTL::ZZ>, std::vector<NTL::ZZ>, std::vector<NTL::ZZ>, std::vector<NTL::ZZ>> GenerateMValidProof(const MPuzzle &Z, const NTL::ZZ &s, const NTL::ZZ &r, const NTL::ZZ &r_prime);
    bool VerifyMValidProof(const MPuzzle &Z, const std::tuple<std::vector<NTL::ZZ>, std::vector<NTL::ZZ>, std::vector<NTL::ZZ>, std::vector<NTL::ZZ>> &proof);

    std::tuple<NTL::ZZ, NTL::ZZ, NTL::ZZ, NTL::ZZ, NTL::ZZ> SolvePuzzleWithProof(const long k, const long gamma, const MPuzzle &Z);
    std::tuple<NTL::ZZ, NTL::ZZ, NTL::ZZ, NTL::ZZ, NTL::ZZ> QuickSolvePuzzleWithProof(const MPuzzle &Z);

    int VerifyProofOfSol(const MPuzzle Z, const std::tuple<NTL::ZZ, NTL::ZZ, NTL::ZZ, NTL::ZZ, NTL::ZZ> &proof);
};

#endif


#include "Derivatives.hpp"

#include <fmt/format.h>
#include <spdlog/spdlog.h>
#include <boost/range/adaptor/indexed.hpp>
#include <boost/range/combine.hpp>
#include <boost/range/irange.hpp>

#include <FeltElements/internal/Format.hpp>
#include "Body.hpp"

namespace
{
using namespace FeltElements;

template <typename T>
int sgn(T val)
{
	return (T(0) < val) - (val < T(0));
}
auto constexpr delta = [](auto const i, auto const j) { return i == j; };

Element::Elasticity const c_lambda = ([]() {  // NOLINT(cert-err58-cpp)
	std::size_t constexpr N = 3;
	Element::Elasticity c{};
	for (std::size_t i = 0; i < N; i++)
		for (std::size_t j = 0; j < N; j++)
			for (std::size_t k = 0; k < N; k++)
				for (std::size_t l = 0; l < N; l++) c(i, j, k, l) = delta(i, j) * delta(k, l);

	return c;
}());

// TODO: constexpr - requires constexpr std::copy (C++20)
Element::Elasticity const c_mu = ([]() {  // NOLINT(cert-err58-cpp)
	std::size_t constexpr N = 3;
	Element::Elasticity c{};
	for (std::size_t i = 0; i < N; i++)
		for (std::size_t j = 0; j < N; j++)
			for (std::size_t k = 0; k < N; k++)
				for (std::size_t l = 0; l < N; l++)
					c(i, j, k, l) = delta(i, k) * delta(j, l) + delta(i, l) * delta(j, k);

	return c;
}());

// TODO: constexpr - requries simd_vector_type to satisfy literal type requirements
Tensor::Matrix<3> const I = ([]() {	 // NOLINT(cert-err58-cpp)
	Tensor::Matrix<3> mat{};
	mat.eye2();
	return mat;
}());
}  // namespace

namespace FeltElements::ex
{
using namespace Tensor;

constexpr auto dX_by_dL = [](auto const & X) {
	return Func::einsum<Idxs<k, i>, Idxs<k, j>>(X, Derivatives::dN_by_dL);
};

constexpr auto dL_by_dX = [](auto const & dX_by_dL_) { return Func::inv(dX_by_dL_); };

constexpr auto dX_by_dS = [](auto const & X) {
	return Func::einsum<Idxs<k, i>, Idxs<k, j>>(X, Derivatives::dN_by_dS);
};

constexpr auto dN_by_dX = [](auto const & dL_by_dX_) {
	// dN/dX^T = dX/dL^(-T) * dN/dL^T => dN/dX = dN/dL * dX/dL^(-1) = dN/dL * dL/dX
	return Func::einsum<Idxs<i, k>, Idxs<k, j>>(Derivatives::dN_by_dL, dL_by_dX_);
};

constexpr auto dx_by_dX = [](auto const & x, auto const & dN_by_dX_) {
	return Func::einsum<Idxs<k, i>, Idxs<k, j>>(x, dN_by_dX_);
};

constexpr auto finger = [](auto const & F) { return Func::einsum<Idxs<i, k>, Idxs<j, k>>(F, F); };

constexpr auto sigma = [](Scalar const J, auto const & b, Scalar const lambda, Scalar const mu) {
	return (mu / J) * (b - I) + (lambda / J) * log(J) * I;
};

constexpr auto T = [](auto const & dN_by_dx, auto const & sigma_) {
	// T = v * sigma * dN/dx^T
	return Func::einsum<Idxs<a, k>, Idxs<i, k>>(dN_by_dx, sigma_);
};

constexpr auto c = [](Scalar J, Scalar lambda, Scalar mu) {
	Scalar const lambda_prime = lambda / J;
	Scalar const mu_prime = (mu - lambda * std::log(J)) / J;

	return lambda_prime * c_lambda + mu_prime * c_mu;
};

constexpr auto Kc = [](auto const & dN_by_dx, auto const & c_) {
	// Kc_ij = v * dN_a/dx_k * c_ikjl * dN_b/dx_l
	return Func::einsum<Idxs<a, k>, Idxs<i, k, j, l>, Idxs<b, l>, Order<a, i, b, j>>(
		dN_by_dx, c_, dN_by_dx);
};

constexpr auto Ks = [](auto const & dN_by_dx, auto const & s) {
	// Ks_ij = v * dN_a/dx_k * sigma_kl * dN_b/dx_l * delta_ij
	return Func::einsum<Idxs<a, k>, Idxs<k, l>, Idxs<b, l>, Idxs<i, j>, Order<a, i, b, j>>(
		dN_by_dx, s, dN_by_dx, I);
};
}  // namespace FeltElements::ex

namespace FeltElements::Derivatives
{
Element::StiffnessResidual KR(
	Element::NodePositions const & x,
	Element::BoundaryVtxhIdxs const & boundary_faces_idxs,
	Element::BoundaryNodePositions const & boundary_faces_x,
	Element::ShapeDerivative const & dN_by_dX,
	Body::Material const & material,
	Body::Forces const & forces)
{
	using Tensor::Func::all;

	Element::Gradient const dx_by_dX = ex::dx_by_dX(x, dN_by_dX);
	auto const b = ex::finger(dx_by_dX);
	Scalar const J = Derivatives::det_dx_by_dX(dx_by_dX);
	if (J < 0)
	{
		std::string const msg =
			fmt::format("Error: J < 0:\nJ = {}\nF = {}\nb = {}", J, dx_by_dX, b);
		spdlog::error(msg);
		throw std::invalid_argument{msg};
	}
	Scalar const v = Derivatives::v(x);	 // TODO: but also v = J*V

	auto const & dx_by_dL = ex::dX_by_dL(x);
	auto const & dL_by_dx = ex::dL_by_dX(dx_by_dL);
	Element::ShapeDerivative const dN_by_dx = ex::dN_by_dX(dL_by_dx);

	Element::Stress const sigma = ex::sigma(J, b, material.lambda, material.mu);

	auto const & c = ex::c(J, material.lambda, material.mu);
	// Elasticity component.
	auto const & Kc = ex::Kc(dN_by_dx, c);
	// Initial stress component.
	auto const & Ks = ex::Ks(dN_by_dx, sigma);
	// Constitutive (traction) component.
	auto const & Kp = Derivatives::Kp(boundary_faces_x, boundary_faces_idxs, forces.p);

	// Tangent stiffness matrix.
	Element::Stiffness K = v * (Kc + Ks) + Kp;

	// Internal forces.
	auto const & T_by_v = ex::T(dN_by_dx, sigma);

	// Residual
	Element::Forces R = v * T_by_v;

	// Body force.
	auto const & F_by_V = forces.F_by_m * material.rho;
	auto const & F_by_v = F_by_V / J;
	auto const & F_by_v_per_node = (1.0 / Element::num_nodes) * F_by_v;
	Node::Force const F_per_node = v * F_by_v_per_node;

	for (const auto node_idx : boost::irange(Element::num_nodes)) R(node_idx, all) -= F_per_node;

	// Traction force.
	for (const auto & [s, idxs] : boost::range::combine(boundary_faces_x, boundary_faces_idxs))
	{
		const Node::Force & t =
			(1.0 / BoundaryElement::num_nodes) * Derivatives::t(forces.p, Derivatives::dX_by_dS(s));
		for (Tensor::Index const node_idx : idxs) R(node_idx, all) -= t;
	}
	/*
		if (!Tensor::Func::all_of(R == R))	// Assert no NaNs
		{
			std::string const & msg = fmt::format(
				"Residual is NaN because: v={}; T_by_v={}; dN_by_dx={}; sigma={}; J={}; b={}; "
				"dx_by_dX={}.  Where J={}; dx_by_dX=\n{}",
				std::isnan(v),
				!Tensor::Func::all_of(T_by_v == T_by_v),
				!Tensor::Func::all_of(dN_by_dx == dN_by_dx),
				!Tensor::Func::all_of(sigma == sigma),
				std::isnan(J),
				!Tensor::Func::all_of(b == b),
				!Tensor::Func::all_of(dx_by_dX == dx_by_dX),
				J,
				dx_by_dX);

			spdlog::error(msg);

			throw std::logic_error{msg};
		};
	*/
	return {K, R};
}

Element::Stiffness Kc(
	Element::ShapeDerivative const & dN_by_dx, Scalar const v, Element::Elasticity const & c)
{
	return v * ex::Kc(dN_by_dx, c);
}

Element::Stiffness Ks(
	Element::ShapeDerivative const & dN_by_dx, Scalar const v, Element::Stress const & s)
{
	return v * ex::Ks(dN_by_dx, s);
}

Element::SurfaceShapeDerivative const dN_by_dS =  // NOLINT(cert-err58-cpp)
	Fastor::evaluate(Fastor::inv(Tensor::Matrix<3, 3>{
		{1, 1, 1},
		{0, 1, 0},
		{0, 0, 1},
	}))(Fastor::all, Fastor::fseq<1, 3>());
// clang-format on

namespace
{
auto const [KpLHS, KpRHS] = ([]() {	 // NOLINT(cert-err58-cpp)
	using Tensor::Func::all;
	Tensor::Matrix<3> KpLHS_, KpRHS_;
	for (auto const a : boost::irange(BoundaryElement::num_nodes))
		for (auto const b : boost::irange(BoundaryElement::num_nodes))
		{
			// Assuming single-point integral, i.e. sample at centre where Na = Nb = 1/3.
			KpLHS_(a, b) = dN_by_dS(a, 1) * (1.0 / 3.0) - dN_by_dS(b, 1) * (1.0 / 3.0);
			KpRHS_(a, b) = dN_by_dS(a, 0) * (1.0 / 3.0) - dN_by_dS(b, 0) * (1.0 / 3.0);
		}

	return std::tuple{KpLHS_, KpRHS_};
}());
}

Element::Stiffness Kp(
	Element::BoundaryNodePositions const & xs,
	const Element::BoundaryVtxhIdxs & S_to_Vs,
	Scalar const p)
{
	using Tensor::Idxs;
	using Tensor::Order;
	using Tensor::Func::all;
	using Tensor::Func::einsum;
	enum
	{
		i,
		j,
		k,
		a,
		b
	};
	Element::Stiffness K = 0;
	for (auto const & [x, S_to_V] : boost::range::combine(xs, S_to_Vs))
	{
		auto const & dx_by_dS = Derivatives::dX_by_dS(x);
		Scalar const A = Derivatives::A(x);

		Tensor::Vector<3> const & dx_by_dS1 = dx_by_dS(all, 0);
		Tensor::Vector<3> const & dx_by_dS2 = dx_by_dS(all, 1);

		BoundaryElement::Stiffness const & K_face = 0.5 * A * p *
			(einsum<Idxs<i, j, k>, Idxs<k>, Idxs<a, b>, Order<a, i, b, j>>(
				 levi_civita, dx_by_dS1, KpLHS) -
			 einsum<Idxs<i, j, k>, Idxs<k>, Idxs<a, b>, Order<a, i, b, j>>(
				 levi_civita, dx_by_dS2, KpRHS));

		for (auto const & [face_idx_a, cell_idx_a] : boost::adaptors::index(S_to_V))
			for (auto const & [face_idx_b, cell_idx_b] : boost::adaptors::index(S_to_V))
				K(cell_idx_a, all, cell_idx_b, all) += K_face(face_idx_a, all, face_idx_b, all);
	}

	return K;
}

Element::Elasticity c(Scalar J, Scalar lambda, Scalar mu)
{
	return ex::c(J, lambda, mu);
}

Node::Force t(Scalar const p, Element::SurfaceGradient const & dX_by_dS)
{
	using Tensor::Func::all;
	using Tensor::Func::fix;
	Node::Force const & dX1_by_dS = dX_by_dS(all, fix<0>);
	Node::Force const & dX2_by_dS = dX_by_dS(all, fix<1>);
	return (1.0 / 2.0) * p * cross(dX1_by_dS, dX2_by_dS);
}

Element::Forces T(
	Element::ShapeDerivative const & dN_by_dx, Scalar const v, Element::Stress const & sigma)
{
	return v * ex::T(dN_by_dx, sigma);
}

Element::Stress sigma(
	Scalar const J, Element::Gradient const & b, Scalar const lambda, Scalar const mu)
{
	return ex::sigma(J, b, lambda, mu);
}

Scalar det_dx_by_dX(Element::Gradient const & dx_by_dX)
{
	return Fastor::det(dx_by_dX);
}

Element::Gradient b(Element::Gradient const & F)
{
	return ex::finger(F);
}

Element::Gradient dx_by_dX(
	Element::NodePositions const & x, Element::ShapeDerivative const & dN_by_dX)
{
	return ex::dx_by_dX(x, dN_by_dX);
}

Element::Gradient dx_by_dX(Element::Gradient const & dx_by_dL, Element::Gradient const & dL_by_dX)
{
	using namespace Tensor;
	return Func::einsum<Idxs<k, i>, Idxs<j, k>>(dx_by_dL, dL_by_dX);
}

Element::ShapeDerivative dN_by_dX(Element::Gradient const & dL_by_dx)
{
	return ex::dN_by_dX(dL_by_dx);
}

Element::ShapeDerivative dN_by_dX(Element::NodePositions const & X)
{
	auto const & dX_by_dL = ex::dX_by_dL(X);
	auto const & dL_by_dX = ex::dL_by_dX(dX_by_dL);
	return ex::dN_by_dX(dL_by_dX);
}

Element::CartesianDerivative dx_by_dN(Element::ShapeCartesianTransform const & N_to_x)
{
	using namespace Tensor::Func;
	return N_to_x(fseq<1, last>{}, all);
}

Element::Gradient dL_by_dX(Element::Gradient const & dX_by_dL)
{
	return ex::dL_by_dX(dX_by_dL);
}

Element::Gradient dX_by_dL(Element::NodePositions const & X)
{
	return ex::dX_by_dL(X);
}

Element::SurfaceGradient dX_by_dS(BoundaryElement::NodePositions const & X)
{
	return ex::dX_by_dS(X);
}

Element::ShapeDerivative dN_by_dX(Element::ShapeCartesianTransform const & N_to_x)
{
	using namespace Tensor::Func;
	// Interpolation: (1, x, y, z)^T = N_to_x * N, where N is 4x natural coordinates (corners).
	// Invert then strip constant terms, leaving just coefficients, i.e. the derivative.
	return evaluate(inv(N_to_x))(all, fseq<1, last>{});
}

Element::ShapeCartesianTransform N_to_x(Element::NodePositions const & X)
{
	Element::ShapeCartesianTransform mat;
	mat(Fastor::ffirst, Fastor::all) = 1.0;
	mat(Fastor::fseq<1, Fastor::last>{}, Fastor::all) = Fastor::transpose(X);
	return mat;
}

Scalar V(Element::NodePositions const & x)
{
	using namespace Tensor::Func;
	auto const & start_3x3 = x(fseq<0, 3>{}, all);
	//	std::clog << x(fix<3>, all).self().size() << "\n";
	Tensor::Vector<3> end_1x3 = x(fix<3>, all);
	Tensor::Matrix<3> end_3x3;
	end_3x3(fix<0>, all) = end_1x3;
	end_3x3(fix<1>, all) = end_1x3;
	end_3x3(fix<2>, all) = end_1x3;
	auto const & delta = start_3x3 - end_3x3;
	return std::abs(Fastor::det(delta) / 6.0);
}

Scalar v(Element::NodePositions const & x)
{
	// Volume in local coords (constant)
	constexpr Scalar v_wrt_L = 1.0 / 6.0;
	return v_wrt_L * det_dx_by_dL(x);
}

Scalar A(BoundaryElement::NodePositions const & s)
{
	using Tensor::Func::all;
	using Tensor::Func::cross;
	using Tensor::Func::norm;
	Tensor::Vector<3> const & v1 = s(0, all) - s(2, all);
	Tensor::Vector<3> const & v2 = s(1, all) - s(2, all);
	return 0.5 * norm(cross(v1, v2));
}

Scalar det_dx_by_dL(Element::NodePositions const & x)
{
	using namespace Tensor;
	using Func::all;
	using Func::einsum;
	using Func::fix;

	//	Scalar det_ = 0;
	//	for (Index i = 0; i < Element::count; i++)
	//		for (Index j = 0; j < Element::count; j++)
	//			for (Index k = 0; k < Element::count; k++)
	//				det_ += det_dN_by_dL(i, j, k) * x(i, 0) * x(j, 1) * x(k, 2);
	//
	//	return det_;

	//	Element::ShapeDerivativeDeterminant xs = ([&x](){
	//		using Tensor::Index;
	//		using Tensor::Func::all;
	//		Element::ShapeDerivativeDeterminant xs_;
	//		for (Index i = 0; i < Element::count; i++)
	//			for (Index j = 0; j < Element::count; j++)
	//				for (Index k = 0; k < Element::count; k++)
	//					xs_(i, j, k) = x(i, 0) * x(j, 1) * x(k, 2);
	//		return xs_;
	//	}());
	//	Tensor::Vector<1> sum = einsum<Idxs<i, j, k>, Idxs<i, j, k>>(
	//		det_dN_by_dL, xs);

	// "Consideration of Body Forces within Finite Element Analysis", Glenk et al., 2018
	Tensor::Vector<4> x0 = x(all, fix<0>);
	Tensor::Vector<4> x1 = x(all, fix<1>);
	Tensor::Vector<4> x2 = x(all, fix<2>);
	Tensor::Vector<1> sum =
		einsum<Idxs<i, j, k>, Idxs<i>, Idxs<j>, Idxs<k>>(Derivatives::det_dN_by_dL, x0, x1, x2);

	return sum(0);
}

// clang-format off
Element::IsoCoordDerivative const dL_by_dN = // NOLINT(cert-err58-cpp)
	Tensor::Matrix<4, 4>{
		{1, 1, 1, 1},
		{0, 1, 0, 0},
		{0, 0, 1, 0},
		{0, 0, 0, 1}}(Fastor::fseq<1, 4>(), Fastor::all);

Element::ShapeDerivative const dN_by_dL = // NOLINT(cert-err58-cpp)
	Fastor::evaluate(Fastor::inv(Tensor::Matrix<4, 4>{
		{1, 1, 1, 1},
		{0, 1, 0, 0},
		{0, 0, 1, 0},
		{0, 0, 0, 1}}))(Fastor::all, Fastor::fseq<1, 4>());

Element::ShapeDerivativeDeterminant const det_dN_by_dL = ([]() {  // NOLINT(cert-err58-cpp)
	using Tensor::Index;
	using Tensor::Func::all;
	using Tensor::Func::det;
	Element::ShapeDerivativeDeterminant det_dN_by_dL_;
	for (Index i = 0; i < Element::num_nodes; i++)
		for (Index j = 0; j < Element::num_nodes; j++)
			for (Index k = 0; k < Element::num_nodes; k++)
			{
				Tensor::Matrix<3> dN_by_dL_ijk;
				dN_by_dL_ijk(0, all) = dN_by_dL(i, all);
				dN_by_dL_ijk(1, all) = dN_by_dL(j, all);
				dN_by_dL_ijk(2, all) = dN_by_dL(k, all);
				det_dN_by_dL_(i, j, k) = det(dN_by_dL_ijk);
			}
	return det_dN_by_dL_;
}());

Tensor::Multi<Node::dim, Node::dim, Node::dim> const levi_civita =	// NOLINT(cert-err58-cpp)
	([]() {
		using LeviCivita = Tensor::Multi<Node::dim, Node::dim, Node::dim>;
		LeviCivita E;
		int constexpr const count = static_cast<int>(Node::dim);
		for (int i = 0; i < count; i++)
			for (int j = 0; j < count; j++)
				for (int k = 0; k < count; k++) E(i, j, k) = sgn(j - i) * sgn(k - i) * sgn(k - j);
		return E;
	}());
}  // namespace FeltElements::Derivatives


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/constants/constants.hpp>

#include "miMaS/field.h"
#include "miMaS/weno.h"
#include "miMaS/fft.h"
#include "miMaS/array_view.h"
#include "miMaS/poisson.h"
#include "miMaS/rk.h"

#ifndef SIGMA
#define SIGMA (1.0)
#endif
#define E_MAX (2.0)

/*
template <unsigned int i>
struct phi
{
  static std::complex<double>
  operator () ( std::complex<double> const & z ) {
    static std::valarray<std::complex<double>> coeff(i);
    coeff[0] = 1.;

    for ( unsigned int k=1 ; k<coeff.size() ; ++k ) {
      coeff[k] = coeff[k-1] * z / (double(k));
    }

    return (std::exp(z) - std::accumulate( std::begin(coeff) , std::end(coeff) , std::complex<double>(0.,0.) ))/(std::pow(z,i));
  }
};
*/
template <unsigned int i>
std::complex<double>
phi ( std::complex<double> const & _z )
{
  std::valarray<std::complex<double>> coeff(i);
  coeff[0] = 1.;

  std::complex<double> z = _z;
  if ( _z == 0. ) { z = std::complex<double>(1.,0.); }

  for ( unsigned int k=1 ; k<coeff.size() ; ++k ) {
    coeff[k] = coeff[k-1] * z / (double(k));
  }
  //std::copy(std::begin(coeff),std::end(coeff),std::ostream_iterator<std::complex<double>>(std::cout," . "));
  //std::cout << std::endl;

  if ( z != 0. ) {
    return (std::exp(z) - std::accumulate( std::begin(coeff) , std::end(coeff) , std::complex<double>(0.,0.) ))/(std::pow(z,i));
  }
  return coeff[i-1];
}

namespace o2 {
  template < typename _T , std::size_t NumDimsV >
  auto
  trp_v ( field<_T,NumDimsV> const & u , ublas::vector<_T> const& E )
  {
    field<_T,NumDimsV> trp(tools::array_view<const std::size_t>(u.shape(),NumDimsV+1));

    { auto k=0, km1=trp.size(0)-1;
      for ( auto i=0 ; i<trp.size(1) ; ++i ) {
        trp[k][i] = ( E(i)*(u[k+1][i]-u[km1][i])/(2.*u.step.dv) );
      }
    }
    for ( auto k=1 ; k<trp.size(0)-1 ; ++k ) {
      for ( auto i=0 ; i<trp.size(1) ; ++i ) {
        trp[k][i] = ( E(i)*(u[k+1][i]-u[k-1][i])/(2.*u.step.dv) );
      }
    }
    { auto k=trp.size(0)-1, kp1=0;
      for ( auto i=0 ; i<trp.size(1) ; ++i ) {
        trp[k][i] = ( E(i)*(u[kp1][i]-u[k-1][i])/(2.*u.step.dv) );
      }
    }

    return trp;
  }
}

namespace math = boost::math::constants;
const std::complex<double> & I = std::complex<double>(0.,1.);

#define SQ(X) ((X)*(X))
#define Xi(i) (i*f.step.dx+f.range.x_min)
#define Vk(k) (k*f.step.dv+f.range.v_min)

auto
maxwellian ( double rho , double u , double T ) {
  //std::cout << rho << " " << u << " " << T << std::endl;
  //std::cout << rho/(std::sqrt(2.*math::pi<double>()*T)) << std::endl;
  return [=](double x,double v){ return rho/(std::sqrt(2.*math::pi<double>()*T))*std::exp( -0.5*SQ(v-u)/T ); };
}

int main(int,char**)
{
	std::size_t Nx = 135, Nv = 256 , Nb_iter=10;
	field<double,1> f(boost::extents[Nv][Nx]);

	f.range.v_min = -8.; f.range.v_max = 8.;
	f.step.dv = (f.range.v_max-f.range.v_min)/Nv;

  const double Kx = 0.5;
	//f.range.x_min = 0.; f.range.x_max = 20.*math::pi<double>();
  f.range.x_min = 0.; f.range.x_max = 2./Kx*math::pi<double>(); //10.0*math::pi<double>();
  //f.range.x_min = -8.; f.range.x_max = 8.;
	f.step.dx = (f.range.x_max-f.range.x_min)/Nx;

  //field<double,1> f_sol = f;
  //field<double,1> f_ini = f;

	ublas::vector<double> v (Nv,0.);
  ublas::vector<double> E (Nx,0.),rho(Nx);
  for ( std::size_t k=0 ; k<Nv ; ++k ) { v[k] = Vk(k); }
  //for ( std::size_t i=0 ; i<Nx ; ++i ) { E[i] = -Xi(i); }

	const double l = f.range.x_max-f.range.x_min;
	ublas::vector<double> kx(Nx);
	//for ( auto i=0 ; i<Nx/2+1 ; ++i )   { kx[i] = 2.*math::pi<double>()*i/l; }
	//for ( auto i=0 ; i<((Nx/2)) ; ++i ) { kx[i+Nx/2+1] = -kx[Nx/2-i]; }
  for ( auto i=0 ; i<Nx/2 ; ++i ) { kx[i]    = 2.*math::pi<double>()*i/l; }
  for ( int i=-Nx/2 ; i<0 ; ++i ) { kx[Nx+i] = 2.*math::pi<double>()*i/l; }

  double np = 0.9 , nb = 0.2 , ui = 4.5;
  double alpha = 0.1;
  double Tc = 0.01; //, u = 4.5;
  //auto Mmu = maxwellian(1.,-u,1.) , Mpu = maxwellian(1.,u,1.) , M0c = maxwellian(1.,0.,Tc);

  ui = 2.;
  alpha = 0.2;
  Tc = 0.01;
  auto landau_M = maxwellian(1.,0.,1.);
  auto db_M1 = maxwellian(0.5,-ui,1.) , db_M2 = maxwellian(0.5,ui,1.);
  auto bot_M1 = maxwellian(1.-alpha,0.,1.) , bot_M2 = maxwellian(alpha,ui,0.25);
  auto tb_MC = maxwellian(1.-alpha,0.,Tc) , tb_M1 = maxwellian(0.5*alpha,ui,1.) , tb_M2 = maxwellian(0.5*alpha,-ui,1.);
  auto v10_MC = maxwellian(1.-alpha,0,Tc) , v10_Mh = maxwellian(alpha,0.,1.);

  for (field<double,2>::size_type k=0 ; k<f.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<f.size(1) ; ++i ) {
      //f[k][i] = ((1.-alpha)*M0c(Xi(i),Vk(k)) + 0.5*alpha*( Mpu(Xi(i),Vk(k)) + Mmu(Xi(i),Vk(k)) ))*(1.+0.01*std::cos(0.5*Xi(i)));

      //// landau damping : Kx=0.5
      //f[k][i] = landau_M(Xi(i),Vk(k))*(1.+0.001*std::cos(Kx*Xi(i)));
      //// double beam Kx=0.2, ui=2.4 ou Kx=0.2, ui=4.5
      //f[k][i] = (db_M1(Xi(i),Vk(k))+db_M2(Xi(i),Vk(k)))*(1.+0.001*std::cos(Kx*Xi(i)));
      //// bot Kx=0.5 , alpha=0.2 , ui=4.5
      //f[k][i] = (bot_M1(Xi(i),Vk(k)) + bot_M2(Xi(i),Vk(k)))*(1.+0.01*std::cos(Kx*Xi(i)));
      //// tb Kx=0.5 , ui=4. , alpha=0.2 , Tc=0.01
      f[k][i] = tb_MC(Xi(i),Vk(k)) + (tb_M1(Xi(i),Vk(k)) + tb_M2(Xi(i),Vk(k)) )*(1. + 0.01*std::cos(Kx*Xi(i)));
      //// v10 Kx=0.5 , alpha=0.2
      //f[k][i] = v10_MC(Xi(i),Vk(k)) + ( std::pow(Vk(k),10)*v10_Mh(Xi(i),Vk(k))/945. )*(1. + 0.01*std::cos(Kx*Xi(i)));
    }
  }
  f.write("vphl/kin/init_tb.dat");

  poisson<double> poisson_solver(Nx,l);
  rho = f.density();
  E = poisson_solver(rho);

  //double Tf = 60.;//2*math::pi<double>();
  const double Tf = 200.;
  int i_t=0;

  double current_time = 0.;
  // SIGMA is the CFL number
  double dt = f.step.dv;//std::min( 0.1 , SIGMA*f.step.dv/std::abs(*std::max_element( E.begin() , E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );

  std::cout << "Nx: " << Nx << "\n";
  std::cout << "Nv: " << Nv << "\n";
  std::cout << "v_min: " << f.range.v_min << "\n";
  std::cout << "v_max: " << f.range.v_max << "\n";
  std::cout << "x_min: " << f.range.x_min << "\n";
  std::cout << "x_max: " << f.range.x_max << "\n";
  std::cout << "dt: " << dt << "\n";
  std::cout << "dx: " << f.step.dx << "\n";
  std::cout << "dv: " << f.step.dv << "\n";
  std::cout << "Tf: " << Tf << "\n";
  std::cout << "f_0: " << "\"tb\"" << "\n";
  std::cout << std::endl;

  std::ofstream info("info.yaml");

  info << "Nx: " << Nx << "\n";
  info << "Nv: " << Nv << "\n";
  info << "v_min: " << f.range.v_min << "\n";
  info << "v_max: " << f.range.v_max << "\n";
  info << "x_min: " << f.range.x_min << "\n";
  info << "x_max: " << f.range.x_max << "\n";
  info << "dt: " << dt << "\n";
  info << "dx: " << f.step.dx << "\n";
  info << "dv: " << f.step.dv << "\n";
  info << "Tf: " << Tf << "\n";
  info << "f_0: " << "\"tb\"" << "\n";
  info << std::endl;
  info.close();

  std::vector<double> ee;   ee.reserve(int(std::ceil(Tf/dt))+1);
  std::vector<double> Emax; Emax.reserve(int(std::ceil(Tf/dt))+1);
  std::vector<double> H;    H.reserve(int(std::ceil(Tf/dt))+1);

  std::vector<double> times; times.reserve(int(std::ceil(Tf/dt))+1);

  // space scheme
  auto weno = [&](field<double,1>const& f , ublas::vector<double> const& E )->field<double,1> { return weno::trp_v(f,E); };
  auto cd2 = [&](field<double,1>const& f , ublas::vector<double> const& E )->field<double,1> { return o2::trp_v(f,E); };

  // time scheme init
  lawson::RK33<poisson<double>> rk(Nx,Nv,l,f.shape(),v,kx,weno);

  rk.E = rk.poisson_solver(f.density());
  Emax.push_back( std::abs(*std::max_element( rk.E.begin() , rk.E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );
  double electric_energy = 0.;
  for ( const auto & ei : rk.E ) { electric_energy += ei*ei*f.step.dx; }
  ee.push_back( std::sqrt(electric_energy) );
  H.push_back( energy(f,rk.E) );
  times.push_back( 0. );

  while (  current_time < Tf ) {
    std::cout<<" ["<<std::setw(5)<<i_t<<"] "<< current_time <<"\r"<<std::flush;

    f = rk(f,dt);

    // end of time loop

    // MONITORING
    rk.E = rk.poisson_solver(f.density());
    Emax.push_back( std::abs(*std::max_element( rk.E.begin() , rk.E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );
    electric_energy = 0.;
    for ( const auto & ei : rk.E ) { electric_energy += ei*ei*f.step.dx; }
    ee.push_back( std::sqrt(electric_energy) );
    H.push_back( energy(f,rk.E) );

    dt = std::min( 0.1 , SIGMA*f.step.dv/Emax[i_t] );

    // increment time
    ++i_t;
    current_time += dt;
    times.push_back( current_time );
	} // while (  i_t*dt < Tf )

//#define FOLDER "lukas/landau/"
#define FOLDER "vphl/kin/"
#define SPACE_SCHEME "weno"

  std::cout<<" ["<<std::setw(5)<<i_t<<"] "<<i_t*dt <<std::endl;

  std::stringstream ss; ss << FOLDER << "vp_" << rk.label << "_" << SPACE_SCHEME << ".dat";
  f.write("vphl/kin/vp_tb.dat");

  ss.str(std::string());
  std::ofstream of;
  std::size_t count = 0;
  auto dt_y = [&,count=0](auto const& y) mutable { std::stringstream ss; ss<<times[count++]<<" "<<y; return ss.str(); };
  //ss << FOLDER << "ee_" << rk.label << "_" << SPACE_SCHEME << ".dat";
  of.open("vphl/kin/ee_tb.dat"); ss.str(std::string());
  for ( auto i=0; i<ee.size() ; ++i ) {
    of << times[i] <<" " << ee[i] << "\n";
  }
  of.close();
  //ss << FOLDER << "H_" << rk.label << "_" << SPACE_SCHEME << ".dat";
  of.open("vphl/kin/H_tb.dat"); ss.str(std::string());
  for ( auto i=0; i<H.size() ; ++i ) {
    of << times[i] <<" " << (H[i]-H[0])/std::abs(H[0]) << "\n";
  }

  //double h = std::abs(*std::max_element( H.begin() , H.end() , [&](double a,double b){return ( std::abs((a-H[0])/std::abs(H[0])) < std::abs((b-H[0])/std::abs(H[0])) );} ));
  //std::cout << dt << " " << std::abs((h-H[0])/std::abs(H[0])) << "\n";

  of.close();
  //ss << FOLDER << "Emax_" << rk.label << "_" << SPACE_SCHEME << ".dat";
  of.open("vphl/kin/Emax_tb.dat"); ss.str(std::string());
  std::transform( Emax.begin() , Emax.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();

	return 0;
}


/*
 * LibHEOM: Copyright (c) Tatsushi Ikeda
 * This library is distributed under BSD 3-Clause License.
 * See LINCENSE.txt for licence.
 *------------------------------------------------------------------------*/

// Note: In order to avoid compile errors caused by the two phase name
//       lookup rule regarding template class, all class variables in
//       this source have this-> modifier.

#include "redfield.h"

#include <Eigen/SparseLU>

#include "const.h"
// #include "mkl_wrapper.h"

namespace libheom
{

// calculate Fourier-Laplace transform of quantum correlation function.
// The exponent is +1j*omega*t.
template<typename T>
inline T correlation
/**/(redfield<T>& rf,
     int u,
     real_t<T> omega)
{
  if (rf.use_corr_func && rf.use_corr_func[u]) {
    return rf.corr_func[u](omega);
  } else {

    Eigen::SparseMatrix<T, Eigen::RowMajor> I(rf.len_gamma[u],rf.len_gamma[u]);
    I.setIdentity();
    Eigen::SparseLU<Eigen::SparseMatrix<T, Eigen::RowMajor>> solver;
    solver.compute(rf.gamma[u] - i_unit<T>()*omega*I);
    if (solver.info() != Eigen::Success) {
      std::cerr << "[Error] LU decomposition failed. " << std::endl;
      std::exit(1);
      return zero<T>();
    }
    return static_cast<T>(rf.sigma[u].transpose()*(rf.s[u] + i_unit<T>()*rf.a[u])*solver.solve(rf.phi_0[u])) + rf.S_delta[u];

  }
}

template<typename T>
void redfield<T>::init_aux_vars
/**/()
{
  qme<T>::init_aux_vars();

  this->Lambda.reset(new lil_matrix<T>[this->n_noise]);
  for (int s = 0; s < this->n_noise; ++s) {
    this->Lambda[s].set_shape(this->n_state, this->n_state);
    for (auto& V_ijv : this->V[s].data) {
      int i = V_ijv.first;
      for (auto& V_jv: V_ijv.second) {
        int j = V_jv.first;
        T val = V_jv.second;
        if (val != zero<T>()) {
          real_t<T> omega_ji;
          try {
            omega_ji = std::real(this->H.data[j][j] - this->H.data[i][i]);
          } catch (std::out_of_range&) {
            continue;
          }
          T corr = correlation(*this, s, omega_ji);
          this->Lambda[s].data[i][j] += val*corr;
        }
      }
    }
  }
}

//========================================================================
// redfield Module (Hilbert space expression)
//========================================================================

template<typename T,
         template <typename, int> class matrix_type,
         int num_state>
void redfield_h<T, matrix_type, num_state>::init_aux_vars
/**/()
{
  redfield<T>::init_aux_vars();

  if (this->secular) {
    std::cerr << "[Error] Secular approximation is supported only in Liouville space." << std::endl;
    std::exit(1);
  }

  this->H.template dump<num_state>(this->H_impl);
  matrix_type<T, num_state> H_c_impl;
  this->H_c.template dump<num_state>(H_c_impl);
  this->H_impl += H_c_impl;

  this->V_impl.reset(new matrix_hilb[this->n_noise]);
  this->Lambda_impl.reset(new matrix_hilb[this->n_noise]);
  this->Lambda_dagger_impl.reset(new matrix_hilb[this->n_noise]);

  for (int s = 0; s < this->n_noise; ++s) {
    this->V[s].template dump<num_state>(this->V_impl[s]);
    this->Lambda[s].template dump<num_state>(this->Lambda_impl[s]);
    auto tmp = this->Lambda[s].hermite_conjugate();
    tmp.template dump<num_state>(this->Lambda_dagger_impl[s]);
  }
}

// template<typename T, template <typename> class matrix_type>
// void redfield_h<T, matrix_type>::ConstructCommutator(
//     lil_matrix<T>& x,
//     T coef_l,
//     T coef_r,
//     std::function<void(int)> callback,
//     int interval_callback) {
//   this->X_impl = static_cast<matrix_type<T>>(this->x);
//   this->coef_l_X = coef_l;
//   this->coef_r_X = coef_r;
// }

template<typename T,
         template <typename, int> class matrix_type,
         int num_state>
void redfield_h<T, matrix_type, num_state>::calc_diff
/**/(ref<dense_vector<T,Eigen::Dynamic>>              drho_dt_raw,
     const ref<const dense_vector<T,Eigen::Dynamic>>& rho_raw,
     real_t<T> alpha,
     real_t<T> beta)
{
  auto n_state = this->n_state;

  auto rho     = Eigen::Map<const dense_matrix<T,num_state>>(rho_raw.data(),n_state,n_state);
  auto drho_dt = Eigen::Map<dense_matrix<T,num_state>>(drho_dt_raw.data(),n_state,n_state);
  dense_matrix<T,num_state> tmp(n_state,n_state);

  drho_dt *= beta;
  drho_dt.noalias() += -alpha*i_unit<T>()*this->H_impl*rho;
  drho_dt.noalias() += +alpha*i_unit<T>()*rho*this->H_impl;

  for (int s = 0; s < this->n_noise; ++s) {
    tmp.noalias()  = +i_unit<T>()*this->Lambda_impl[s]*rho;
    tmp.noalias() += -i_unit<T>()*rho*this->Lambda_dagger_impl[s];
    drho_dt.noalias() += alpha*i_unit<T>()*this->V_impl[s]*tmp;
    drho_dt.noalias() -= alpha*i_unit<T>()*tmp*this->V_impl[s];
  }
}

// template<typename T, template <typename, int> class matrix_type, int num_state>
// void redfield_h<T, matrix_type, num_state>::ApplyCommutator(T* rho_raw) {
//   DenseMatrixWrapper<T> rho(this->n_state, this->n_state, rho_raw);
//   DenseMatrixWrapper<T> sub(this->n_state, this->n_state, this->sub_vector.data());

//   gemm(this->coef_l_X, this->X_impl, rho, zero<T>(), sub);
//   gemm(this->coef_r_X, rho, this->X_impl, one <T>(), sub);
// }

//========================================================================
// redfield Module (Liouville space expression)
//========================================================================

template<typename T,
         template <typename, int> class matrix_type,
         int num_state>
void redfield_l<T, matrix_type, num_state>::init_aux_vars
/**/()
{
  redfield<T>::init_aux_vars();

  this->n_state_liou = this->n_state*this->n_state;

  this->L.set_shape(this->n_state_liou, this->n_state_liou);
  kron_identity_right(+i_unit<T>(), this->H, zero<T>(), this->L);
  kron_identity_left (-i_unit<T>(), this->H, one<T>(), this->L);

  this->Phi.  reset(new lil_matrix<T>[this->n_noise]);
  this->Theta.reset(new lil_matrix<T>[this->n_noise]);

  for (int s = 0; s < this->n_noise; ++s){
    this->Phi[s].set_shape(this->n_state_liou, this->n_state_liou);
    kron_identity_right(+i_unit<T>(), this->V[s], zero<T>(), this->Phi[s]);
    kron_identity_left (-i_unit<T>(), this->V[s], one<T>(),  this->Phi[s]);

    this->Theta[s].set_shape(this->n_state_liou, this->n_state_liou);
    kron_identity_right(+i_unit<T>(), this->Lambda[s],
                        zero<T>(), this->Theta[s]);
    kron_identity_left (-i_unit<T>(), this->Lambda[s].hermite_conjugate(),
                        one<T>(),  this->Theta[s]);
  }

  this->R_redfield.set_shape(this->n_state_liou, this->n_state_liou);
  kron_identity_right(+i_unit<T>(), this->H_c, zero<T>(), this->R_redfield);
  kron_identity_left (-i_unit<T>(), this->H_c, one<T>(),  this->R_redfield);
  for (int s = 0; s < this->n_noise; ++s) {
    gemm(-one<T>(), this->Phi[s], this->Theta[s], one<T>(), this->R_redfield);
  }
  if (this->secular) {
    dense_matrix<T,num_state_liou> i_omega;
    this->L.optimize();
    this->L.template dump<num_state_liou>(i_omega);
    this->R_redfield.optimize();
    for (auto& ijv : this->R_redfield.data) {
      int i = ijv.first;
      for (auto& jv: ijv.second) {
        int j = jv.first;
        if (abs(i_omega(i,i) - i_omega(j,j))
            >= std::numeric_limits<typename T::value_type>::epsilon()) {
          jv.second = zero<T>();
        }
      }
    }
    this->R_redfield.optimize();
  }
  axpy(one<T>(), this->L, this->R_redfield);
  this->R_redfield.optimize();

  R_redfield.template dump<num_state_liou>(this->R_redfield_impl);
}

// template<typename T, template <typename> class matrix_type>
// void redfield_l<T, matrix_type>::ConstructCommutator(
//     lil_matrix<T>& x,
//     T coef_l,
//     T coef_r,
//     std::function<void(int)> callback,
//     int interval_callback) {
//   this->X_impl = static_cast<matrix_type<T>>(this->x);
//   this->coef_l_X = coef_l;
//   this->coef_r_X = coef_r;
// }

template<typename T,
         template <typename, int> class matrix_type,
         int num_state>
void redfield_l<T, matrix_type, num_state>::calc_diff
/**/(ref<dense_vector<T,Eigen::Dynamic>>              drho_dt_raw,
     const ref<const dense_vector<T,Eigen::Dynamic>>& rho_raw,
     real_t<T> alpha,
     real_t<T> beta)
{
  auto n_state_liou   = this->n_state_liou;
  auto rho     = blk<T,num_state_liou,1>::value(rho_raw,    0,0,n_state_liou,1);
  auto drho_dt = blk<T,num_state_liou,1>::value(drho_dt_raw,0,0,n_state_liou,1);

  drho_dt *= beta;
  drho_dt.noalias() += -alpha*this->R_redfield_impl*rho;
}

// template<typename T, template <typename, int> class matrix_type, int num_state>
// void redfield_l<T, matrix_type, num_state>::ApplyCommutator(T* rho_raw) {
//   DenseMatrixWrapper<T> rho(this->n_state, this->n_state, rho_raw);
//   DenseMatrixWrapper<T> sub(this->n_state, this->n_state, this->sub_vector.data());

//   gemm(this->coef_l_X, this->X_impl, rho, zero<T>(), sub);
//   gemm(this->coef_r_X, rho, this->X_impl, one <T>(), sub);
// }

}

// Explicit instantiations
namespace libheom
{

#define DECLARE_EXPLICIT_INSTANTIATIONS(qme_type, T, matrix_type, num_state) \
  template void qme_type<T, matrix_type, num_state>::init_aux_vars();        \
  template void qme_type<T, matrix_type, num_state>::calc_diff(              \
      ref<dense_vector<T, Eigen::Dynamic>> drho_dt,                          \
      const ref<const dense_vector<T, Eigen::Dynamic>>& rho,                 \
      real_t<T> alpha, real_t<T> beta);
// template void qme_type<T, matrix_type>::ConstructCommutator(            \
//     lil_matrix<T>& x,                                                  \
//     T coef_l,                                                         \
//     T coef_r,                                                         \
//     std::function<void(int)> callback,                                \
//     int interval_callback);                                           \
// template void qme_type<T, matrix_type>::ApplyCommutator(ref<dense_vector<T>> rho);

DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex64,  dense_matrix, Eigen::Dynamic);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex64,  csr_matrix,   Eigen::Dynamic);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex128, dense_matrix, Eigen::Dynamic);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex128, csr_matrix,   Eigen::Dynamic);

DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex64,  dense_matrix, Eigen::Dynamic);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex64,  csr_matrix,   Eigen::Dynamic);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex128, dense_matrix, Eigen::Dynamic);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex128, csr_matrix,   Eigen::Dynamic);

DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex64,  dense_matrix, 2);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex64,  csr_matrix,   2);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex128, dense_matrix, 2);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex128, csr_matrix,   2);

DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex64,  dense_matrix, 2);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex64,  csr_matrix,   2);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex128, dense_matrix, 2);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex128, csr_matrix,   2);

DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex64,  dense_matrix, 3);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex64,  csr_matrix,   3);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex128, dense_matrix, 3);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex128, csr_matrix,   3);

DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex64,  dense_matrix, 3);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex64,  csr_matrix,   3);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex128, dense_matrix, 3);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex128, csr_matrix,   3);

}


/*

   Recon Code for Cartesian and Non-Cartesian Data
   (in process of reorganizing for template + 4D recons)

 */

#include "ArrayTemplates.cpp"
#include "tictoc.hpp"
#include <omp.h>
#include <armadillo>
#include <cmath>

using namespace std;
using namespace NDarray;

int dideal_recon_2D_CG(
	Array< float,2 >&kx,
	Array< float,2 >&ky,
	Array< complex<float>,2 >&kdata,
	Array< float,3 >&fieldmap,
	Array< float,2 >&ktimes,
	float *freqs,
	int Ns,
	float lambda_space,
	float lambda_time,
	int max_iter);

int dideal_recon_2D_Gradient(
	Array< float,2 >&kx,
	Array< float,2 >&ky,
	Array< complex<float>,2 >&kdata,
	Array< float,3 >&fieldmap,
	Array< float,2 >&ktimes,
	float *freqs,
	int Ns,
	float lambda_space,
	float lambda_time,
	float lambda_lowrank,
	int max_iter);

void lowrank_thresh( Array< complex<float>, 4> &image, float thresh);

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

	// Hard Coded
	int Nt = 20;
	int Nr = 6692*2;
	int Ny = 128;
	int Nx = 128;
	int Ns = 3;
	int max_iter = 50;
	float lambda_space= 0.000;
	float lambda_time = 0.000;
	float lambda_lowrank = 0.000;
	float *freqs = new float[16];
	freqs[0]=-42;
	freqs[1]=-653;
	freqs[2]=-462;
	freqs[3]=-302;

#define float_flag(name,val)    }else if(strcmp(name,argv[pos]) == 0){ pos++; val = atof(argv[pos]);
#define int_flag(name,val)    }else if(strcmp(name,argv[pos]) == 0){ pos++; val = atoi(argv[pos]);
	for(int pos=0; pos < argc; pos++){
		if (strcmp("-h", argv[pos] ) == 0) {

			int_flag("-Nt",Nt);
			int_flag("-Nr",Nr);
			int_flag("-Ny",Ny);
			int_flag("-Nx",Nx);
			int_flag("-Ns",Ns);
			int_flag("-max_iter",max_iter);
			float_flag("-lambda_space",lambda_space);
			float_flag("-lambda_time",lambda_time);
			float_flag("-lambda_lowrank",lambda_lowrank);
			float_flag("-f0",freqs[0]);
			float_flag("-f1",freqs[1]);
			float_flag("-f2",freqs[2]);
			float_flag("-f3",freqs[3]);
			float_flag("-f4",freqs[4]);
			float_flag("-f5",freqs[5]);
			float_flag("-f6",freqs[6]);
		}
	}

	cout << "Imaging Parameters" << endl;
	cout << "  Nx = " << Nx << endl;
	cout << "  Ny = " << Ny << endl;
	cout << "  Nt = " << Nt << endl;
	cout << "  Ns = " << Ns << endl;
	cout << "  Nr = " << Nr << endl;
	cout << "Recon Parameters" << endl;
	for(int sp = 0; sp <Ns; sp++){
	cout << "  f" << sp << " = " << freqs[sp] << endl;
	}
	cout << "  lambda_space = " << lambda_space << endl;
	cout << "  lambda_time = " << lambda_time << endl;
	cout << "  lambda_lowrank = " << lambda_lowrank << endl;
	cout << "  max_iter = " << max_iter << endl;

	// Allocate Memory and Copy Values
	cout << "Kx" << endl << flush;
	Array< float,2>kx(Nr,Nt,ColumnMajorArray<2>());
	ArrayRead(kx,"Kx.dat");
	kx*=(float)1.0 / (float)Nx;

	cout << "Ky" << endl << flush;
	Array< float,2>ky(Nr,Nt,ColumnMajorArray<2>());
	ArrayRead(ky,"Ky.dat");
	ky*=(float)1.0 / (float)Ny;

	cout << "Kt" << endl << flush;
	Array< float,2>kt(Nr,Nt,ColumnMajorArray<2>());
	ArrayRead(kt,"Kt.dat");

	cout << "Kdata" << endl << flush;
	Array< complex<float> ,2>kdata(Nr,Nt,ColumnMajorArray<2>());
	ArrayRead(kdata,"Kdata.dat");

	cout << "Fieldmap" << endl << flush;
	Array< float,3>fieldmap(Nx,Ny,Nt,ColumnMajorArray<3>());
	ArrayRead(fieldmap,"FieldMap.dat");

	if(lambda_lowrank == 0){
		dideal_recon_2D_CG( kx, ky, kdata, fieldmap, kt, freqs, Ns,lambda_space,lambda_time,max_iter);
	}else{
		dideal_recon_2D_Gradient( kx, ky, kdata, fieldmap, kt, freqs, Ns,lambda_space,lambda_time,lambda_lowrank,max_iter);
	}

	return(0);
}

// Generate Images from Kdata
void transpose_dideal(
	const Array< complex<float>,2 > &diff_data,
	Array< complex<float>,4 > &X,
	const Array< float, 2 > &kx,
	const Array< float, 2 > &ky,
	const Array< float, 2 > &kt,
	const Array< float, 3 > &fieldmap,
	float *freqs){

	int Ns = X.length(fourthDim);
	int Nt = diff_data.length(secondDim); // Time Frame
	int Nr = diff_data.length(firstDim);  // Readout position

	// Spatial Coordinates
	int Nx = fieldmap.length(firstDim);
	int Ny = fieldmap.length(secondDim);
	float cx = (float)Ny/2.0;
	float cy = (float)Nx/2.0;
	const float pic=-6.28318530718;

	X = 0;

	float fov = (float)(Nx*Nx)/(4.0);
	// Loop Kx/Ky
	tictoc T;
	T.tic();

#pragma omp parallel for schedule(static,1)
	for(int j=0; j<Ny; j++){

		// cout << "," << j << flush;
		float y = (float)j - cy;

		for(int i=0; i<Nx; i++){
			float x = (float)i - cx;
			float rad = x*x + y*y;
			if( rad > fov) continue;

			for(int species=0; species< Ns; species++){
				for(int t=0; t< Nt; t++){
					float Pf = freqs[species]+fieldmap(i,j,t);
					complex<float>temp(0.0,0.0);
					for(int rpos=0; rpos< Nr; rpos++){
						// Loop over image
						complex<float>basis = polar<float>((float)1.0, pic*(kx(rpos,t)*x + ky(rpos,t)*y + kt(rpos,t)*Pf));
						temp+= ( diff_data(rpos,t)*basis);
					}
					X(i,j,t,species)=temp;	// This takes all the time
				}// Nt
			}
		}
	}//Image Loop
	cout << "Transpose Took = " << T << endl;
}

// Generate Images from Kdata
void forward_dideal(
	Array< complex<float>,2 > &diff_data,
	const Array< complex<float>,4 > &X,
	const Array< float, 2 > &kx,
	const Array< float, 2 > &ky,
	const Array< float, 2 > &kt,
	const Array< float, 3 > &fieldmap,
	float *freqs){

	int Ns = X.length(fourthDim);
	int Nt = diff_data.length(secondDim); // Time Frame
	int Nr = diff_data.length(firstDim);  // Readout position

	// Spatial Coordinates
	int Nx = fieldmap.length(firstDim);
	int Ny = fieldmap.length(secondDim);
	float cx = (float)Ny/2.0;
	float cy = (float)Nx/2.0;
	const float pic= 6.28318530718;

	diff_data = 0;

	float fov = (float)(Nx*Nx)/(4.0);
	// Loop Kx/Ky

	tictoc T;
	T.tic();

#pragma omp parallel for
	for(int rpos=0; rpos< Nr; rpos++){
		for(int t=0; t< Nt; t++){

			// Position in k-t space
			float Kx = kx(rpos,t);
			float Ky = ky(rpos,t);
			float Kt = kt(rpos,t);

			// Loop over image
			for(int j=0; j<Ny; j++){
				float y = ((float)j - cy);
				float Py = y*Ky;
				for(int i=0; i<Nx; i++){
					float x =((float)i - cx);
					float Px = x*Kx;

					float rad = x*x + y*y;
					if( rad > fov) continue;
					for(int species=0; species< Ns; species++){
						complex<float>basis = polar<float>((float)1.0, pic*(Px + Py + Kt*( freqs[species]+fieldmap(i,j,t) )));
						diff_data(rpos,t) += (X(i,j,t,species)*basis);
					}
			}}//Image Loop

	}}// K-t  + Species Looop
	cout << "Forward Took = " << T << endl;
}

int dideal_recon_2D_CG(
	Array< float,2 >&kx,
	Array< float,2 >&ky,
	Array< complex<float>,2 >&kdata,
	Array< float,3 >&fieldmap,
	Array< float,2 >&ktimes,
	float *freqs,
	int Ns,
	float lambda_space,
	float lambda_time,
	int max_iter){

	// Inputs
	//   kx      	[Nr,Nt ]          = Matrix with Nt readouts of length Nr
	//   ky      	[Nr,Nt ]          = Matrix with Nt readouts of length Nr
	//   kdata   	[Nr,Nt ]   		  = Matrix with Nt readouts of length Nr x Coils
	//   fieldmap	[ResX,ResY,Nt]    = Fieldmap
	//   kt      	[Nr,Nt]           = Readout times
	//   freqs   	[Ns x 1]          = Offsets for Species
	//   max_iter   [scalar]     		maximum iterations

	// Dimensions of Problem
	int Nr = ky.length(firstDim);
	int Nt = ky.length(secondDim);
	int Ny = fieldmap.length(firstDim);
	int Nx = fieldmap.length(secondDim);
	cout << "Array Size = " << Nx << " x " << Ny << " x " << Nt << " x " << Ns << endl;

	// ------------------------------------
	// Iterative Soft Thresholding  x(n+1)=  thresh(   x(n) - E*(Ex(n) - d)  )
	//  Designed to not use memory
	// Uses gradient descent x(n+1) = x(n) - ( R'R ) / ( R'E'E R) * Grad  [ R = E'(Ex-d)]
	// ------------------------------------

	// Final Image Solution
	Array< complex<float>,4 >X(Nx,Ny,Nt,Ns,ColumnMajorArray<4>());
	X = 0;

	// Residue
	Array< complex<float>,4 >R(Nx,Ny,Nt,Ns,ColumnMajorArray<4>());
	R = 0;;

	// Residue
	Array< complex<float>,4 >Reg(Nx,Ny,Nt,Ns,ColumnMajorArray<4>());
	Reg = 0;

	// Temp variable for E'ER
	Array< complex<float>,4 >P(Nx,Ny,Nt,Ns,ColumnMajorArray<4>());
	P = 0;;

	// Temp variable for E'ER
	Array< complex<float>,2 >diff_data(Nr,Nt,ColumnMajorArray<2>());
	diff_data = 0;

	// RHS (CG)
	Array< complex<float>,4 >LHS(Nx,Ny,Nt,Ns,ColumnMajorArray<4>());

	cout << "Init CG" << endl;

	LHS= complex<float>(0,0);
	R= complex<float>(0,0);
	P= complex<float>(0,0);

	transpose_dideal(kdata,R,kx,ky,ktimes,fieldmap,freqs);
	R = -R;
	P = R;

	// Conjugate Gradient
	float error0=0.0;
	float reg_scale2=0.0;
	for(int iteration =0; iteration< max_iter; iteration++){

		cout << "\nIteration = " << iteration << endl;

		diff_data = complex<float>(0,0);
		forward_dideal(diff_data,P,kx,ky,ktimes,fieldmap,freqs);

		LHS = complex<float>(0,0);
		transpose_dideal(diff_data,LHS,kx,ky,ktimes,fieldmap,freqs);

		// Convolve with TV
		Reg = complex<float>(0.0,0.0);
		for(int sp=0; sp<Ns; sp++){
			for(int t=0;t<Nt;t++){
				for(int j=0;j<Ny;j++){
					for(int i=0;i<Nx;i++){

						// Space
						Reg(i,j,t,sp)=complex<float>(4.0,0)*P(i,j,t,sp);
						Reg(i,j,t,sp)-=P( (i+1+Nx)%Nx,j,t,sp);
						Reg(i,j,t,sp)-=P( (i-1+Nx)%Nx,j,t,sp);
						Reg(i,j,t,sp)-=P( i,(j+1+Ny)%Ny,t,sp);
						Reg(i,j,t,sp)-=P( i,(j-1+Ny)%Ny,t,sp);

						//Time
						Reg(i,j,t,sp) +=lambda_time*complex<float>(2.0,0)*P(i,j,t,sp);
						Reg(i,j,t,sp) -=lambda_time*P(i,j,(Nt+t-1)%Nt,sp);
						Reg(i,j,t,sp) -=lambda_time*P(i,j,(Nt+t+1)%Nt,sp);
		}}}}

		if(iteration==0){
			error0 = ArrayEnergy(R);
			reg_scale2 = lambda_space*sqrt( error0 );
		}
		Reg *= reg_scale2;
		LHS += Reg;

		complex< float> sum_R0_R0(0.0,0.0);
		complex< float> sum_R_R(0.0,0.0);
		complex< float> sum_P_LHS(0.0,0.0);

		// Calc R'R and P'*LHS
		for(int sp=0; sp<Ns; sp++){
			for(int t=0;t<Nt;t++){
				for(int j=0;j<Ny;j++){
					for(int i=0;i<Nx;i++){
						sum_R0_R0 += norm( R(i,j,t,sp));
						sum_P_LHS += conj( P(i,j,t,sp))*LHS(i,j,t,sp);
		}}}}
		complex< float> scale = sum_R0_R0 / sum_P_LHS;

		// Calc R'R and P'*LHS
		for(int sp=0; sp<Ns; sp++){
			for(int t=0;t<Nt;t++){
				for(int j=0;j<Ny;j++){
					for(int i=0;i<Nx;i++){
						X(i,j,t,sp) += ( scale*P(i,j,t,sp) );
						R(i,j,t,sp) -= ( scale*LHS(i,j,t,sp) );
						sum_R_R += norm( R(i,j,t,sp) );
		}}}}

		cout << "Sum R'R = " << sum_R_R << endl;
		complex< float> scale2 = sum_R_R / sum_R0_R0;

		// Take step size
		for(int sp=0; sp<Ns; sp++){
			for(int t=0;t<Nt;t++){
				for(int j=0;j<Ny;j++){
					for(int i=0;i<Nx;i++){
						P(i,j,t,sp) = R(i,j,t,sp) + scale2*P(i,j,t,sp);
		}}}}

		char fname[80];
		for(int sp=0; sp<Ns; sp++){
			sprintf(fname,"Species_%d.dat",sp);
			Array< complex<float>,3> TempS = X( Range::all(),Range::all(),Range::all(),sp);
			ArrayWrite( TempS,fname);
		}
	}

	return(0);
}

int dideal_recon_2D_Gradient(
	Array< float,2 >&kx,
	Array< float,2 >&ky,
	Array< complex<float>,2 >&kdata,
	Array< float,3 >&fieldmap,
	Array< float,2 >&ktimes,
	float *freqs,
	int Ns,
	float lambda_space,
	float lambda_time,
	float lambda_lowrank,
	int max_iter){

	// Inputs
	//   kx      	[Nr,Nt ]          = Matrix with Nt readouts of length Nr
	//   ky      	[Nr,Nt ]          = Matrix with Nt readouts of length Nr
	//   kdata   	[Nr,Nt ]   		  = Matrix with Nt readouts of length Nr x Coils
	//   fieldmap	[ResX,ResY,Nt]    = Fieldmap
	//   kt      	[Nr,Nt]           = Readout times
	//   freqs   	[Ns x 1]          = Offsets for Species
	//   max_iter   [scalar]     		maximum iterations

	// Dimensions of Problem
	int Nr = ky.length(firstDim);
	int Nt = ky.length(secondDim);
	int Ny = fieldmap.length(firstDim);
	int Nx = fieldmap.length(secondDim);
	cout << "Array Size = " << Nx << " x " << Ny << " x " << Nt << " x " << Ns << endl;

	// ------------------------------------
	// Iterative Soft Thresholding  x(n+1)=  thresh(   x(n) - E*(Ex(n) - d)  )
	//  Designed to not use memory
	// Uses gradient descent x(n+1) = x(n) - ( R'R ) / ( R'E'E R) * Grad  [ R = E'(Ex-d)]
	// ------------------------------------

	// Final Image Solution
	Array< complex<float>,4 >X(Nx,Ny,Nt,Ns,ColumnMajorArray<4>());
	Array< complex<float>,4 >R(Nx,Ny,Nt,Ns,ColumnMajorArray<4>());
	Array< complex<float>,4 >Reg(Nx,Ny,Nt,Ns,ColumnMajorArray<4>());
	Array< complex<float>,2 >diff_data(Nr,Nt,ColumnMajorArray<2>());
	Array< complex<float>,4 >P(Nx,Ny,Nt,Ns,ColumnMajorArray<4>());

	R= complex<float>(0,0);
	X= complex<float>(0,0);

	// Conjugate Gradient
	float error0=0.0;
	float reg_scale2=0.0;
	for(int iteration =0; iteration< max_iter; iteration++){

		cout << "Iteration = " << iteration << endl;

		// Get data
		diff_data = complex<float>(0,0);
		forward_dideal(diff_data,X,kx,ky,ktimes,fieldmap,freqs);

		// Difference
		diff_data -= kdata;

		// Get Residue
		R= complex<float>(0,0);
		transpose_dideal(diff_data,R,kx,ky,ktimes,fieldmap,freqs);

		// Convolve with TV
		Reg = complex<float>(0.0,0.0);
		for(int sp=0; sp<Ns; sp++){
			for(int t=0;t<Nt;t++){
				for(int j=0;j<Ny;j++){
					for(int i=0;i<Nx;i++){

						// Space
						Reg(i,j,t,sp)=lambda_space*complex<float>(4.0,0)*X(i,j,t,sp);
						Reg(i,j,t,sp)-=lambda_space*X( (i+1+Nx)%Nx,j,t,sp);
						Reg(i,j,t,sp)-=lambda_space*X( (i-1+Nx)%Nx,j,t,sp);
						Reg(i,j,t,sp)-=lambda_space*X( i,(j+1+Ny)%Ny,t,sp);
						Reg(i,j,t,sp)-=lambda_space*X( i,(j-1+Ny)%Ny,t,sp);

						//Time
						Reg(i,j,t,sp) +=lambda_time*complex<float>(2.0,0)*X(i,j,t,sp);
						Reg(i,j,t,sp) -=lambda_time*X(i,j,(Nt+t-1)%Nt,sp);
						Reg(i,j,t,sp) -=lambda_time*X(i,j,(Nt+t+1)%Nt,sp);

		}}}}

		if(iteration==0){
			error0 = ArrayEnergy(R);
			reg_scale2 = sqrt( error0 );
		}
		Reg *= reg_scale2;
		R += Reg;

		// Now get Scale
		diff_data = complex<float>(0,0);
		forward_dideal(diff_data,R,kx,ky,ktimes,fieldmap,freqs);
		P = complex<float>(0,0);
		transpose_dideal(diff_data,P,kx,ky,ktimes,fieldmap,freqs);

		cout << "Energy = " << ArrayEnergy(R)/error0 << endl;

		P *= conj(R);
		cout << "RhR = " << ArrayEnergy(R) << endl;
		cout << "RhP = " << sum(P) << endl;

		complex<float>scale = ArrayEnergy(R)/sum(P);
		cout << "Scale = " << scale << endl;

		R *= scale;
		X -= R;

		{
			char fname[80];
			for(int sp=0; sp<Ns; sp++){
				sprintf(fname,"Species_%d.dat.slice",sp);
				Array< complex<float>,2> TempS = X( Range::all(),Range::all(),(int)(Nt/2),sp);
				ArrayWriteMagAppend(TempS,fname);
			}
		}

		if(lambda_lowrank > 0){
			lowrank_thresh(X,lambda_lowrank);
		}

		{
			char fname[80];
			for(int sp=0; sp<Ns; sp++){
				sprintf(fname,"Species_%d.dat.slice",sp);
				Array< complex<float>,2> TempS = X( Range::all(),Range::all(),(int)(Nt/2),sp);
				ArrayWriteMagAppend(TempS,fname);
			}
		}
	}

	char fname[80];
	for(int sp=0; sp<Ns; sp++){
		sprintf(fname,"Species_%d.dat",sp);
		Array< complex<float>,3> TempS = X( Range::all(),Range::all(),Range::all(),sp);
		ArrayWrite( TempS,fname);
	}

	return(0);
}

void lowrank_thresh( Array< complex<float>,4> &image, float thresh){

	int Nx = image.length(firstDim);
	int Ny = image.length(secondDim);
	int Nt = image.length(thirdDim);
	int Ns = image.length(fourthDim);
	int Np = Nx*Ny*Ns;

	// Copy image into matrix
	arma::cx_mat A;
	A.zeros(Nx*Ny*Ns,Nt);

	for(int t =0; t < Nt; t++){
		int count = 0;

		for(int s =0; s<Ns; s++){
		for(int j =0; j<Ny; j++){
		for(int i =0; i<Nx; i++){
			A(count,t) = image(i,j,t,s);
			count++;
		}}}
	}

	// SVD
	cout << "Svd" << endl;
	arma::cx_mat U;
	arma::vec s;
	arma::cx_mat V;
	arma::svd(U,s,V,A);

	arma::mat S;
	S.zeros(Np,Nt); // Pixels x Coils

	cout << "Thresh with " << thresh << endl;
	float smax = thresh*max(s);
	for(int pos =0; pos< min(Nt,Np); pos++){
		S(pos,pos)=   max( s(pos) - smax, 0.0 );
	}

	A = U*S*V.t();

	for(int t =0; t < Nt; t++){
		int count = 0;

		for(int s =0; s<Ns; s++){
		for(int j =0; j<Ny; j++){
		for(int i =0; i<Nx; i++){
			image(i,j,t,s) = A(count,t);
			count++;
		}}}
	}
}


//
// Copyright 2016 MIT Lincoln Laboratory, Massachusetts Institute of Technology
//
// Licensed under the Apache License, Version 2.0 (the "License"); you may not use these files except in compliance with
// the License.
//
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on
// an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
//

// MFCCs

// Refactored from LLSpeech 2.2.10 code & BC Speech Tools:
// filtbank.cc, mfb_to_cep.cc, mfb_to_cep_subs.cc, mfb_utils.cc, pcm_to_feat.cc, pcm_to_mfb_subs.cc, vec_utils.cc

// Written by BC 10/18/10
#include "stdafx.h"
#include <stdio.h>
#include <math.h>
#include "speech_tools.h"
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
using boost::property_tree::ptree;
using boost::property_tree::read_json;

#define PI 3.141592653589793

typedef float* flt_ptr;
static float warp (float freq, float alpha, float sfreq);

//
// Main processing routine
//
void MFCC_Features::process (Signal &x, string &config_str)
{

	// MFCC configuration
	config(x, config_str);

	// Set up storage
	Frame f(win_len, win_inc);
	mag_fft m(win_len);
	vec<REAL_FEAT> x_frame(win_len);
	vec<REAL_FFT> x_mag(n2_fft+1);
	vec<REAL_FFT> x_fb(num_filt_bl);
	vec<REAL_FEAT> x_cep(num_features);
	REAL_FEAT *feat_ptr;

	// Allocate space for features
	num_vectors = f.get_num_frames(x);
	feat_ptr = feat = new REAL_FEAT[num_vectors*num_features];
	energy = new REAL_FEAT[num_vectors];

	// Loop across frames
	int i, j;
	for (i=0; i<num_vectors; i++) {

		// Frame based pre-processing
		f.get_frame(x, i);
		f.dither(dither);
		f.rm_dc();
		f.hamming_window();
		energy[i] = f.energy();

		// FFT magnitudes -- FFT not quite the same as LLSpeech
		x_frame = f.vector();
		x_mag = m.calc(x_frame);

		// Pre-emphasis
		x_mag.scale(*preemp_filt);

		// Calculate filterbanks with bandlimiting
		x_fb = calc_fb(x_mag);

		if (fb_only) {
			for (j=0; j<num_features; j++, feat_ptr++)
				*feat_ptr = x_fb.data[j];
		} else {
			// Cepstral coefficients
			x_cep = icostrans(x_fb);
			for (j=0; j<num_features; j++, feat_ptr++)
				*feat_ptr = x_cep.data[j];
		}

	}

	features_available = true;
	energy_available = true;

}

MFCC_Features::MFCC_Features ()
{
	filt_beg = 0;
	filt_end = 0;
	filt = 0;
	fc = 0;
	preemp_filt = 0;
	icos_matrix = 0;
}

MFCC_Features::~MFCC_Features ()
{
	delete[] filt_beg;
	delete[] filt_end;
	delete[] fc;
	if (filt!=0) {
		for (int i=0; i<num_filt; i++)
			delete[] (filt[i]);
	}
	delete[] filt;
	delete preemp_filt;
	delete[] icos_matrix;
}

void MFCC_Features::config (Signal &x, string &config)
{
   bool linear = false;
   int tgt_num_filt = -1;

   istringstream is(config);
   ptree pt;

   try {
      read_json(is, pt);
      alpha = pt.get<float>("alpha");
      dither = pt.get<float>("dither");
      fb_low = pt.get<float>("fb_low");
      fb_hi = pt.get<float>("fb_hi");
		keep_c0 = pt.get<bool>("keep_c0");
      linear = pt.get<bool>("linear");
      num_cep = pt.get<int>("num_cep");
      win_inc_ms = pt.get<float>("win_inc_ms");
      win_len_ms = pt.get<float>("win_len_ms");
		if (pt.find("fb_only")==pt.not_found())
			fb_only = false;
		else
			fb_only = pt.get<bool>("fb_only");
      if (pt.find("tgt_num_filt")!=pt.not_found())
         tgt_num_filt = pt.get<int>("tgt_num_filt");
   } catch (exception &e) {
      throw ST_exception(string("MFCC_Features::config:  error in config string, ")+e.what());
   }

	// Sanity check
	float f_max = x.sampling_freq()/2.0f;
	if ((fb_low<0) || (fb_hi<fb_low) || (fb_hi>f_max))
      throw ST_exception("MFCC_Features::process -- filterbank parameters don't make sense.");
	sampling_freq = x.sampling_freq();

	// Find window parameters
	win_inc = (int) ((x.sampling_freq()*win_inc_ms)/1000);
	win_len = (int) ((x.sampling_freq()*win_len_ms)/1000);
	if ((win_len % 2) != 0)
		win_len++;
	if (win_inc<=0 || win_len<=0)
		throw ST_exception("MFCC_Features::process -- Window increment or length is <= 0.");

	// Initialize filterbank
	for (n_fft=1; n_fft<win_len; n_fft*= 2);
	n2_fft = n_fft/2;
	init_filtbank(f_max, n2_fft+1, linear, tgt_num_filt);
	if (fb_low==0 && fb_hi==f_max) { // Use full band
		fb_index_low = 0;
		fb_index_hi = num_filt-1;
	} else {
		fb_index_low = filtbank_get_filt_num(fb_low);
		fb_index_hi = filtbank_get_filt_num(fb_hi);
	}
   num_filt_bl = fb_index_hi-fb_index_low+1;

	// Initialize cepstral processing
	if ((num_cep <= 0) || (num_cep>(num_filt_bl-1)))
		throw ST_exception("Number of cepstral coefficients larger than number of filters.");

	num_features = 0;
	if (fb_only) {
		num_features = num_filt_bl;
	} else {
		if (keep_c0)
			num_features = 1;
		num_features += num_cep;
	}
	num_base_features = num_features;
	init_icostrans();

	// Initialize pre-emphasis
	preemp_filt = new vec<REAL_FFT> (n2_fft+1);
	float finc = (float) (f_max/(n2_fft+1.0));
	float f = 0;
	for (int j=0; j<=n2_fft; f += finc, j++)
		preemp_filt->data[j] = (REAL_FEAT) (1.0 + f*f/2.5e5);

}

vec<REAL_FFT> MFCC_Features::calc_fb (vec<REAL_FFT> &mag)
{
	int i, j, k, l;
	vec<REAL_FFT> x_fb(num_filt_bl);
	REAL_FFT val;

	for (i=fb_index_low, l=0; i<=fb_index_hi; i++, l++) {
		val = 0;
		for (j=filt_beg[i], k=0; j<filt_end[i]; j++, k++)
			val += mag.data[j]*filt[i][k];
		val = (REAL_FFT) (10.0*log10(val + 1e-20)-40.0);
		x_fb.data[l] = val;
	}
	return x_fb;

}

int MFCC_Features::filtbank_get_filt_num (float freq)
{
  int i;
  for (i=1; i<=num_filt && fc[i]<=freq; i++);
  i--;
  if(i < 1)
	  return 0;
  if (i==num_filt)
	  return num_filt-1;
  if ((fc[i+1]-freq) < (freq-fc[i]))
	  i++; // move to closest center freq
  return i-1;

}

vec<REAL_FEAT> MFCC_Features::icostrans (vec<REAL_FFT> &x)
{
	vec<REAL_FEAT> out(num_features);
	int i, j, k;
	REAL_FFT val;

	// Matrix vector multiply -- should use BLAS
	i = keep_c0 ? 0 : 1;
	for (k=0; k<num_features; k++, i++) {
		val = 0;
		for (j=0; j<num_filt_bl; j++)
			val += icos_matrix[i*num_filt_bl+j]*x.data[j];
		out.data[k] = (REAL_FEAT) val;
	}

	return out;

}

void MFCC_Features::init_icostrans ()
{
	float tmp, W;
	int i, j;

	tmp = (float) (1.0/num_filt_bl);
	W = (float) (PI*tmp);
	icos_matrix = new REAL_FFT[(num_cep+1)*num_filt_bl];

	for (i=0; i < (num_cep+1); i++) {
		for (j=0; j < num_filt_bl; j++) {
			icos_matrix[i*num_filt_bl+j] = (float) (cos((double) i*(j+0.5)*W)*tmp);
		}
	}

}

void MFCC_Features::init_filtbank (const float fmax, const int num_dft, bool linear, int tgt_num_filt)
{
  float f, *Fptr, area;
  float finc, delta_f;
  int i, j, nf, init_len, final_len;

  finc = fmax/(num_dft-1);

  if (linear) {
     if (tgt_num_filt<=0)
        delta_f = 100;
     else
        delta_f = fmax/(tgt_num_filt+1);
     for (nf=0, f=-delta_f; f<fmax; nf++)
        f += delta_f;
     fc = new float[nf];
     for (nf=0, f=-delta_f; f<fmax; nf++)
        fc[nf] = warp(f+=delta_f, alpha, fmax);
     nf -= 2;
     if ((fc[nf]+fc[nf+1])/2 > fmax)
        nf--;
     init_len = (int) ((fc[2]-fc[0])/finc + 1);
  } else {
     // Determine length of fc
     for (nf=11, f=1000; f<fmax; nf++)
        f *= 1.1f;
     fc = new float[nf];

     // Center frequencies
     for (i=0; i<=10; i++) // 100 Hz
        fc[i] = (float) warp((float) i*100, alpha, fmax);
     for (nf=11, f=1000; f<fmax; nf++) { // 10% spacing
        f *= 1.1f;
        fc[nf] = warp(f, alpha, fmax);
     }
     nf -= 2;
     if ((fc[nf]+fc[nf+1])/2 > fmax)
        nf--;

     // Determine maximum initial length
     init_len = 0;
     for (i = 1; i < nf + 1; i++) {
        int x = (int) ((fc[i+1]-fc[i-1])/finc + 1);
        init_len = x > init_len ? x : init_len;
     }
  }

  filt_beg = new int[nf];
  filt_end = new int[nf];
  filt = new flt_ptr[nf];
  float *tmp_filt = new float[init_len];

  for (i=1; i<=nf; ++i) {
	  area = 0.0;
	  Fptr = tmp_filt;
	  filt_beg[i-1] = 0;
	  filt_end[i-1] = 0;
	  for (j=0, f=0.0; (f<fmax) && (j<num_dft); f+=finc, j++) {
		  if ((f <= fc[i-1]) || (f >= fc[i+1]))
			  continue;
		  if (f < fc[i]) {
			  if (filt_beg[i-1] == 0)
				  filt_beg[i-1] = j;
			  *Fptr = (f-fc[i-1])/(fc[i]-fc[i-1]);
			  area += (*Fptr++);
		  } else {
			  *Fptr = (fc[i+1]-f)/(fc[i+1]-fc[i]);
			  filt_end[i-1] = j+1;
			  area += (*Fptr++);
		  }
	  }

	  //  Single DFT sample in filter passband, happens with very small windows (e.g., 5ms)
	  if ((filt_end[i-1]==0) && (filt_beg[i-1]!=0))
		  filt_end[i-1] = filt_beg[i-1]+1;
	  else if ((filt_end[i-1]!=0) && (filt_beg[i-1]==0))
		  filt_beg[i-1] = filt_end[i-1]-1;

	  final_len = filt_end[i-1] - filt_beg[i-1];
	  filt[i-1] = new float[final_len];
	  for (j=0; j<final_len; j++)
		  filt[i-1][j] = tmp_filt[j]/area;

  } // next i

  delete[] tmp_filt;
  num_filt = nf;

} // init_filterbank

float MFCC_Features::duration(void)
{
   return (float) (((float) num_vectors)*(0.001*win_inc_ms));
}

float MFCC_Features::get_win_inc_ms (void)
{
   return win_inc_ms;
}

//
// Local functions
//
static float warp (float freq, float alpha, float sfreq)
{
  float ret;
  float fr = (float) (freq * PI / sfreq);
  ret = (float) (fr + 2.0f * atan((double)(1.0 - alpha) * sin((double)fr) / (1.0 - (1.0 - alpha) * cos((double)fr))));
  ret *= (float) (sfreq/PI);
  return ret;
}


#pragma once
#ifndef KERNEL_H
#define KERNEL_H

#include <vector>
#include <Eigen/Core>

namespace Kernel
{
    double dot(const std::vector<Eigen::Vector3d> & v1, const std::vector<Eigen::Vector3d> & v2);

    void run_eigen_solver(const std::vector<Eigen::Matrix3f> &m);
}

#endif