/******************************************************************************
**
** Copyright (C) 2009-2011 Kyle Lutz <kyle.r.lutz@gmail.com>
** All rights reserved.
**
** This file is a part of the chemkit project. For more information
** see <http://www.chemkit.org>.
**
** Redistribution and use in source and binary forms, with or without
** modification, are permitted provided that the following conditions
** are met:
**
**   * Redistributions of source code must retain the above copyright
**     notice, this list of conditions and the following disclaimer.
**   * Redistributions in binary form must reproduce the above copyright
**     notice, this list of conditions and the following disclaimer in the
**     documentation and/or other materials provided with the distribution.
**   * Neither the name of the chemkit project nor the names of its
**     contributors may be used to endorse or promote products derived
**     from this software without specific prior written permission.
**
** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
** "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
** LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
** A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
** OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
** LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
**
******************************************************************************/

#include "mmffcalculation.h"

#include <boost/lexical_cast.hpp>

#include <chemkit/topology.h>
#include <chemkit/constants.h>
#include <chemkit/forcefield.h>
#include <chemkit/cartesiancoordinates.h>

#include "mmffparameters.h"

// === MmffCalculation ===================================================== //
MmffCalculation::MmffCalculation(int type, int atomCount, int parameterCount)
    : ForceFieldCalculation(type, atomCount, parameterCount)
{
}

// === MmffBondStrechCalculation =========================================== //
MmffBondStrechCalculation::MmffBondStrechCalculation(size_t a, size_t b)
    : MmffCalculation(BondStrech, 2, 2)
{
    setAtom(0, a);
    setAtom(1, b);
}

bool MmffBondStrechCalculation::setup(const MmffParameters *parameters)
{
    const boost::shared_ptr<chemkit::Topology> &topology = this->topology();

    size_t a = atom(0);
    size_t b = atom(1);

    int typeA = boost::lexical_cast<int>(topology->type(a));
    int typeB = boost::lexical_cast<int>(topology->type(b));
    int bondType = topology->bondedInteractionType(a, b);

    const MmffBondStrechParameters *bondStrechParameters = parameters->bondStrechParameters(bondType, typeA, typeB);
    if(bondStrechParameters){
        setParameter(0, bondStrechParameters->kb);
        setParameter(1, bondStrechParameters->r0);
        return true;
    }

    return false;
}

chemkit::Real MmffBondStrechCalculation::energy(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);

    chemkit::Real kb = parameter(0);
    chemkit::Real r0 = parameter(1);

    chemkit::Real r = coordinates->distance(a, b);
    chemkit::Real dr = r - r0;
    chemkit::Real cs = -2.0; // cubic strech constant

    // equation 2
    return 143.9325 * (kb / 2) * (dr*dr) * (1 + cs * dr + ((7.0/12.0)*(cs*cs)) * (dr*dr));
}

std::vector<chemkit::Vector3> MmffBondStrechCalculation::gradient(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);

    chemkit::Real kb = parameter(0);
    chemkit::Real r0 = parameter(1);

    chemkit::Real r = coordinates->distance(a, b);
    chemkit::Real dr = r - r0;
    chemkit::Real cs = -2.0; // cubic strech constant

    // dE/dr
    chemkit::Real de_dr = 143.9325 * kb * dr * (1 + cs * dr + (7.0/12.0 * (cs*cs) * (dr*dr)) + 0.5 * dr * (cs + (14.0/12.0 * (cs*cs) * dr)));

    boost::array<chemkit::Vector3, 2> gradient = coordinates->distanceGradient(a, b);

    gradient[0] *= de_dr;
    gradient[1] *= de_dr;

    return std::vector<chemkit::Vector3>(gradient.begin(), gradient.end());
}

// === MmffAngleBendCalculation ============================================ //
MmffAngleBendCalculation::MmffAngleBendCalculation(size_t a, size_t b, size_t c)
    : MmffCalculation(AngleBend, 3, 2)
{
    setAtom(0, a);
    setAtom(1, b);
    setAtom(2, c);
}

bool MmffAngleBendCalculation::setup(const MmffParameters *parameters)
{
    const boost::shared_ptr<chemkit::Topology> &topology = this->topology();

    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);

    int typeA = boost::lexical_cast<int>(topology->type(a));
    int typeB = boost::lexical_cast<int>(topology->type(b));
    int typeC = boost::lexical_cast<int>(topology->type(c));
    int angleType = topology->angleInteractionType(a, b, c);

    const MmffAngleBendParameters *angleBendParameters =
        parameters->angleBendParameters(angleType, typeA, typeB, typeC);

    if(angleBendParameters){
        setParameter(0, angleBendParameters->ka);
        setParameter(1, angleBendParameters->theta0);
        return true;
    }

    return false;
}

chemkit::Real MmffAngleBendCalculation::energy(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);

    chemkit::Real ka = parameter(0);
    chemkit::Real t0 = parameter(1);

    chemkit::Real cb = -0.007; // cubic bend constant
    chemkit::Real t = coordinates->angle(a, b, c);
    chemkit::Real dt = t - t0;

    // equation 3
    return 0.043844 * (ka / 2.0) * pow(dt, 2) * (1 + cb * dt);
}

std::vector<chemkit::Vector3> MmffAngleBendCalculation::gradient(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);

    chemkit::Real ka = parameter(0);
    chemkit::Real t0 = parameter(1);

    chemkit::Real cb = -0.007; // cubic bend constant
    chemkit::Real t = coordinates->angle(a, b, c);
    chemkit::Real dt = t - t0;

    // dE/dt
    chemkit::Real de_dt = 0.043844 * ka * dt * (1 + cb * dt + 0.5 * cb * dt);

    boost::array<chemkit::Vector3, 3> gradient = coordinates->angleGradient(a, b, c);

    gradient[0] *= de_dt;
    gradient[1] *= de_dt;
    gradient[2] *= de_dt;

    return std::vector<chemkit::Vector3>(gradient.begin(), gradient.end());
}

// === MmffStrechBendCalculation =========================================== //
MmffStrechBendCalculation::MmffStrechBendCalculation(size_t a, size_t b, size_t c)
    : MmffCalculation(BondStrech | AngleBend, 3, 5)
{
    setAtom(0, a);
    setAtom(1, b);
    setAtom(2, c);
}

bool MmffStrechBendCalculation::setup(const MmffParameters *parameters)
{
    const boost::shared_ptr<chemkit::Topology> &topology = this->topology();

    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);

    int typeA = boost::lexical_cast<int>(topology->type(a));
    int typeB = boost::lexical_cast<int>(topology->type(b));
    int typeC = boost::lexical_cast<int>(topology->type(c));
    int bondTypeAB = topology->bondedInteractionType(a, b);
    int bondTypeBC = topology->bondedInteractionType(b, c);
    int angleType = topology->angleInteractionType(a, b, c);

    int strechBendType =
        parameters->calculateStrechBendType(bondTypeAB, bondTypeBC, angleType);

    bool parametersSwapped = false;
    const MmffStrechBendParameters *strechBendParameters =
        parameters->strechBendParameters(strechBendType, typeA, typeB, typeC);
    if(!strechBendParameters){
        strechBendType =
            parameters->calculateStrechBendType(bondTypeBC, bondTypeAB, angleType);
        strechBendParameters =
            parameters->strechBendParameters(strechBendType, typeC, typeB, typeA);

        if(strechBendParameters){
            parametersSwapped = true;
        }
        else{
            strechBendParameters = parameters->defaultStrechBendParameters(typeA, typeB, typeC);

            if(!strechBendParameters){
                strechBendParameters = parameters->defaultStrechBendParameters(typeC, typeB, typeA);
                parametersSwapped = true;
            }
        }
    }

    const MmffBondStrechParameters *bondStrechParameters_ab =
        parameters->bondStrechParameters(bondTypeAB, typeA, typeB);
    const MmffBondStrechParameters *bondStrechParameters_bc =
        parameters->bondStrechParameters(bondTypeBC, typeB, typeC);
    const MmffAngleBendParameters *angleBendParameters =
        parameters->angleBendParameters(angleType, typeA, typeB, typeC);
    if(strechBendParameters && bondStrechParameters_ab && bondStrechParameters_bc && angleBendParameters){
        if(parametersSwapped){
            setParameter(1, strechBendParameters->kba_ijk);
            setParameter(0, strechBendParameters->kba_kji);
        }
        else{
            setParameter(0, strechBendParameters->kba_ijk);
            setParameter(1, strechBendParameters->kba_kji);
        }

        setParameter(2, bondStrechParameters_ab->r0);
        setParameter(3, bondStrechParameters_bc->r0);
        setParameter(4, angleBendParameters->theta0);
        return true;
    }

    return false;
}

chemkit::Real MmffStrechBendCalculation::energy(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);

    chemkit::Real kba_ijk = parameter(0);
    chemkit::Real kba_kji = parameter(1);
    chemkit::Real r0_ab = parameter(2);
    chemkit::Real r0_bc = parameter(3);
    chemkit::Real t0 = parameter(4);

    chemkit::Real r_ab = coordinates->distance(a, b);
    chemkit::Real r_bc = coordinates->distance(b, c);
    chemkit::Real dr_ab = r_ab - r0_ab;
    chemkit::Real dr_bc = r_bc - r0_bc;
    chemkit::Real t = coordinates->angle(a, b, c);
    chemkit::Real dt = t - t0;

    // equation 5
    return 2.51210 * (kba_ijk * dr_ab + kba_kji * dr_bc) * dt;
}

std::vector<chemkit::Vector3> MmffStrechBendCalculation::gradient(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);

    chemkit::Real kba_ijk = parameter(0);
    chemkit::Real kba_kji = parameter(1);
    chemkit::Real r0_ab = parameter(2);
    chemkit::Real r0_bc = parameter(3);
    chemkit::Real t0 = parameter(4);

    chemkit::Real r_ab = coordinates->distance(a, b);
    chemkit::Real r_bc = coordinates->distance(b, c);
    chemkit::Real dr_ab = r_ab - r0_ab;
    chemkit::Real dr_bc = r_bc - r0_bc;
    chemkit::Real t = coordinates->angle(a, b, c);
    chemkit::Real dt = t - t0;

    std::vector<chemkit::Vector3> gradient(3);

    boost::array<chemkit::Vector3, 2> distanceGradientAB = coordinates->distanceGradient(a, b);
    boost::array<chemkit::Vector3, 2> distanceGradientBC = coordinates->distanceGradient(b, c);
    boost::array<chemkit::Vector3, 3> angleGradientABC = coordinates->angleGradient(a, b, c);

    gradient[0] = (distanceGradientAB[0] * kba_ijk * dt + angleGradientABC[0] * (kba_ijk * dr_ab + kba_kji * dr_bc)) * 2.51210;
    gradient[1] = ((distanceGradientAB[1] * kba_ijk + distanceGradientBC[0] * kba_kji) * dt + angleGradientABC[1] * (kba_ijk * dr_ab + kba_kji * dr_bc)) * 2.51210;
    gradient[2] = ((distanceGradientBC[1] * kba_kji) * dt + angleGradientABC[2] * (kba_ijk * dr_ab + kba_kji * dr_bc)) * 2.51210;

    return gradient;
}

// === MmffOutOfPlaneBendingCalculation ==================================== //
MmffOutOfPlaneBendingCalculation::MmffOutOfPlaneBendingCalculation(size_t a, size_t b, size_t c, size_t d)
    : MmffCalculation(Inversion, 4, 1)
{
    setAtom(0, a);
    setAtom(1, b);
    setAtom(2, c);
    setAtom(3, d);
}

bool MmffOutOfPlaneBendingCalculation::setup(const MmffParameters *parameters)
{
    const boost::shared_ptr<chemkit::Topology> &topology = this->topology();

    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);
    size_t d = atom(3);

    int typeA = boost::lexical_cast<int>(topology->type(a));
    int typeB = boost::lexical_cast<int>(topology->type(b));
    int typeC = boost::lexical_cast<int>(topology->type(c));
    int typeD = boost::lexical_cast<int>(topology->type(d));

    const MmffOutOfPlaneBendingParameters *outOfPlaneBendingParameters =
        parameters->outOfPlaneBendingParameters(typeA, typeB, typeC, typeD);
    if(!outOfPlaneBendingParameters){
        return false;
    }

    setParameter(0, outOfPlaneBendingParameters->koop);
    return true;
}

chemkit::Real MmffOutOfPlaneBendingCalculation::energy(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);
    size_t d = atom(3);

    chemkit::Real angle = coordinates->wilsonAngle(a, b, c, d);
    chemkit::Real koop = parameter(0);

    // equation 6
    return 0.043844 * (koop / 2.0) * (angle*angle);
}

std::vector<chemkit::Vector3> MmffOutOfPlaneBendingCalculation::gradient(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);
    size_t d = atom(3);

    chemkit::Real angle = coordinates->wilsonAngle(a, b, c, d);
    chemkit::Real koop = parameter(0);

    // dE/dw
    chemkit::Real de_dw = 0.043844 * koop * angle;

    boost::array<chemkit::Vector3, 4> gradient = coordinates->wilsonAngleGradient(a, b, c, d);

    gradient[0] *= de_dw;
    gradient[1] *= de_dw;
    gradient[2] *= de_dw;
    gradient[3] *= de_dw;

    return std::vector<chemkit::Vector3>(gradient.begin(), gradient.end());
}

// === MmffTorsionCalculation ============================================== //
MmffTorsionCalculation::MmffTorsionCalculation(size_t a, size_t b, size_t c, size_t d)
    : MmffCalculation(Torsion, 4, 3)
{
    setAtom(0, a);
    setAtom(1, b);
    setAtom(2, c);
    setAtom(3, d);
}

bool MmffTorsionCalculation::setup(const MmffParameters *parameters)
{
    const boost::shared_ptr<chemkit::Topology> &topology = this->topology();

    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);
    size_t d = atom(3);

    int typeA = boost::lexical_cast<int>(topology->type(a));
    int typeB = boost::lexical_cast<int>(topology->type(b));
    int typeC = boost::lexical_cast<int>(topology->type(c));
    int typeD = boost::lexical_cast<int>(topology->type(d));
    int torsionType = topology->torsionInteractionType(a, b, c, d);

    const MmffTorsionParameters *torsionParameters =
        parameters->torsionParameters(torsionType, typeA, typeB, typeC, typeD);
    if(!torsionParameters){
        return false;
    }

    setParameter(0, torsionParameters->V1);
    setParameter(1, torsionParameters->V2);
    setParameter(2, torsionParameters->V3);

    return true;
}

chemkit::Real MmffTorsionCalculation::energy(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);
    size_t d = atom(3);

    chemkit::Real angle = coordinates->torsionAngleRadians(a, b, c, d);
    chemkit::Real V1 = parameter(0);
    chemkit::Real V2 = parameter(1);
    chemkit::Real V3 = parameter(2);

    // equation 7
    return 0.5 * (V1 * (1.0 + cos(angle)) + V2 * (1.0 - cos(2.0 * angle)) + V3 * (1.0 + cos(3.0 * angle)));
}

std::vector<chemkit::Vector3> MmffTorsionCalculation::gradient(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);
    size_t d = atom(3);

    chemkit::Real phi = coordinates->torsionAngleRadians(a, b, c, d);
    chemkit::Real V1 = parameter(0);
    chemkit::Real V2 = parameter(1);
    chemkit::Real V3 = parameter(2);

    // dE/dphi
    chemkit::Real de_dphi = 0.5 * (-V1 * sin(phi) + 2 * V2 * sin(2 * phi) - 3 * V3 * sin(3 * phi));

    boost::array<chemkit::Vector3, 4> gradient = coordinates->torsionAngleGradientRadians(a, b, c, d);

    gradient[0] *= de_dphi;
    gradient[1] *= de_dphi;
    gradient[2] *= de_dphi;
    gradient[3] *= de_dphi;

    return std::vector<chemkit::Vector3>(gradient.begin(), gradient.end());
}

// === MmffVanDerWaalsCalculation ========================================== //
MmffVanDerWaalsCalculation::MmffVanDerWaalsCalculation(size_t a, size_t b)
    : MmffCalculation(VanDerWaals, 2, 2)
{
    setAtom(0, a);
    setAtom(1, b);
}

bool MmffVanDerWaalsCalculation::setup(const MmffParameters *parameters)
{
    const boost::shared_ptr<chemkit::Topology> &topology = this->topology();

    size_t a = atom(0);
    size_t b = atom(1);

    int typeA = boost::lexical_cast<int>(topology->type(a));
    int typeB = boost::lexical_cast<int>(topology->type(b));

    const MmffVanDerWaalsParameters *parametersA = parameters->vanDerWaalsParameters(typeA);
    const MmffVanDerWaalsParameters *parametersB = parameters->vanDerWaalsParameters(typeB);
    if(!parametersA || !parametersB){
        return false;
    }

    chemkit::Real N_a = parametersA->N;
    chemkit::Real N_b = parametersB->N;
    chemkit::Real A_a = parametersA->A;
    chemkit::Real A_b = parametersB->A;
    chemkit::Real G_a = parametersA->G;
    chemkit::Real G_b = parametersB->G;
    chemkit::Real alpha_a = parametersA->alpha;
    chemkit::Real alpha_b = parametersB->alpha;
    char DA_a = parametersA->DA;
    char DA_b = parametersB->DA;

    // equation 9
    chemkit::Real rs_aa = A_a * pow(alpha_a, (1.0/4.0));
    chemkit::Real rs_bb = A_b * pow(alpha_b, (1.0/4.0));

    // equation 11
    chemkit::Real gamma = (rs_aa - rs_bb) / (rs_aa + rs_bb);

    // equation 10
    chemkit::Real rs;

    if(DA_a == 'D' || (DA_b == 'D')){
        rs = 0.5 * (rs_aa + rs_bb);
    }
    else{
        rs = 0.5 * (rs_aa + rs_bb) * (1.0 + 0.2 * (1.0 - exp(-12.0 * gamma * gamma)));
    }

    // equation 12
    chemkit::Real eps = ((181.16 * G_a * G_b * alpha_a * alpha_b) / (sqrt(alpha_a / N_a) + sqrt(alpha_b / N_b))) * pow(rs, -6.0);

    if((DA_a == 'D' && DA_b == 'A') || (DA_a == 'A' && DA_b == 'D')){
        rs *= 0.8;
        eps *= 0.5;
    }

    setParameter(0, rs);
    setParameter(1, eps);

    return true;
}

chemkit::Real MmffVanDerWaalsCalculation::energy(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);

    chemkit::Real rs = parameter(0);
    chemkit::Real eps = parameter(1);
    chemkit::Real r = coordinates->distance(a, b);

    // equation 8
    return eps * pow(((1.07 * rs) / (r + 0.07 * rs)), 7) * (((1.12 * pow(rs, 7)) / (pow(r, 7) + 0.12 * pow(rs, 7))) - 2);
}

std::vector<chemkit::Vector3> MmffVanDerWaalsCalculation::gradient(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);

    chemkit::Real rs = parameter(0);
    chemkit::Real eps = parameter(1);
    chemkit::Real r = coordinates->distance(a, b);

    // dE/dr
    chemkit::Real de_dr = 7 * eps * pow(1.07 * rs / (r + 0.07 * rs), 6) *
                           ((-1.07 * rs / pow(r + 0.07 * rs, 2)) * (1.12 * pow(rs, 7) / (pow(r, 7) + 0.12 * pow(rs, 7)) - 2) +
                           (-1.12 * pow(rs, 7) * pow(r, 6) / pow(pow(r, 7) + 0.12 * pow(rs, 7), 2)) * (1.07 * rs / (r + 0.07 * rs)));

    boost::array<chemkit::Vector3, 2> gradient = coordinates->distanceGradient(a, b);

    gradient[0] *= de_dr;
    gradient[1] *= de_dr;

    return std::vector<chemkit::Vector3>(gradient.begin(), gradient.end());
}

// === MmffElectrostaticCalculation ======================================== //
MmffElectrostaticCalculation::MmffElectrostaticCalculation(size_t a, size_t b)
    : MmffCalculation(Electrostatic, 2, 3)
{
    setAtom(0, a);
    setAtom(1, b);
}

bool MmffElectrostaticCalculation::setup(const MmffParameters *parameters)
{
    CHEMKIT_UNUSED(parameters);

    const boost::shared_ptr<chemkit::Topology> &topology = this->topology();

    size_t a = atom(0);
    size_t b = atom(1);

    chemkit::Real oneFourScaling;

    if(topology->isOneFour(a, b)){
        oneFourScaling = 0.75;
    }
    else{
        oneFourScaling = 1.0;
    }

    setParameter(0, topology->charge(a));
    setParameter(1, topology->charge(b));
    setParameter(2, oneFourScaling);

    return true;
}

chemkit::Real MmffElectrostaticCalculation::energy(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);

    chemkit::Real qa = parameter(0);
    chemkit::Real qb = parameter(1);
    chemkit::Real oneFourScaling = parameter(2);

    chemkit::Real r = coordinates->distance(a, b);
    chemkit::Real e = 1.0; // dielectric constant
    chemkit::Real d = 0.05; // electrostatic buffering constant

    // equation 13
    return ((332.0716 * qa * qb) / (e * (r + d))) * oneFourScaling;
}

std::vector<chemkit::Vector3> MmffElectrostaticCalculation::gradient(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);

    chemkit::Real qa = parameter(0);
    chemkit::Real qb = parameter(1);
    chemkit::Real oneFourScaling = parameter(2);

    chemkit::Real r = coordinates->distance(a, b);
    chemkit::Real e = 1.0; // dielectric constant
    chemkit::Real d = 0.05; // electrostatic buffering constant

    chemkit::Real de_dr = 332.0716 * qa * qb * oneFourScaling * (-1.0 / (e * pow(r + d, 2)));

    boost::array<chemkit::Vector3, 2> gradient = coordinates->distanceGradient(a, b);

    gradient[0] *= de_dr;
    gradient[1] *= de_dr;

    return std::vector<chemkit::Vector3>(gradient.begin(), gradient.end());
}


#include <cstdint>
#include <functional>
#include <limits>
#include <queue>
#include <stdexcept>
#include <unordered_map>
#include <vector>
#include <Eigen/Geometry>
#include <arc_utilities/arc_helpers.hpp>

#ifndef DIJKSTRAS_HPP
#define DIJKSTRAS_HPP

namespace arc_dijkstras
{
    class GraphEdge
    {
        protected:

            int64_t from_index_;
            int64_t to_index_;
            double weight_;

        public:

            static uint64_t Serialize(const GraphEdge& edge, std::vector<uint8_t>& buffer)
            {
                return edge.SerializeSelf(buffer);
            }

            static std::pair<GraphEdge, uint64_t> Deserialize(const std::vector<uint8_t>& buffer, const uint64_t current)
            {
                GraphEdge temp_edge;
                const uint64_t bytes_read = temp_edge.DeserializeSelf(buffer, current);
                return std::make_pair(temp_edge, bytes_read);
            }

            GraphEdge(const int64_t from_index, const int64_t to_index, const double weight)
                : from_index_(from_index), to_index_(to_index), weight_(weight)
            {}

            GraphEdge()
                : from_index_(-1), to_index_(-1), weight_(0.0)
            {}

            uint64_t SerializeSelf(std::vector<uint8_t>& buffer) const
            {
                const uint64_t start_buffer_size = buffer.size();
                arc_helpers::SerializeFixedSizePOD<int64_t>(from_index_, buffer);
                arc_helpers::SerializeFixedSizePOD<int64_t>(to_index_, buffer);
                arc_helpers::SerializeFixedSizePOD<double>(weight_, buffer);
                // Figure out how many bytes were written
                const uint64_t end_buffer_size = buffer.size();
                const uint64_t bytes_written = end_buffer_size - start_buffer_size;
                return bytes_written;
            }

            uint64_t DeserializeSelf(const std::vector<uint8_t>& buffer, const uint64_t current)
            {
                uint64_t current_position = current;
                const std::pair<int64_t, uint64_t> deserialized_from_index = arc_helpers::DeserializeFixedSizePOD<int64_t>(buffer, current_position);
                from_index_ = deserialized_from_index.first;
                current_position += deserialized_from_index.second;
                const std::pair<int64_t, uint64_t> deserialized_to_index = arc_helpers::DeserializeFixedSizePOD<int64_t>(buffer, current_position);
                to_index_ = deserialized_to_index.first;
                current_position += deserialized_to_index.second;
                const std::pair<double, uint64_t> deserialized_weight = arc_helpers::DeserializeFixedSizePOD<double>(buffer, current_position);
                weight_ = deserialized_weight.first;
                current_position += deserialized_weight.second;
                // Figure out how many bytes were read
                const uint64_t bytes_read = current_position - current;
                return bytes_read;
            }

            bool operator==(const GraphEdge& other) const
            {
                return (from_index_ == other.GetFromIndex() && to_index_ == other.GetToIndex() && weight_ == other.GetWeight());
            }

            std::string Print() const
            {
                return std::string("(" + std::to_string(from_index_) + "->" + std::to_string(to_index_) + ") : " + std::to_string(weight_));
            }

            int64_t GetFromIndex() const
            {
                return from_index_;
            }

            int64_t GetToIndex() const
            {
                return to_index_;
            }

            double GetWeight() const
            {
                return weight_;
            }

            void SetFromIndex(const int64_t new_from_index)
            {
                from_index_ = new_from_index;
            }

            void SetToIndex(const int64_t new_to_index)
            {
                to_index_ = new_to_index;
            }

            void SetWeight(const double new_weight)
            {
                weight_ = new_weight;
            }
    };

    inline std::ostream& operator<< (std::ostream& stream, const GraphEdge& edge)
    {
        stream << edge.Print();
        return stream;
    }

    template<typename NodeValueType, typename Allocator=std::allocator<NodeValueType>>
    class GraphNode
    {
        protected:

            NodeValueType value_;
            double distance_;
            std::vector<GraphEdge> in_edges_;
            std::vector<GraphEdge> out_edges_;

        public:

            EIGEN_MAKE_ALIGNED_OPERATOR_NEW

            static uint64_t Serialize(const GraphNode<NodeValueType, Allocator>& node, std::vector<uint8_t>& buffer, const std::function<uint64_t(const NodeValueType&, std::vector<uint8_t>&)>& value_serializer)
            {
                return node.SerializeSelf(buffer, value_serializer);
            }

            static std::pair<GraphNode<NodeValueType, Allocator>, uint64_t> Deserialize(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<NodeValueType, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
            {
                GraphNode<NodeValueType, Allocator> temp_node;
                const uint64_t bytes_read = temp_node.DeserializeSelf(buffer, current, value_deserializer);
                return std::make_pair(temp_node, bytes_read);
            }

            GraphNode(const NodeValueType& value, const double distance, const std::vector<GraphEdge>& new_in_edges, const std::vector<GraphEdge>& new_out_edges)
                : value_(value), distance_(distance), in_edges_(new_in_edges), out_edges_(new_out_edges)
            {}

            explicit GraphNode(const NodeValueType& value)
                : value_(value), distance_(std::numeric_limits<double>::infinity())
            {}

            GraphNode()
                : distance_(std::numeric_limits<double>::infinity())
            {}

            uint64_t SerializeSelf(std::vector<uint8_t>& buffer, const std::function<uint64_t(const NodeValueType&, std::vector<uint8_t>&)>& value_serializer) const
            {
                const uint64_t start_buffer_size = buffer.size();
                // Serialize the value
                value_serializer(value_, buffer);
                // Serialize the distance
                arc_helpers::SerializeFixedSizePOD<double>(distance_, buffer);
                // Serialize the in edges
                arc_helpers::SerializeVector<GraphEdge>(in_edges_, buffer, GraphEdge::Serialize);
                // Serialize the in edges
                arc_helpers::SerializeVector<GraphEdge>(out_edges_, buffer, GraphEdge::Serialize);
                // Figure out how many bytes were written
                const uint64_t end_buffer_size = buffer.size();
                const uint64_t bytes_written = end_buffer_size - start_buffer_size;
                return bytes_written;
            }

            uint64_t DeserializeSelf(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<NodeValueType, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
            {
                uint64_t current_position = current;
                // Deserialize the value
                const std::pair<NodeValueType, uint64_t> value_deserialized = value_deserializer(buffer, current_position);
                value_ = value_deserialized.first;
                current_position += value_deserialized.second;
                // Deserialize the distace
                const std::pair<double, uint64_t> distance_deserialized = arc_helpers::DeserializeFixedSizePOD<double>(buffer, current_position);
                distance_ = distance_deserialized.first;
                current_position += distance_deserialized.second;
                // Deserialize the in edges
                const std::pair<std::vector<GraphEdge>, uint64_t> in_edges_deserialized = arc_helpers::DeserializeVector<GraphEdge>(buffer, current_position, GraphEdge::Deserialize);
                in_edges_ = in_edges_deserialized.first;
                current_position += in_edges_deserialized.second;
                // Deserialize the out edges
                const std::pair<std::vector<GraphEdge>, uint64_t> out_edges_deserialized = arc_helpers::DeserializeVector<GraphEdge>(buffer, current_position, GraphEdge::Deserialize);
                out_edges_ = out_edges_deserialized.first;
                current_position += out_edges_deserialized.second;
                // Figure out how many bytes were read
                const uint64_t bytes_read = current_position - current;
                return bytes_read;
            }

            std::string Print() const
            {
                std::ostringstream strm;
                strm << "Node : " << distance_ << " In Edges : ";
                if (in_edges_.size() > 0)
                {
                    strm << in_edges_[0].Print();
                    for (size_t idx = 1; idx < in_edges_.size(); idx++)
                    {
                        strm << ", " << in_edges_[idx].Print();
                    }
                }
                strm << " Out Edges : ";
                if (out_edges_.size() > 0)
                {
                    strm << out_edges_[0].Print();
                    for (size_t idx = 1; idx < out_edges_.size(); idx++)
                    {
                        strm << ", " << out_edges_[idx].Print();
                    }
                }
                return strm.str();
            }

            const NodeValueType& GetValueImmutable() const
            {
                return value_;
            }

            NodeValueType& GetValueMutable()
            {
                return value_;
            }

            void AddInEdge(const GraphEdge& new_in_edge)
            {
                in_edges_.push_back(new_in_edge);
            }

            void AddOutEdge(const GraphEdge& new_out_edge)
            {
                out_edges_.push_back(new_out_edge);
            }

            void AddEdgePair(const GraphEdge& new_in_edge, const GraphEdge& new_out_edge)
            {
                AddInEdge(new_in_edge);
                AddOutEdge(new_out_edge);
            }

            double GetDistance() const
            {
                return distance_;
            }

            void SetDistance(const double distance)
            {
                distance_ = distance;
            }

            const std::vector<GraphEdge>& GetInEdgesImmutable() const
            {
                return in_edges_;
            }

            std::vector<GraphEdge>& GetInEdgesMutable()
            {
                return in_edges_;
            }

            const std::vector<GraphEdge>& GetOutEdgesImmutable() const
            {
                return out_edges_;
            }

            std::vector<GraphEdge>& GetOutEdgesMutable()
            {
                return out_edges_;
            }

            void SetInEdges(const std::vector<GraphEdge>& new_in_edges)
            {
                in_edges_ = new_in_edges;
            }

            void SetOutEdges(const std::vector<GraphEdge>& new_out_edges)
            {
                out_edges_ = new_out_edges;
            }
    };

    template<typename NodeValueType, typename Allocator=std::allocator<NodeValueType>>
    class Graph
    {
        protected:

            std::vector<GraphNode<NodeValueType, Allocator>> nodes_;

        public:

            static uint64_t Serialize(const Graph<NodeValueType, Allocator>& graph, std::vector<uint8_t>& buffer, const std::function<uint64_t(const NodeValueType&, std::vector<uint8_t>&)>& value_serializer)
            {
                return graph.SerializeSelf(buffer, value_serializer);
            }

            static std::pair<Graph<NodeValueType, Allocator>, uint64_t> Deserialize(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<NodeValueType, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
            {
                Graph<NodeValueType, Allocator> temp_graph;
                const uint64_t bytes_read = temp_graph.DeserializeSelf(buffer, current, value_deserializer);
                return std::make_pair(temp_graph, bytes_read);
            }

            Graph(const std::vector<GraphNode<NodeValueType, Allocator>>& nodes)
            {
                if (CheckGraphLinkage(nodes))
                {
                    nodes_ = nodes;
                }
                else
                {
                    throw std::invalid_argument("Invalid graph linkage");
                }
            }

            Graph(const size_t expected_size)
            {
                nodes_.reserve(expected_size);
            }

            Graph()
            {}

            uint64_t SerializeSelf(std::vector<uint8_t>& buffer, const std::function<uint64_t(const NodeValueType&, std::vector<uint8_t>&)>& value_serializer) const
            {
                const uint64_t start_buffer_size = buffer.size();
                std::function<uint64_t(const GraphNode<NodeValueType, Allocator>&, std::vector<uint8_t>&)> graph_state_serializer = std::bind(GraphNode<NodeValueType, Allocator>::Serialize, std::placeholders::_1, std::placeholders::_2, value_serializer);
                arc_helpers::SerializeVector<GraphNode<NodeValueType, Allocator>>(nodes_, buffer, graph_state_serializer);
                // Figure out how many bytes were written
                const uint64_t end_buffer_size = buffer.size();
                const uint64_t bytes_written = end_buffer_size - start_buffer_size;
                return bytes_written;
            }

            uint64_t DeserializeSelf(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<NodeValueType, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
            {
                const std::function<std::pair<GraphNode<NodeValueType, Allocator>, uint64_t>(const std::vector<uint8_t>&, const uint64_t)> graph_state_deserializer = std::bind(GraphNode<NodeValueType, Allocator>::Deserialize, std::placeholders::_1, std::placeholders::_2, value_deserializer);
                const std::pair<std::vector<GraphNode<NodeValueType, Allocator>>, uint64_t> deserialized_nodes = arc_helpers::DeserializeVector<GraphNode<NodeValueType, Allocator>>(buffer, current, graph_state_deserializer);
                nodes_ = deserialized_nodes.first;
                return deserialized_nodes.second;
            }

            std::string Print() const
            {
                std::ostringstream strm;
                strm << "Graph - Nodes : ";
                if (nodes_.size() > 0)
                {
                    strm << nodes_[0].Print();
                    for (size_t idx = 1; idx < nodes_.size(); idx++)
                    {
                        strm << "\n" << nodes_[idx].Print();
                    }
                }
                return strm.str();
            }

            void ShrinkToFit()
            {
                nodes_.shrink_to_fit();
            }

            bool IndexInRange(const int64_t index) const
            {
                if (index >= 0)
                {
                    if (index < (int64_t)(nodes_.size()))
                    {
                        return true;
                    }
                    else
                    {
                        return false;
                    }
                }
                else
                {
                    return false;
                }
            }

            bool CheckGraphLinkage() const
            {
                return CheckGraphLinkage(GetNodesImmutable());
            }

            static bool CheckGraphLinkage(const Graph<NodeValueType, Allocator>& graph)
            {
                return CheckGraphLinkage(graph.GetNodesImmutable());
            }

            static bool CheckGraphLinkage(const std::vector<GraphNode<NodeValueType, Allocator>>& nodes)
            {
                // Go through every node and make sure the edges are valid
                for (size_t idx = 0; idx < nodes.size(); idx++)
                {
                    const GraphNode<NodeValueType, Allocator>& current_node = nodes[idx];
                    // Check the in edges first
                    const std::vector<GraphEdge>& in_edges = current_node.GetInEdgesImmutable();
                    for (size_t in_edge_idx = 0; in_edge_idx < in_edges.size(); in_edge_idx++)
                    {
                        const GraphEdge& current_edge = in_edges[in_edge_idx];
                        // Check from index to make sure it's in bounds
                        const int64_t from_index = current_edge.GetFromIndex();
                        if (from_index < 0 || from_index >= (int64_t)nodes.size())
                        {
                            return false;
                        }
                        // Check to index to make sure it matches our own index
                        const int64_t to_index = current_edge.GetToIndex();
                        if (to_index != (int64_t)idx)
                        {
                            return false;
                        }
                        // Check edge validity (edges to ourself are not allowed)
                        if (from_index == to_index)
                        {
                            return false;
                        }
                        // Check to make sure that the from index node is linked to us
                        const GraphNode<NodeValueType, Allocator>& from_node = nodes[(size_t)from_index];
                        const std::vector<GraphEdge>& from_node_out_edges = from_node.GetOutEdgesImmutable();
                        bool from_node_connection_valid = false;
                        // Make sure at least one out edge of the from index node corresponds to the current node
                        for (size_t from_node_out_edge_idx = 0; from_node_out_edge_idx < from_node_out_edges.size(); from_node_out_edge_idx++)
                        {
                            const GraphEdge& current_from_node_out_edge = from_node_out_edges[from_node_out_edge_idx];
                            if (current_from_node_out_edge.GetToIndex() == (int64_t)idx)
                            {
                                from_node_connection_valid = true;
                            }
                        }
                        if (from_node_connection_valid == false)
                        {
                            return false;
                        }
                    }
                    // Check the out edges second
                    const std::vector<GraphEdge>& out_edges = current_node.GetOutEdgesImmutable();
                    for (size_t out_edge_idx = 0; out_edge_idx < out_edges.size(); out_edge_idx++)
                    {
                        const GraphEdge& current_edge = out_edges[out_edge_idx];
                        // Check from index to make sure it matches our own index
                        const int64_t from_index = current_edge.GetFromIndex();
                        if (from_index != (int64_t)idx)
                        {
                            return false;
                        }
                        // Check to index to make sure it's in bounds
                        const int64_t to_index = current_edge.GetToIndex();
                        if (to_index < 0 || to_index >= (int64_t)nodes.size())
                        {
                            return false;
                        }
                        // Check edge validity (edges to ourself are not allowed)
                        if (from_index == to_index)
                        {
                            return false;
                        }
                        // Check to make sure that the to index node is linked to us
                        const GraphNode<NodeValueType, Allocator>& to_node = nodes[(size_t)to_index];
                        const std::vector<GraphEdge>& to_node_in_edges = to_node.GetInEdgesImmutable();
                        bool to_node_connection_valid = false;
                        // Make sure at least one in edge of the to index node corresponds to the current node
                        for (size_t to_node_in_edge_idx = 0; to_node_in_edge_idx < to_node_in_edges.size(); to_node_in_edge_idx++)
                        {
                            const GraphEdge& current_to_node_in_edge = to_node_in_edges[to_node_in_edge_idx];
                            if (current_to_node_in_edge.GetFromIndex() == (int64_t)idx)
                            {
                                to_node_connection_valid = true;
                            }
                        }
                        if (to_node_connection_valid == false)
                        {
                            return false;
                        }
                    }
                }
                return true;
            }

            const std::vector<GraphNode<NodeValueType, Allocator>>& GetNodesImmutable() const
            {
                return nodes_;
            }

            std::vector<GraphNode<NodeValueType, Allocator>>& GetNodesMutable()
            {
                return nodes_;
            }

            const GraphNode<NodeValueType, Allocator>& GetNodeImmutable(const int64_t index) const
            {
                return nodes_.at((size_t)index);
            }

            GraphNode<NodeValueType, Allocator>& GetNodeMutable(const int64_t index)
            {
                return nodes_.at((size_t)index);
            }

            int64_t AddNode(const GraphNode<NodeValueType, Allocator>& new_node)
            {
                nodes_.push_back(new_node);
                return (int64_t)(nodes_.size() - 1);
            }

            int64_t AddNode(const NodeValueType& new_value)
            {
                nodes_.push_back(GraphNode<NodeValueType, Allocator>(new_value));
                return (int64_t)(nodes_.size() - 1);
            }

            void AddEdgeBetweenNodes(const int64_t from_index, const int64_t to_index, const double edge_weight)
            {
                // We retrieve the nodes first, since retrieval performs bounds checks first
                GraphNode<NodeValueType, Allocator>& from_node = GetNodeMutable(from_index);
                GraphNode<NodeValueType, Allocator>& to_node = GetNodeMutable(to_index);
                if (from_index == to_index)
                {
                    throw std::invalid_argument("Invalid circular edge from==to not allowed");
                }
                const GraphEdge new_edge(from_index, to_index, edge_weight);
                from_node.AddOutEdge(new_edge);
                to_node.AddInEdge(new_edge);
            }

            void AddEdgesBetweenNodes(const int64_t first_index, const int64_t second_index, const double edge_weight)
            {
                // We retrieve the nodes first, since retrieval performs bounds checks first
                GraphNode<NodeValueType, Allocator>& first_node = GetNodeMutable(first_index);
                GraphNode<NodeValueType, Allocator>& second_node = GetNodeMutable(second_index);
                if (first_index == second_index)
                {
                    throw std::invalid_argument("Invalid circular edge first==second not allowed");
                }
                const GraphEdge first_edge(first_index, second_index, edge_weight);
                first_node.AddOutEdge(first_edge);
                second_node.AddInEdge(first_edge);
                const GraphEdge second_edge(second_index, first_index, edge_weight);
                second_node.AddOutEdge(second_edge);
                first_node.AddInEdge(second_edge);
            }
    };

    template<typename NodeValueType, typename Allocator=std::allocator<NodeValueType>>
    class SimpleDijkstrasAlgorithm
    {
        protected:

            class CompareIndexFn
            {
                public:

                    constexpr bool operator()(const std::pair<int64_t, double>& lhs, const std::pair<int64_t, double>& rhs) const
                    {
                        return lhs.second > rhs.second;
                    }
            };

            SimpleDijkstrasAlgorithm() {}

        public:

            typedef std::pair<Graph<NodeValueType, Allocator>, std::pair<std::vector<int64_t>, std::vector<double>>> DijkstrasResult;

            static DijkstrasResult PerformDijkstrasAlgorithm(const Graph<NodeValueType, Allocator>& graph, const int64_t start_index)
            {
                if ((start_index < 0) && (start_index >= (int64_t)graph.GetNodesImmutable().size()))
                {
                    throw std::invalid_argument("Start index out of range");
                }
                Graph<NodeValueType, Allocator> working_copy = graph;
                // Setup
                std::vector<int64_t> previous_index_map(working_copy.GetNodesImmutable().size(), -1);
                std::vector<double> distances(working_copy.GetNodesImmutable().size(), std::numeric_limits<double>::infinity());
                std::priority_queue<std::pair<int64_t, double>, std::vector<std::pair<int64_t, double>>, CompareIndexFn> queue;
                std::unordered_map<int64_t, uint32_t> explored(graph.GetNodesImmutable().size());
                for (size_t idx = 0; idx < working_copy.GetNodesImmutable().size(); idx++)
                {
                    working_copy.GetNodeMutable((int64_t)idx).SetDistance(std::numeric_limits<double>::infinity());
                    queue.push(std::make_pair((int64_t)idx, std::numeric_limits<double>::infinity()));
                }
                working_copy.GetNodeMutable(start_index).SetDistance(0.0);
                previous_index_map[(size_t)start_index] = start_index;
                distances[(size_t)start_index] = 0.0;
                queue.push(std::make_pair(start_index, 0.0));
                while (queue.size() > 0)
                {
                    const std::pair<int64_t, double> top_node = queue.top();
                    const int64_t& top_node_index = top_node.first;
                    const double& top_node_distance = top_node.second;
                    queue.pop();
                    if (explored[top_node.first] > 0)
                    {
                        // We've already been here
                        continue;
                    }
                    else
                    {
                        // Note that we've been here
                        explored[top_node.first] = 1;
                        // Get our neighbors
                        const std::vector<GraphEdge>& neighbor_edges = working_copy.GetNodeImmutable(top_node_index).GetInEdgesImmutable();
                        // Go through our neighbors
                        for (size_t neighbor_idx = 0; neighbor_idx < neighbor_edges.size(); neighbor_idx++)
                        {
                            const int64_t neighbor_index = neighbor_edges[neighbor_idx].GetFromIndex();
                            const double neighbor_edge_weight = neighbor_edges[neighbor_idx].GetWeight();
                            const double new_neighbor_distance = top_node_distance + neighbor_edge_weight;
                            // Check against the neighbor
                            const double stored_neighbor_distance = working_copy.GetNodeImmutable(neighbor_index).GetDistance();
                            if (new_neighbor_distance < stored_neighbor_distance)
                            {
                                // We've found a better way to get to this node
                                // Check if it's already been explored
                                if (explored[neighbor_index] > 0)
                                {
                                    // If it's already been explored, we just update it in place
                                    working_copy.GetNodeMutable(neighbor_index).SetDistance(new_neighbor_distance);
                                }
                                else
                                {
                                    // If it hasn't been explored, we need to update it and add it to the queue
                                    working_copy.GetNodeMutable(neighbor_index).SetDistance(new_neighbor_distance);
                                    queue.push(std::make_pair(neighbor_index, new_neighbor_distance));
                                }
                                // Update that we're the best previous node
                                previous_index_map[(size_t)neighbor_index] = top_node_index;
                                distances[(size_t)neighbor_index] = new_neighbor_distance;
                            }
                            else
                            {
                                // Do nothing
                                continue;
                            }
                        }
                    }
                }
                return std::make_pair(working_copy, std::make_pair(previous_index_map, distances));
            }

            static uint64_t SerializeDijstrasResult(const DijkstrasResult& result, std::vector<uint8_t>& buffer, const std::function<uint64_t(const NodeValueType&, std::vector<uint8_t>&)>& value_serializer)
            {
                const uint64_t start_buffer_size = buffer.size();
                // Serialize the graph
                result.first.SerializeSelf(buffer, value_serializer);
                // Serialize the previous index
                SerializeVector(result.second.first, std::bind(arc_helpers::SerializeFixedSizePOD<uint64_t>, std::placeholders::_1, std::placeholders::_2));
                // Serialze the distances
                SerializeVector(result.second.second, std::bind(arc_helpers::SerializeFixedSizePOD<uint64_t>, std::placeholders::_1, std::placeholders::_2));
                // Figure out how many bytes were written
                const uint64_t end_buffer_size = buffer.size();
                const uint64_t bytes_written = end_buffer_size - start_buffer_size;
                return bytes_written;
            }

            static std::pair<DijkstrasResult, uint64_t> DijstrasResult(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<NodeValueType, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
            {
                uint64_t current_position = current;
                // Deserialize the graph itself
                std::pair<DijkstrasResult, uint64_t> deserialized;
                const std::pair<Graph<NodeValueType, Allocator>, uint64_t> graph_deserialized = Graph<NodeValueType, Allocator>::Deserialize(buffer, current_position, value_deserializer);
                deserialized.first.first = graph_deserialized.first;
                current_position += graph_deserialized.second;
                // Deserialize the previous index
                const std::pair<std::vector<int64_t>, uint64_t> prev_index_deserialized = arc_helpers::DeserializeVector<int64_t>(buffer, current_position, std::bind(arc_helpers::DeserializeFixedSizePOD<uint64_t>, std::placeholders::_1, std::placeholders::_2));
                deserialized.first.second.first = prev_index_deserialized.first;
                current_position += prev_index_deserialized.second;
                // Deserialize the distances
                const std::pair<std::vector<double>, uint64_t> distance_deserialized = arc_helpers::DeserializeVector<double>(buffer, current_position, std::bind(arc_helpers::DeserializeFixedSizePOD<double>, std::placeholders::_1, std::placeholders::_2));
                deserialized.first.second.second = distance_deserialized.first;
                current_position += distance_deserialized.second;
                // Figure out how many bytes were read
                deserialized.second = current_position - current;
                return deserialized;
            }
    };

    template<typename NodeValueType, typename Allocator=std::allocator<NodeValueType>>
    class SimpleGraphAstar
    {
    protected:

        SimpleGraphAstar() {}

    public:

        static arc_helpers::AstarResult PerformLazyAstar(const Graph<NodeValueType, Allocator>& graph, const int64_t start_index, const int64_t goal_index, const std::function<bool(const Graph<NodeValueType, Allocator>&, const GraphEdge&)>& edge_validity_check_fn, const std::function<double(const Graph<NodeValueType, Allocator>&, const GraphEdge&)>& distance_fn, const std::function<double(const NodeValueType&, const NodeValueType&)>& heuristic_fn, const bool limit_pqueue_duplicates)
        {
            // Enforced sanity checks
            if ((start_index < 0) && (start_index >= (int64_t)graph.GetNodesImmutable().size()))
            {
                throw std::invalid_argument("Start index out of range");
            }
            if ((goal_index < 0) && (goal_index >= (int64_t)graph.GetNodesImmutable().size()))
            {
                throw std::invalid_argument("Goal index out of range");
            }
            if (start_index == goal_index)
            {
                throw std::invalid_argument("Start and goal indices must be different");
            }
            // Make helper function
            const auto heuristic_function = [&] (const int64_t node_index) { return heuristic_fn(graph.GetNodeImmutable(node_index).GetValueImmutable(), graph.GetNodeImmutable(goal_index).GetValueImmutable()); };
            // Setup
            std::priority_queue<arc_helpers::AstarPQueueElement, std::vector<arc_helpers::AstarPQueueElement>, arc_helpers::CompareAstarPQueueElementFn> queue;
            // Optional map to reduce the number of duplicate items added to the pqueue
            // Key is the node index in the provided graph
            // Value is cost-to-come
            std::unordered_map<int64_t, double> queue_members_map;
            // Key is the node index in the provided graph
            // Value is a pair<backpointer, cost-to-come>
            // backpointer is the parent index in the provided graph
            std::unordered_map<int64_t, std::pair<int64_t, double>> explored;
            // Initialize
            queue.push(arc_helpers::AstarPQueueElement(start_index, -1, 0.0, heuristic_function(start_index)));
            if (limit_pqueue_duplicates)
            {
                queue_members_map[start_index] = 0.0;
            }
            // Search
            while (queue.size() > 0)
            {
                // Get the top of the priority queue
                const arc_helpers::AstarPQueueElement top_node = queue.top();
                queue.pop();
                // Remove from queue map if necessary
                if (limit_pqueue_duplicates)
                {
                    queue_members_map.erase(top_node.NodeID());
                }
                // Check if the node has already been discovered
                const auto node_explored_find_itr = explored.find(top_node.NodeID());
                // We have not been here before, or it is cheaper now
                const bool node_in_explored = (node_explored_find_itr != explored.end());
                const bool explored_node_is_better = (node_in_explored) ? (top_node.CostToCome() >= node_explored_find_itr->second.second) : false;
                if (!explored_node_is_better)
                {
                    // Add to the explored list
                    explored[top_node.NodeID()] = std::make_pair(top_node.Backpointer(), top_node.CostToCome());
                    // Check if we have reached the goal
                    if (top_node.NodeID() == goal_index)
                    {
                        break;
                    }
                    // Explore and add the children
                    const std::vector<GraphEdge>& out_edges = graph.GetNodeImmutable(top_node.NodeID()).GetOutEdgesImmutable();
                    for (size_t out_edge_idx = 0; out_edge_idx < out_edges.size(); out_edge_idx++)
                    {
                        // Get the next potential child node
                        const GraphEdge& current_out_edge = out_edges[out_edge_idx];
                        const int64_t child_node_index = current_out_edge.GetToIndex();
                        // Check if the top node->child edge is valid
                        if (edge_validity_check_fn(graph, current_out_edge))
                        {
                            // Compute the cost-to-come for the new child
                            const double parent_cost_to_come = top_node.CostToCome();
                            const double parent_to_child_cost = distance_fn(graph, current_out_edge);
                            const double child_cost_to_come = parent_cost_to_come + parent_to_child_cost;
                            // Check if the child state has already been explored
                            const auto child_explored_find_itr = explored.find(child_node_index);
                            // It is not in the explored list, or is there with a higher cost-to-come
                            const bool child_in_explored = (child_explored_find_itr != explored.end());
                            const bool explored_child_is_better = (child_in_explored) ? (child_cost_to_come >= child_explored_find_itr->second.second) : false;
                            // Check if the child state is already in the queue
                            bool queue_is_better = false;
                            if (limit_pqueue_duplicates)
                            {
                                const auto queue_members_map_itr = queue_members_map.find(child_node_index);
                                const bool in_queue = (queue_members_map_itr != queue_members_map.end());
                                queue_is_better = (in_queue) ? (child_cost_to_come >= queue_members_map_itr->second) : false;
                            }
                            // Only add the new state if we need to
                            if (!explored_child_is_better && !queue_is_better)
                            {
                                // Compute the heuristic for the child
                                const double child_heuristic = heuristic_function(child_node_index);
                                // Compute the child value
                                const double child_value = child_cost_to_come + child_heuristic;
                                queue.push(arc_helpers::AstarPQueueElement(child_node_index, top_node.NodeID(), child_cost_to_come, child_value));
                            }
                        }
                    }
                }
            }
            return arc_helpers::ExtractAstarResult(explored, start_index, goal_index);
        }

        static arc_helpers::AstarResult PerformLazyAstar(const Graph<NodeValueType, Allocator>& graph, const int64_t start_index, const int64_t goal_index, const std::function<bool(const NodeValueType&, const NodeValueType&)>& edge_validity_check_fn, const std::function<double(const NodeValueType&, const NodeValueType&)>& distance_fn, const std::function<double(const NodeValueType&, const NodeValueType&)>& heuristic_fn, const bool limit_pqueue_duplicates)
        {
            const auto edge_validity_check_function = [&] (const Graph<NodeValueType, Allocator>& search_graph, const GraphEdge& edge) { return edge_validity_check_fn(search_graph.GetNodeImmutable(edge.GetFromIndex()).GetValueImmutable(), search_graph.GetNodeImmutable(edge.GetToIndex()).GetValueImmutable()); };
            const auto distance_function = [&] (const Graph<NodeValueType, Allocator>& search_graph, const GraphEdge& edge) { return distance_fn(search_graph.GetNodeImmutable(edge.GetFromIndex()).GetValueImmutable(), search_graph.GetNodeImmutable(edge.GetToIndex()).GetValueImmutable()); };
            return PerformLazyAstar(graph, start_index, goal_index, edge_validity_check_function, distance_function, heuristic_fn, limit_pqueue_duplicates);
        }

        static arc_helpers::AstarResult PerformAstar(const Graph<NodeValueType, Allocator>& graph, const int64_t start_index, const int64_t goal_index, const std::function<double(const NodeValueType&, const NodeValueType&)>& heuristic_fn, const bool limit_pqueue_duplicates)
        {
            const auto edge_validity_check_function = [&] (const Graph<NodeValueType, Allocator>& search_graph, const GraphEdge& edge) { UNUSED(search_graph); if (edge.GetWeight() < std::numeric_limits<double>::infinity()) { return true; } else { return false; } };
            const auto distance_function = [&] (const Graph<NodeValueType, Allocator>& search_graph, const GraphEdge& edge) { UNUSED(search_graph); return edge.GetWeight(); };
            return PerformLazyAstar(graph, start_index, goal_index, edge_validity_check_function, distance_function, heuristic_fn, limit_pqueue_duplicates);
        }
    };

}

#endif // DIJKSTRAS_HPP


#include "NumberClRA.h"

#include "NumberMpq.h"
#include "NumberMpz.h"

#include <boost/numeric/interval/interval.hpp>
#include <boost/numeric/interval/policies.hpp>
#include <boost/numeric/interval/arith2.hpp>
#include <boost/numeric/interval/checking.hpp>

#include <limits>

namespace carl {

#ifdef USE_CLN_NUMBERS

		//constructs a/b:
		//(this looks hacky.. seems to be the only really functioning way though: take the integers as strings, put the sign at the front and construct
		//cl_RA from the string "[-]a/b")
		Number<cln::cl_RA>::Number(const Number<cln::cl_I>& a, const Number<cln::cl_I>& b):
			Number(cln::cl_RA(a) / b) {}

		Number<cln::cl_RA>::Number(const Number<cln::cl_I>& n): Number(n.getValue()) {}
		//Number(const cln::cl_I& n) { mData = cln::cl_RA(n); }

		Number<cln::cl_RA>::Number(const Number<mpq_class>& n) : Number(cln::cl_RA(n.toString().c_str())) {}
		Number<cln::cl_RA>::Number(const Number<mpz_class>& n) : Number(cln::cl_RA(n.toString().c_str())) {}

		//this can maybe be done such that it's the same as for mpq_class
    	   Number<cln::cl_RA>::Number(const std::string& s) {
		//here, we need to distinguish two cases: we want to deal with normal floating point inputs (as "3.5") but also fractions ("7/2")
		if (s.find_first_of('/') == std::string::npos) { //this is the floating point case

			std::vector<std::string> strs;
			boost::split(strs, s, boost::is_any_of("."));

			if(strs.size() > 2)
			{
			    throw std::invalid_argument("More than one delimiter in the string.");
			}
			cln::cl_RA result(0);
			if(!strs.front().empty())
			{
			    result += cln::cl_RA(strs.front().c_str());
			}
			if(strs.size() > 1)
			{
			    result += (cln::cl_RA(strs.back().c_str())/carl::pow(cln::cl_RA(10),static_cast<unsigned>(strs.back().size())));
			}
			mData = cln::cl_RA(result);
		} else { //the case where we have a fraction as input
			cln::cl_read_flags flags = {cln::syntax_rational, cln::lsyntax_all, 10, {cln::default_float_format, cln::default_float_format, false}};
			std::istringstream istr(s);
			mData = cln::cl_RA(cln::read_rational(istr,flags));
		}
	    }
	 /*   Number<cln::cl_RA>::Number(double n) {
		switch (std::fpclassify(n)) {
		    case FP_NORMAL: // normalized are fully supported
		        mData = cln::rationalize(convert<mpq_class, cln::cl_RA>(n));
		    case FP_SUBNORMAL: { // subnormals result in underflows, hence the value of the double is 0.f, where f is the significand precision
					static_assert(sizeof(n) == 8, "double is assumed to be eight bytes wide.");
		        sint significandBits = reinterpret_cast<sint>(&n);
		        significandBits = (significandBits << 12) >> 12;
		        if( n < 0 )
		            significandBits = -significandBits;
		        mData = cln::cl_RA( significandBits ) * ONE_DIVIDED_BY_10_TO_THE_POWER_OF_52;
		}
		case FP_ZERO:
		    mData = cln::cl_RA(0);
		case FP_NAN: // NaN and infinite are not supported
		case FP_INFINITE:
		    assert(false);
		    break;
		}
		mData = cln::cl_RA(0);
	}

	Number<cln::cl_RA>::Number(float n) {
		switch (std::fpclassify(n))
		{
		    case FP_NORMAL: // normalized are fully supported
		        mData = cln::rationalize(convert<mpq_class, cln::cl_RA>(n));
		    case FP_SUBNORMAL: { // subnormals result in underflows, hence the value of the double is 0.f, where f is the significand precision
					static_assert(sizeof(n) == 4, "float is assumed to be four bytes wide.");
		        sint significandBits = reinterpret_cast<sint>(&n);
		        significandBits = (significandBits << 9) >> 9;
		        if( n < 0 )
		            significandBits = -significandBits;
		        mData = cln::cl_RA( significandBits ) * ONE_DIVIDED_BY_10_TO_THE_POWER_OF_23;
		}
		case FP_ZERO:
		    mData = cln::cl_RA(0);
		case FP_NAN: // NaN and infinite are not supported
		case FP_INFINITE:
		    assert(false);
		    break;
		}
		mData = cln::cl_RA(0);
	} */

	//TODO: why not use the standard output of cln here?! Surely that works?
	    std::string Number<cln::cl_RA>::toString(bool _infix) const
	    {

		std::stringstream s;
		bool negative = (mData < cln::cl_RA(0));
		if(negative) s << "(-" << (_infix ? "" : " ");
		if(_infix) s << this->abs().mData;
		else
		{
		    cln::cl_I d = cln::denominator(mData);
		    if(mData != carl::constant_one<cln::cl_I>().get()) s << "(/ " << cln::abs(cln::numerator(mData)) << " " << cln::abs(d) << ")";
		    else s << this->abs().getValue();
		}
		if(negative)
		    s << ")";
		return s.str();
	    }

	 bool Number<cln::cl_RA>::sqrt_exact(Number<cln::cl_RA>& b) const
	 {
		if( mData < 0 ) return false;
		cln::cl_RA result;
		bool boolResult = cln::sqrtp( mData, &result );
		b = Number(result);
		return boolResult;
	 }

	//this is the same as for mpq_class
	 Number<cln::cl_RA> Number<cln::cl_RA>::sqrt() const
	 {
		std::pair<Number<cln::cl_RA>, Number<cln::cl_RA>> r = this->sqrt_safe();
		return Number((r.first.getValue() + r.second.getValue()) / 2); //TODO: remove the getValue once operators are implemented!!
	 }

	cln::cl_RA Number<cln::cl_RA>::scaleByPowerOfTwo(const cln::cl_RA& a, int exp) const {
		if (exp > 0) {
			return cln::cl_RA(cln::numerator(a) << exp) / cln::denominator(a);
		} else if (exp < 0) {
			return cln::cl_RA(cln::numerator(a)) / (cln::denominator(a) << -exp);
		}
		return a;
	}

	    std::pair<Number<cln::cl_RA>, Number<cln::cl_RA>> Number<cln::cl_RA>::sqrt_safe() const
	    {
		assert( mData >= 0 );
		cln::cl_RA exact_root;
		if (cln::sqrtp(mData, &exact_root)) {
		    // root can be computed exactly.
		    return std::make_pair(Number(exact_root), Number(exact_root));
		} else {
				auto factor = int(cln::integer_length(cln::denominator(mData))) - int(cln::integer_length(cln::numerator(mData)));
				if (cln::oddp(factor)) factor += 1;
				cln::cl_RA n = scaleByPowerOfTwo(mData, factor);
				double dn = cln::double_approx(n);
				cln::cl_RA nra = cln::rationalize(dn);
				boost::numeric::interval<double> i;
				if (nra > n) {
					i.assign(cln::double_approx(2*n-nra), dn);
					assert(2*n-nra <= n);
				} else {
					i.assign(dn, cln::double_approx(2*n-nra));
					assert(n <= 2*n-nra);
				}
				i = boost::numeric::sqrt(i);
				i.assign(
					std::nexttoward(i.lower(), -std::numeric_limits<double>::infinity()),
					std::nexttoward(i.upper(), std::numeric_limits<double>::infinity())
				);
				factor = factor / 2;
				cln::cl_RA lower = scaleByPowerOfTwo(cln::rationalize(i.lower()), -factor);
				cln::cl_RA upper = scaleByPowerOfTwo(cln::rationalize(i.upper()), -factor);
				assert(lower*lower <= mData);
				assert(mData <= upper*upper);
				return std::make_pair(Number(lower), Number(upper));
		}
	    }

	    std::pair<Number<cln::cl_RA>, Number<cln::cl_RA>> Number<cln::cl_RA>::sqrt_fast() const
	    {
			assert(mData >= 0);
			cln::cl_RA exact_root;
			if (cln::sqrtp(mData, &exact_root)) {
				// root can be computed exactly.
				return std::make_pair(Number(exact_root), Number(exact_root));
			} else {
				// compute an approximation with sqrt(). we can assume that the surrounding integers contain the actual root.
				//auto factor = cln::integer_length(cln::denominator(a)) - cln::integer_length(cln::numerator(a));
				//if (cln::oddp(factor)) factor += 1;
				cln::cl_I lower = cln::floor1(cln::sqrt(toLF(mData)));
			        cln::cl_I upper = lower + 1;
			        assert(cln::expt_pos(lower,2) < mData);
			        assert(cln::expt_pos(upper,2) > mData);
			        return std::make_pair(Number(lower), Number(upper));
			}
	    }

#endif

}


#include <Python.h>
#include <omp.h>
#include <vector>
#include <set>
#include <map>
#include <string>
#include <iostream>
#include <boost/algorithm/minmax_element.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>

const double machine_epsilon = 1e-8;

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

struct DATUM_X_T
{
    wstring x;
    double proba;
};
typedef vector<DATUM_X_T> DATA_X_T;
struct DATUM_XY_T
{
    wstring x;
    wstring y;
    double proba;
};
typedef vector<DATUM_XY_T> DATA_XY_T;
struct FEATURE1_T
{
    wchar_t prev_label;
    wchar_t label;
};
struct FEATURE2_T
{
    vector<int> positions;
    vector<wchar_t> values;
    wchar_t label;
};
typedef pair<vector<FEATURE1_T>, vector<FEATURE2_T>> FEATURES_T;
typedef map<wchar_t, size_t> YSET_T;

struct X_MS_ITEM_T
{
    wstring x;
    double proba;
    vector<ublas::matrix<double>> ms;
    ublas::matrix<double> alpha;
    ublas::matrix<double> beta;
    double logz;
};

inline double logsumexp(ublas::vector<double>& x )
{
    double maxval = *max_element(x.begin(), x.end());
    double val = 0;
    for(ublas::vector<double>::iterator it=x.begin(); it<x.end(); ++it)
        val += exp(*it - maxval);
    val = maxval + log(val);
    return val;
}

YSET_T pyyset_as_cpp(PyObject* pyyset)
{
    YSET_T yset;
    PyObject *key, *value;
    Py_ssize_t pos = 0;
    while (PyDict_Next(pyyset, &pos, &key, &value))
    {
        wchar_t c;
        PyUnicode_AsWideChar(key, &c, 1);
        size_t i = PyLong_AsSize_t(value);
        yset.insert(make_pair(c, i));
    }
    return yset;
}

DATA_X_T pydata_x_as_cpp(PyObject* pydata_x)
{
    DATA_X_T result;
    for(Py_ssize_t i=0, size = PyList_Size(pydata_x); i<size; ++i)
    {
        PyObject* pyxp = PyList_GetItem(pydata_x, i);
        wstring x( PyUnicode_AsUnicode( PyTuple_GetItem( pyxp, 0) ) );
        double proba = PyFloat_AsDouble( PyTuple_GetItem( pyxp, 1) );
        result.push_back({x, proba});
    }
    return result;
}

DATA_XY_T pydata_xy_as_cpp(PyObject* pydata_xy)
{
    DATA_XY_T result;
    for(Py_ssize_t i=0, size=PyList_Size(pydata_xy); i<size; ++i)
    {
        PyObject* pyxyp = PyList_GetItem( pydata_xy, i );
        PyObject* pyxy = PyTuple_GetItem( pyxyp, 0 );
        wstring x( PyUnicode_AsUnicode( PyTuple_GetItem( pyxy, 0 ) ) );
        wstring y( PyUnicode_AsUnicode( PyTuple_GetItem( pyxy, 1 ) ) );
        double proba = PyFloat_AsDouble( PyTuple_GetItem( pyxyp, 1 ) );
        assert( x.length() == y.length() );
        //wcout << x << y << proba << endl;
        result.push_back({x, y, proba});
    }
    return result;
}

vector<ublas::matrix<double>> xmatrices(const YSET_T& yset,const FEATURES_T& features,const ublas::vector<double>& weights,const wstring& x);
ublas::matrix<double> xalphas(const vector<ublas::matrix<double>>& ms);
ublas::matrix<double> xbetas(const vector<ublas::matrix<double>>& ms);

FEATURES_T pyfeatures_as_cpp(PyObject* pyfeatures)
{
    vector<FEATURE1_T> features1;
    vector<FEATURE2_T> features2;
    Py_ssize_t K = PyList_Size(pyfeatures);
    for(Py_ssize_t k=0; k<K; ++k)
    {
        PyObject* pyfeature = PyList_GetItem(pyfeatures,k);
        Py_ssize_t t = PyTuple_Size(pyfeature);
        if(t==2)
        {
            wchar_t prev_label = *PyUnicode_AsUnicode(PyTuple_GetItem(pyfeature, 0));
            wchar_t label = *PyUnicode_AsUnicode(PyTuple_GetItem(pyfeature, 1));
            features1.push_back({prev_label, label});
        }
        else if(t==3)
        {
            wchar_t label = *PyUnicode_AsUnicode(PyTuple_GetItem(pyfeature, 2));
            PyObject* pytemp = PyTuple_GetItem(pyfeature, 0);
            PyObject* pyValue = PyTuple_GetItem(pyfeature, 1);
            assert( PyTuple_Size(pytemp) == PyTuple_Size(pyValue) );
            vector<int> positions;
            vector<wchar_t> values;
            for(Py_ssize_t i=0; i<PyTuple_Size(pytemp); ++i)
            {
                int pos = PyLong_AsLong(PyTuple_GetItem(pytemp, i));
                wchar_t value = *PyUnicode_AsUnicode(PyTuple_GetItem(pyValue, i));
                positions.push_back(pos);
                values.push_back(value);
            }
            features2.push_back({positions, values, label});
        }
        else
        {
            cout << "feature size error! " << t << endl;
            exit(-1);
        }
    }
    return make_pair(features1, features2);
}

inline bool fkyyxi(const FEATURES_T& features, const size_t k,const wchar_t prev_label,const wchar_t label,const wstring& x,const size_t i)
{
    size_t K1 = features.first.size();
    if ( k<K1 )
        return features.first[k].prev_label == prev_label and features.first[k].label == label ? 1 : 0;
    if( features.second[k-K1].label == label )
    {
        size_t k3 = k-K1, n = x.length();
        for(size_t t=0, T=features.second[k3].positions.size(); t<T; ++t)
        {
            int pos = (int)i+features.second[k3].positions[t];
            if(((pos>=0 && pos<(int)n) ? x[pos] : L'\0') != features.second[k3].values[t]) return 0;
        }
        return 1;
    }
    return 0;
}

inline double featurekxy(const FEATURES_T& features,const  size_t k, const wstring& x,const wstring& y)
{
    assert( x.length() == y.length() );
    double value = 0.0;
    wchar_t prev_label = L'^';
    for(size_t i=0, n = x.length(); i<=n; ++i)
    {
        wchar_t label = i < n? y.at(i) : L'$';
        value += fkyyxi(features, k, prev_label, label, x, i);
        prev_label = label;
    }
    return value;
}

ublas::vector<double> stat_all_xy_features_values(const FEATURES_T& features, const DATA_XY_T& data_xy)
{
    size_t K1 = features.first.size();
    size_t K2 = features.second.size();
    ublas::vector<double> values(K1+K2, 0);
    #pragma omp parallel for
    for(size_t k=0; k<K1+K2; ++k)
    {
        double value = 0;
        for(DATA_XY_T::const_iterator it=data_xy.begin(); it<data_xy.end(); ++it)
            value += it->proba * featurekxy(features, k, it->x, it->y);
        values(k) = value;
    }
    return values;
}

ublas::vector<double> doublelist_as_cpp(PyObject* pydoublelist)
{
    ublas::vector<double> vec(PyList_Size(pydoublelist));
    for(Py_ssize_t i=0; i<PyList_Size(pydoublelist); ++i)
    {
        vec(i) = PyFloat_AsDouble(PyList_GetItem(pydoublelist, i));
    }
    return vec;
}

static PyObject* stat_all_xy_features_values(PyObject* self, PyObject* args)
{
    PyObject* pyfeatures = NULL;
    PyObject* pydata_xy  = NULL;
    if (!PyArg_ParseTuple(args, "OO", &pyfeatures, &pydata_xy))
        return NULL;
    FEATURES_T features = pyfeatures_as_cpp(pyfeatures);
    DATA_XY_T data_xy = pydata_xy_as_cpp(pydata_xy);
    ublas::vector<double> values = stat_all_xy_features_values(features, data_xy);
    ublas::vector<double>::size_type k=0, K=values.size();
    PyObject* pyvalues = PyList_New(K);
    for(; k<K; ++k) PyList_SetItem(pyvalues, k, PyFloat_FromDouble( values(k) ) );
    return pyvalues;
}

double model_values(const ublas::vector<double>& weights,const YSET_T& yset,const FEATURES_T& features, const DATA_X_T& data_x)
{
    double val = 0;
    #pragma omp parallel for reduction(+:val)
    for( DATA_X_T::const_iterator it=data_x.begin(); it<data_x.end(); ++it )
    {
        vector<ublas::matrix<double>> ms = xmatrices(yset, features, weights, it->x);
        ublas::matrix<double> alpha = xalphas(ms);
        //ublas::matrix<double> beta = xbetas(ms);
        ublas::vector<double> alphalastrow = ublas::matrix_row<ublas::matrix<double>>(alpha, alpha.size1()-1);
        //ublas::vector<double> betalastrow = ublas::matrix_row<ublas::matrix<double>>(beta, 0);
        double logz1 = logsumexp(alphalastrow);
        //double logz2 = logsumexp(betalastrow);
        //cout << logz1 << " , " << logz2 << " abs:" << abs(logz1 - logz2) << endl;
        //assert( abs(logz1 - logz2) < machine_epsilon );
        val += it->proba * logz1;
    }
    return val;
}

static PyObject* model_values(PyObject* self, PyObject* args)
{
    PyObject* pyweights = NULL;
    PyObject* pyyset = NULL;
    PyObject* pyfeatures = NULL;
    PyObject* pydata_x  = NULL;
    if (!PyArg_ParseTuple(args, "OOOO", &pyweights, &pyyset, &pyfeatures, &pydata_x))
        return NULL;
    ublas::vector<double> weights = doublelist_as_cpp(pyweights);
    YSET_T yset = pyyset_as_cpp(pyyset);
    FEATURES_T features = pyfeatures_as_cpp(pyfeatures);
    DATA_X_T data_x = pydata_x_as_cpp(pydata_x);
    return PyFloat_FromDouble(model_values(weights, yset, features, data_x));
}

inline double yyxi(const FEATURES_T& features,const ublas::vector<double>& weights,const wchar_t prev_label,const wchar_t label,const wstring& x,const size_t i)
{
    static map<tuple<wchar_t, wchar_t, wstring, size_t>, vector<size_t>> yyxi_cache_helper;
    double val=0;
    auto key = make_tuple(prev_label, label, x, i);
    if( yyxi_cache_helper.find(key) != yyxi_cache_helper.end() )
    {
        vector<size_t> ks = yyxi_cache_helper[key];
        for(vector<size_t>::iterator it=ks.begin(); it<ks.end(); ++it)
            val += weights[*it];
        return val;
    }
    for(size_t k=0; k<weights.size(); ++k)
    {
        if( fkyyxi(features, k, prev_label, label, x, i) > 0.5 )
        {
            if ( yyxi_cache_helper.find(key) == yyxi_cache_helper.end() )
            {
                #pragma omp critical
                yyxi_cache_helper.insert(make_pair(key, vector<size_t>()));
            }
            #pragma omp critical
            yyxi_cache_helper[key].push_back(k);
            val += weights[k];
        }
    }
    return val;
}

inline ublas::matrix<double> xmatrixi(const YSET_T& yset,const FEATURES_T& features,const ublas::vector<double>& weights,const wstring& x,const size_t i)
{
    ublas::matrix<double> m(yset.size(), yset.size());
    for(YSET_T::const_iterator y0it=yset.begin(); y0it!=yset.end(); ++y0it)
    {
        for(YSET_T::const_iterator y1it=yset.begin(); y1it!=yset.end(); ++y1it)
        {
            if(i==0)
            {
                m(y0it->second, y1it->second) = (y0it->second==0) ? yyxi(features, weights, L'^', y1it->first, x, i) : 0;
            }
            else if(i==x.length())
            {
                m(y0it->second, y1it->second) = (y1it->second==0) ? yyxi(features, weights, y0it->first, L'$', x, i) : 0;
            }
            else
            {
                m(y0it->second, y1it->second) = yyxi(features, weights, y0it->first, y1it->first, x, i);
            }
        }
    }
    return m;
}

inline vector<ublas::matrix<double>> xmatrices(const YSET_T& yset,const FEATURES_T& features,const ublas::vector<double>& weights,const wstring& x)
{
    size_t n = x.length();
    vector<ublas::matrix<double>> ms(n+1);
    for(size_t i=0; i<=n; ++i)
        ms.at(i) = xmatrixi(yset, features, weights, x, i);
    return ms;
}

inline ublas::matrix<double> xalphas(const vector<ublas::matrix<double>>& ms)
{
    size_t mslen = ms.size();
    ublas::matrix<double> alpha(mslen, ms[0].size1());
    ublas::row<ublas::matrix<double>>(alpha, 0) = ublas::row<ublas::matrix<double>>(ms[0], 0);
    for(size_t i=1; i<mslen-1; ++i)
    {
        for(size_t j=0; j<ms[i].size2(); ++j)
        {
            ublas::vector<double> values = ublas::row<ublas::matrix<double>>(alpha, i-1)
                                           + ublas::column<ublas::matrix<double>>(ms[i], j);
            alpha(i, j) = logsumexp( values );
        }
    }
    ublas::row<ublas::matrix<double>>(alpha, mslen-1) = ublas::row<ublas::matrix<double>>(alpha, mslen-2)
                                   + ublas::column<ublas::matrix<double>>(ms[mslen-1], 0);
    return alpha;
}

inline ublas::matrix<double> xbetas(const vector<ublas::matrix<double>>& ms)
{
    size_t mslen = ms.size();
    ublas::matrix<double> beta(mslen, ms[0].size1());
    ublas::row<ublas::matrix<double>>(beta, mslen-1) = ublas::column<ublas::matrix<double>>(ms[mslen-1], 0);
    for(size_t i=mslen-2; i>0; --i)
    {
        for(size_t j=0; j<ms[i].size1(); ++j)
        {
            ublas::vector<double> values = ublas::row<ublas::matrix<double>>(ms[i], j)
                                           + ublas::row<ublas::matrix<double>>(beta, i+1);
            beta(i, j) = logsumexp( values );
        }
    }
    ublas::row<ublas::matrix<double>>(beta, 0) = ublas::row<ublas::matrix<double>>(ms[0], 0)
                                   + ublas::row<ublas::matrix<double>>(beta, 1);
    return beta;
}

inline double xyiiproba(const YSET_T& yset,const vector<ublas::matrix<double>>& ms,const ublas::matrix<double>& alpha,const ublas::matrix<double>& beta,const size_t s0,const size_t s1,const size_t i,const double logz)
{
    //assert( 0<=i and i<ms.size() );
    double p = -logz;
    if (i==0)
    {
        p += ms[i](0, s1) + beta(i+1, s1);
    }
    else if(i==ms.size()-1)
    {
        p += alpha(i-1, s0) + ms[i](s0, 0);
    }
    else
    {
        p += alpha(i-1, s0) + ms[i](s0, s1) + beta(i+1, s1);
    }
    return exp( p );
}

inline double xyproba(const YSET_T& yset, const vector<ublas::matrix<double>>& ms, const double logz,const wstring& y)
{
    double v=0;
    v += ms.front()(0, yset.at(y.front()));
    for(size_t i=1; i<y.size(); ++i)
        v+=ms.at(i)(yset.at(y.at(i-1)), yset.at(y.at(i)));
    v += ms.back()( yset.at(y.back()), 0);
    return exp( v - logz );
}

inline double kx(const YSET_T& yset,const FEATURES_T& features,const size_t k,const wstring& x,const vector<ublas::matrix<double>>& ms,const ublas::matrix<double>& alpha,const ublas::matrix<double>& beta,const double logz)
{
    //assert( x.length()+1 == ms.size() );
    double v = 0;
    size_t n = x.length();

    for(YSET_T::const_iterator y1it=yset.begin(); y1it!=yset.end(); ++y1it)
        if( fkyyxi(features, k, L'^', y1it->first, x, 0) )
            v += xyiiproba(yset, ms, alpha, beta, 0, y1it->second, 0, logz);

    for(size_t i=1; i<n; ++i)
        for(YSET_T::const_iterator y0it=yset.begin(); y0it!=yset.end(); ++y0it)
            for(YSET_T::const_iterator y1it=yset.begin(); y1it!=yset.end(); ++y1it)
                if( fkyyxi(features, k, y0it->first, y1it->first, x, i) )
                    v += xyiiproba(yset, ms, alpha, beta, y0it->second, y1it->second, i, logz);

    for(YSET_T::const_iterator y0it=yset.begin(); y0it!=yset.end(); ++y0it)
        if( fkyyxi(features, k, y0it->first, L'$', x, n) )
            v += xyiiproba(yset, ms, alpha, beta, y0it->second, 0, n, logz);

    return v;
}

inline double allxv(const YSET_T& yset, const FEATURES_T& features,const size_t k,const vector<X_MS_ITEM_T>& malphabetaz)
{
    double grad=0;
    for(vector<X_MS_ITEM_T>::const_iterator it=malphabetaz.cbegin(); it<malphabetaz.cend(); ++it)
        grad += it->proba * kx( yset, features, k, it->x, it->ms, it->alpha, it->beta, it->logz );
    return grad;
}

ublas::vector<double> model_grads(const YSET_T& yset,const FEATURES_T& features,const ublas::vector<double>& weights,const DATA_X_T& data_x)
{
    size_t K = weights.size();
    ublas::vector<double> grads(K);
    vector<X_MS_ITEM_T> malphabetaz( data_x.size() );
    #pragma omp parallel for
    for(DATA_X_T::size_type i=0; i<data_x.size(); ++i)
    {
        vector<ublas::matrix<double>> ms = xmatrices( yset, features, weights, data_x.at(i).x );
        ublas::matrix<double> alpha = xalphas(ms);
        ublas::matrix<double> beta = xbetas(ms);
        ublas::vector<double> alphalastrow = ublas::matrix_row<ublas::matrix<double>>(alpha, alpha.size1()-1);
        ublas::vector<double> betafirstrow = ublas::matrix_row<ublas::matrix<double>>(beta, 0);
        double logz1 = logsumexp( alphalastrow );
        double logz2 = logsumexp( betafirstrow );
        assert( abs(logz1 - logz2) < machine_epsilon );
        assert( data_x.at(i).x.length()+1 == ms.size() );
        malphabetaz.at(i) = {data_x.at(i).x, data_x.at(i).proba, ms, alpha, beta, logz1};
    }
    #pragma omp parallel for
    for(size_t k=0; k<K; ++k)
        grads(k) = allxv(yset, features, k, malphabetaz);
    return grads;
}

ublas::vector<double> model_grads1(const YSET_T& yset,const FEATURES_T& features,const ublas::vector<double>& weights,const DATA_X_T& data_x)
{
    size_t K = weights.size();
    ublas::vector<double> grads(K);
    vector<X_MS_ITEM_T> malphabetaz( data_x.size() );
    #pragma omp parallel for
    for(DATA_X_T::size_type i=0; i<data_x.size(); ++i)
    {
        vector<ublas::matrix<double>> ms = xmatrices( yset, features, weights, data_x.at(i).x );
        ublas::matrix<double> alpha = xalphas(ms);
        ublas::matrix<double> beta = xbetas(ms);
        ublas::vector<double> alphalastrow = ublas::matrix_row<ublas::matrix<double>>(alpha, alpha.size1()-1);
        ublas::vector<double> betafirstrow = ublas::matrix_row<ublas::matrix<double>>(beta, 0);
        double logz1 = logsumexp( alphalastrow );
        double logz2 = logsumexp( betafirstrow );
        assert( abs(logz1 - logz2) < machine_epsilon );
        assert( data_x.at(i).x.length()+1 == ms.size() );
        malphabetaz.at(i) = {data_x.at(i).x, data_x.at(i).proba, ms, alpha, beta, logz1};
    }
    struct XS0S1I_T
    {
        size_t s0;
        size_t s1;
        size_t xi;
    };
    typedef vector<XS0S1I_T> XS0S1I_LIST_T;
    struct KX_T
    {
        size_t xnum;
        XS0S1I_LIST_T pxs;
    };
    typedef vector<KX_T> KX_LIST_T;
    static vector<KX_LIST_T> kxtablecache(K);
    static bool kxtablecache_flag = 0;
    if ( !kxtablecache_flag )
    {
        #pragma omp parallel for
        for(size_t k=0; k<K; ++k)
        {
            KX_LIST_T kx;
            for(size_t xnum=0; xnum<data_x.size(); ++xnum)
            {
                XS0S1I_LIST_T xs0s1i;
                wstring x = data_x.at(xnum).x;
                size_t n = x.length();
                for(YSET_T::const_iterator y1it=yset.begin(); y1it!=yset.end(); ++y1it)
                    if( fkyyxi(features, k, L'^', y1it->first, x, 0) )
                        xs0s1i.push_back({ 0, y1it->second, 0 });
                for(size_t i=1; i<n; ++i)
                    for(YSET_T::const_iterator y0it=yset.begin(); y0it!=yset.end(); ++y0it)
                        for(YSET_T::const_iterator y1it=yset.begin(); y1it!=yset.end(); ++y1it)
                            if( fkyyxi(features, k, y0it->first, y1it->first, x, i) )
                                xs0s1i.push_back({ y0it->second, y1it->second, i });
                for(YSET_T::const_iterator y0it=yset.begin(); y0it!=yset.end(); ++y0it)
                    if( fkyyxi(features, k, y0it->first, L'$', x, n) )
                        xs0s1i.push_back( {y0it->second, 0, n});
                if ( xs0s1i.size() > 0 )
                    kx.push_back({xnum, xs0s1i});
            }
            kxtablecache.at(k) = kx;
        }
        kxtablecache_flag = 1;
    }

    #pragma omp parallel for
    for(size_t k=0; k<K; ++k)
    {
        const KX_LIST_T& kx = kxtablecache.at(k);
        for( KX_LIST_T::const_iterator kxit=kx.begin(); kxit<kx.end(); ++kxit  )
        {
            const X_MS_ITEM_T& xitem = malphabetaz.at(kxit->xnum);
            const XS0S1I_LIST_T& xs0s1i = kxit->pxs;
            double v=0;
            for( XS0S1I_LIST_T::const_iterator it=xs0s1i.begin(); it<xs0s1i.end(); ++it )
            {
                v += xyiiproba(yset, xitem.ms, xitem.alpha, xitem.beta, it->s0, it->s1, it->xi, xitem.logz );
            }
            grads(k) += xitem.proba * v;
        }
    }
    return grads;
}

static PyObject* model_grads(PyObject* self, PyObject* args)
{
    PyObject* pyyset = NULL;
    PyObject* pyfeatures = NULL;
    PyObject* pyweights = NULL;
    PyObject* pydata_x  = NULL;
    if (!PyArg_ParseTuple(args, "OOOO", &pyyset, &pyfeatures, &pyweights, &pydata_x))
        return NULL;
    YSET_T yset = pyyset_as_cpp(pyyset);
    FEATURES_T features = pyfeatures_as_cpp(pyfeatures);
    ublas::vector<double> weights = doublelist_as_cpp(pyweights);
    DATA_X_T data_x = pydata_x_as_cpp(pydata_x);
    ublas::vector<double> grads = model_grads(yset, features, weights, data_x);
    PyObject* pygrads = PyList_New(grads.size());
    for(size_t i=0; i<grads.size(); ++i)
        PyList_SetItem(pygrads, i, PyFloat_FromDouble( grads(i) ) );
    return pygrads;
}

static PyObject* model_grads1(PyObject* self, PyObject* args)
{
    PyObject* pyyset = NULL;
    PyObject* pyfeatures = NULL;
    PyObject* pyweights = NULL;
    PyObject* pydata_x  = NULL;
    if (!PyArg_ParseTuple(args, "OOOO", &pyyset, &pyfeatures, &pyweights, &pydata_x))
        return NULL;
    YSET_T yset = pyyset_as_cpp(pyyset);
    FEATURES_T features = pyfeatures_as_cpp(pyfeatures);
    ublas::vector<double> weights = doublelist_as_cpp(pyweights);
    DATA_X_T data_x = pydata_x_as_cpp(pydata_x);
    ublas::vector<double> grads = model_grads1(yset, features, weights, data_x);
    PyObject* pygrads = PyList_New(grads.size());
    for(size_t i=0; i<grads.size(); ++i)
        PyList_SetItem(pygrads, i, PyFloat_FromDouble( grads(i) ) );
    return pygrads;
}

static PyMethodDef crfextMethods[] =
{
    {"model_grads", model_grads, METH_VARARGS, "grads"},
    {"model_grads1", model_grads1, METH_VARARGS, "grads"},
    {"model_values", model_values, METH_VARARGS, ""},
    {"stat_all_xy_features_values", stat_all_xy_features_values, METH_VARARGS, "stat all features values on all data"},
    {NULL, NULL, 0, NULL}        /* Sentinel */
};

static struct PyModuleDef crfextmodule =
{
    PyModuleDef_HEAD_INIT,
    "crfext",
    NULL,
    -1,
    crfextMethods
};

PyMODINIT_FUNC PyInit_crfext(void)
{
    return PyModule_Create(&crfextmodule);
};