#include "GltfUtil.h"
#include "SimpleMesh.h"
#include "Sphere.h"
#include "VoxelGrid.h"
#include "mathkit.h"
#include <Eigen/Core>
#include <bobyqa.h>
#include <chrono>
#include <fmt/format.h>
#include <fstream>
#include <iomanip>
#include <json.hpp>
#include <random>
#include <spherical_harmonics.h>
#include <unordered_set>
#include <vector>

#ifdef WIN32
#define NOMINMAX
#include <direct.h>
#include <windows.h>
#else
#include <unistd.h>
#endif

struct SphericalHarmonics {
    vec3 L00 {};
    vec3 L11 {};
    vec3 L10 {};
    vec3 L1_1 {};
    vec3 L21 {};
    vec3 L2_1 {};
    vec3 L2_2 {};
    vec3 L20 {};
    vec3 L22 {};
};

struct SphereSet {
    SphereSet(const SimpleMesh& mesh)
        : mesh(mesh)
    {
    }

    std::vector<Sphere> spheres;
    std::vector<SphericalHarmonics> sphereSH;

    const SimpleMesh& mesh;

    struct {
        std::unique_ptr<VoxelGrid> shell;
        std::unique_ptr<VoxelGrid> filled;
        std::unique_ptr<VoxelGrid> inside;
    } grids;
};

struct Triangle {
public:
    Triangle(vec3 v0, vec3 v1, vec3 v2)
    {
        v[0] = v0;
        v[1] = v1;
        v[2] = v2;

        vec3 crossP = cross(v1 - v0, v2 - v0);
        area = length(crossP) / 2.0f;
        normal = normalize(crossP);
    }

    vec3 v[3];
    vec3 normal;
    float area;
};

vec3 getRandomSphereLocationInsideMesh(const VoxelGrid& grid, std::default_random_engine& generator)
{
    aabb3 bounds = grid.gridBounds();
    std::uniform_real_distribution<float> uniformInGridX { bounds.min.x, bounds.max.x };
    std::uniform_real_distribution<float> uniformInGridY { bounds.min.y, bounds.max.y };
    std::uniform_real_distribution<float> uniformInGridZ { bounds.min.z, bounds.max.z };

    vec3 location;
    ivec3 gridCoord;

    do {
        location = { uniformInGridX(generator),
                     uniformInGridY(generator),
                     uniformInGridZ(generator) };
        gridCoord = grid.remapToGridSpace(location, std::round);
    } while (grid.get(gridCoord) == 0);

    return location;
}

void assignSpheresRandomlyInVolume(SphereSet& set, unsigned numSpheres)
{
    auto& grid = *set.grids.inside;
    assert(numSpheres <= grid.numFilledVoxels());

    unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
    std::default_random_engine generator { seed };

    for (size_t i = 0; i < numSpheres; ++i) {
        vec3 location = getRandomSphereLocationInsideMesh(grid, generator);
        Sphere sphere { location, 0.0 };
        set.spheres.push_back(sphere);
    }

    aabb3 bounds = grid.gridBounds();
    std::uniform_real_distribution<float> uniformInGridX { bounds.min.x, bounds.max.x };
    std::uniform_real_distribution<float> uniformInGridY { bounds.min.y, bounds.max.y };
    std::uniform_real_distribution<float> uniformInGridZ { bounds.min.z, bounds.max.z };

    while (set.spheres.size() < numSpheres) {

        float x = uniformInGridX(generator);
        float y = uniformInGridY(generator);
        float z = uniformInGridZ(generator);

        ivec3 gridCoord = grid.remapToGridSpace({ x, y, z }, std::round);
        if (grid.get(gridCoord) > 0) {
            Sphere sphere { vec3(x, y, z), 0.0f };
            set.spheres.push_back(sphere);
        }
    }
}

bool sphereTriangleIntersection(const Sphere& sphere, const vec3& v0, const vec3& v1, const vec3& v2)
{
    // From https://realtimecollisiondetection.net/blog/?p=103

    vec3 P = sphere.origin;
    float r = sphere.radius;
    float rr = r * r;

    vec3 A = v0 - P;
    vec3 B = v1 - P;
    vec3 C = v2 - P;

    vec3 V = cross(B - A, C - A);
    float d = dot(A, V);
    float e = dot(V, V);
    bool sep1 = d * d > rr * e;

    float aa = dot(A, A);
    float ab = dot(A, B);
    float ac = dot(A, C);
    float bb = dot(B, B);
    float bc = dot(B, C);
    float cc = dot(C, C);

    bool sep2 = (aa > rr) & (ab > aa) & (ac > aa);
    bool sep3 = (bb > rr) & (ab > bb) & (bc > bb);
    bool sep4 = (cc > rr) & (ac > cc) & (bc > cc);

    vec3 AB = B - A;
    vec3 BC = C - B;
    vec3 CA = A - C;

    float d1 = ab - aa;
    float d2 = bc - bb;
    float d3 = ac - cc;

    float e1 = dot(AB, AB);
    float e2 = dot(BC, BC);
    float e3 = dot(CA, CA);

    vec3 Q1 = A * e1 - d1 * AB;
    vec3 Q2 = B * e2 - d2 * BC;
    vec3 Q3 = C * e3 - d3 * CA;

    vec3 QC = C * e1 - Q1;
    vec3 QA = A * e2 - Q2;
    vec3 QB = B * e3 - Q3;

    bool sep5 = (dot(Q1, Q1) > rr * e1 * e1) && (dot(Q1, QC) > 0);
    bool sep6 = (dot(Q2, Q2) > rr * e2 * e2) && (dot(Q2, QA) > 0);
    bool sep7 = (dot(Q3, Q3) > rr * e3 * e3) && (dot(Q3, QB) > 0);

    bool separated = sep1 | sep2 | sep3 | sep4 | sep5 | sep6 | sep7;
    return !separated;
}

bool sphereInsideMeshVolume(const Sphere& sphere, const SimpleMesh& mesh)
{
    // NOTE: Implicit, but this assumes that the mesh is one solid blob with no islands!

    // NOTE: To avoid expensive(/not-implemented) checks to see if the sphere centers are inside the volume
    //  of the object, we will assume they are and through other means verify that it will be invariant. The
    //  other means are: 1) spawning points inside the volume, and 2) always making steps small enough that
    //  we wont leave the volume when optimizing.

    if (sphere.radius == 0.0) {
        return true;
    }

    volatile bool result = true;
    #pragma omp parallel for shared(result)
    for (int i = 0; i < mesh.triangleCount(); ++i) {
        if (result) {
            // TODO: Maybe don't reconstruct these every query?!
            vec3 v0, v1, v2;
            mesh.triangle(i, v0, v1, v2);

            if (sphereTriangleIntersection(sphere, v0, v1, v2)) {
                result = false;
            }
        }
    }

    return result;
}

bool sphereSetInsideMeshVolume(const SphereSet& set, const SimpleMesh& mesh)
{
    for (const Sphere& sphere : set.spheres) {
        if (!sphereInsideMeshVolume(sphere, mesh)) {
            return false;
        }
    }
    return true;
}

void expandSpheresMaximally(SphereSet& set)
{
    aabb3 gridBounds = set.grids.shell->gridBounds();
    double gridDiameter = distance(gridBounds.min, gridBounds.max);

    #pragma omp parallel for
    for (int i = 0; i < set.spheres.size(); ++i) {
        Sphere& sphere = set.spheres[i];

        Sphere minSphere { sphere.origin, 0.0 };
        Sphere maxSphere { sphere.origin, gridDiameter };

        assert(sphereInsideMeshVolume(minSphere, set.mesh));
        assert(!sphereInsideMeshVolume(maxSphere, set.mesh));

        while (abs(maxSphere.radius - minSphere.radius) > 1e-8) {

            double midpoint = (minSphere.radius + maxSphere.radius) / 2.0;
            Sphere middleSphere { sphere.origin, midpoint };

            bool midOk = sphereInsideMeshVolume(middleSphere, set.mesh);
            if (midOk) {
                minSphere = middleSphere;
            } else {
                maxSphere = middleSphere;
            }
        }

        // Make sure to be conservative with the choice to we don't break the invariant
        sphere.radius = minSphere.radius;
        assert(sphereInsideMeshVolume(sphere, set.mesh));
    }

#if 0
    std::sort(set.spheres.begin(), set.spheres.end(), [](const Sphere& lhs, const Sphere& rhs) {
        return lhs.radius > rhs.radius;
    });
    fmt::print("   grid 'radius' {}\n", gridDiameter / 2.0);
    for (auto& sphere : set.spheres) {
        fmt::print("     sphere radius {}\n", sphere.radius);
    }
#endif
}

double sphereSetIntersectionVolumeOverestimation(const SphereSet& set)
{
    // Except some special cases there are no analytical solutions for volume intersection of many spheres. See
    // https://www.researchgate.net/publication/289097700_Volume_of_intersection_of_six_spheres_A_special_case_of_practical_interest
    // for some more information. In this case we don't really care about the exact intersection, but we do want to avoid as much
    // intersection as possible, so just overcounting it works here, I think..

    double intersection = 0.0;

    size_t sphereCount = set.spheres.size();
    for (size_t i = 0; i < sphereCount; ++i) {
        const Sphere& self = set.spheres[i];
        for (size_t k = i + 1; k < sphereCount; ++k) {
            const Sphere& other = set.spheres[k];
            intersection += self.overlapWith(other);
        }
    }

    return intersection;
}

double sphereSetUnionVolume(const SphereSet& set)
{
    double volume = 0.0;

    size_t sphereCount = set.spheres.size();
    for (const Sphere& sphere : set.spheres) {
        volume += sphere.volume();
    }

    // TODO: Maybe scale down the intersection a bit, since we know it overestimates?
    double intersectionOverestimation = sphereSetIntersectionVolumeOverestimation(set);
    volume -= 0.25 * intersectionOverestimation;

    return volume;
}

struct SphereFittingData {
    SphereSet* set;
    unsigned sphereIndex;
};

REAL sphereFittingObjectiveFunction(const INTEGER n, const REAL* x, void* dataPtr)
{
    assert(n == 4);

    Sphere testSphere;
    testSphere.radius = x[3];
    testSphere.origin = { x[0], x[1], x[2] };

    SphereFittingData& data = *reinterpret_cast<SphereFittingData*>(dataPtr);
    SphereSet& set = *data.set;

    // Temporarily swap in the sphere to test
    Sphere currentSphere = set.spheres[data.sphereIndex];
    set.spheres[data.sphereIndex] = testSphere;

    if (!sphereInsideMeshVolume(testSphere, set.mesh)) {
        return 99999.99; // BOBYQA doesn't seem to like infinities, but this will do
    }

    // Since BOBYQA solves a minimization problem we want to minimize
    // the *negative* volume to get a maximal volume solution.
    double metric = -sphereSetUnionVolume(set);

    // Replace the current sphere back into the sphere set
    set.spheres[data.sphereIndex] = currentSphere;

    return metric;
}

void sphereFitting(SphereSet& set)
{
    // To avoid fitting the spheres in the same order every time.
    unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
    std::shuffle(set.spheres.begin(), set.spheres.end(), std::default_random_engine(seed));

    for (size_t sphereIdx = 0; sphereIdx < set.spheres.size(); ++sphereIdx) {

        Sphere& sphere = set.spheres[sphereIdx];
        assert(sphere.radius > 0.0);

        // Origin (3) + radius (1)
        constexpr INTEGER n = 3 + 1;

        // NPT is the number of interpolation conditions.  Its value must be in the interval
        // [N+2,(N+1)(N+2)/2].  Choices that exceed 2*N+1 are not recommended.
        // TODO: Find a good value!
#if 0
        constexpr INTEGER npt = n + 2; // (6)
#else
        constexpr INTEGER npt = (n + 1) * (n + 2) / 2; // (15)
#endif

        REAL x[n] = {
            sphere.origin.x,
            sphere.origin.y,
            sphere.origin.z,
            sphere.radius
        };

        // To inscribe a box = {min, max} in a sphere we want to find the min & max
        // points which lie along the diagonal that goes through the box center.
        //
        //  sqrt(x^2 + y^2 + y^3) = r
        //   since diagonal -> x=y=z = d
        //             sqrt(3d^2) = r
        //                   3d^2 = r^2
        //                      d = sqrt(r^2 / 3)
        //
        double d = std::sqrt(sphere.radius * sphere.radius / 3.0);
        vec3 boxDiagonal = vec3(d, d, d) * 0.99f; // (avoid overstepping!)
        aabb3 boxInSphere { sphere.origin - boxDiagonal, sphere.origin + boxDiagonal };

        REAL xLower[n] = {
            boxInSphere.min.x,
            boxInSphere.min.y,
            boxInSphere.min.z,
            1e-8
        };

        aabb3 gridBounds = set.grids.shell->gridBounds();
        double gridMaxDistance = length(gridBounds.max - gridBounds.min);

        REAL xUpper[n] = {
            boxInSphere.max.x,
            boxInSphere.max.y,
            boxInSphere.max.z,
            gridMaxDistance
        };

        // "Typically, RHOBEG should be about one tenth of the greatest expected change to a variable"
        // TODO: Find good value!
        REAL rhoBeg = (2.0 * d) / 10.0;

        // RHOEND should indicate the accuracy that is required in the final values of the variables
        // NOTE: Some models are tiny, others are huge.. So an absolute/constant accuracy is probably
        //  not very helpful here. A fraction of the sphere radius could maybe work as a scale-aware metric?
        // TODO: Find good value!
        REAL rhoEnd = gridMaxDistance * 1e-8;

        //The array W will be used for working space.  Its length must be at least
        // (NPT+5)*(NPT+N)+3*N*(N+5)/2.  Upon successful return, the first element of W
        // will be set to the function value at the solution. */
        REAL workingMemory[(npt + 5) * (npt + n) + 3 * n * (n + 5) / 2];

        INTEGER logLevel = 0;
        INTEGER maxObjFunCalls = 10'000;

        SphereFittingData data { &set, sphereIdx };

        int status = bobyqa(n, npt, sphereFittingObjectiveFunction, (void*)(&data),
                            x, xLower, xUpper, rhoBeg, rhoEnd,
                            logLevel, maxObjFunCalls, workingMemory);

        switch (status) {
        case BOBYQA_SUCCESS: {
            // algorithm converged!
            vec3 newOrigin { x[0], x[1], x[2] };
            double newRadius = x[3];

#if 0
            fmt::print("      - ({}, {}, {}) r={}\n", newOrigin.x, newOrigin.y, newOrigin.z, newRadius);

            vec3 diffOrigin = newOrigin - sphere.origin;
            double diffRadius = newRadius - sphere.radius;
            //fmt::print("      - ({}, {}, {}) r={}\n", diffOrigin.x, diffOrigin.y, diffOrigin.z, diffRadius);
            //assert(length(diffOrigin) > 1e-6 && abs(diffRadius > 1e-6));
#endif

            sphere.origin = newOrigin;
            sphere.radius = newRadius;
            return;
        }
        case BOBYQA_BAD_NPT:
            fmt::print("bobyqa error: NPT is not in the required interval\n");
            assert(false); // npt is hardcoded to a valid value!
            break;
        case BOBYQA_TOO_CLOSE:
            fmt::print("bobyqa error: insufficient space between the bounds\n");
            break;
        case BOBYQA_ROUNDING_ERRORS:
            fmt::print("bobyqa error: too much cancellation in a denominator\n");
            break;
        case BOBYQA_TOO_MANY_EVALUATIONS:
            fmt::print("bobyqa error: maximum number of function evaluations exceeded\n");
            break;
        case BOBYQA_STEP_FAILED:
            fmt::print("bobyqa error: a trust region step has failed to reduce Q\n");
            break;
        default:
            assert(false);
        }
    }
}

vec3 randomPointInSphere(const Sphere& sphere)
{
    std::uniform_real_distribution<double> randomOffset { -sphere.radius, +sphere.radius };

    unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
    std::default_random_engine generator { seed };

    vec3 offset;
    do {
        offset = { randomOffset(generator), randomOffset(generator), randomOffset(generator) };
    } while (length(offset) > sphere.radius);

    return sphere.origin + offset;
}

void sphereTeleportation(SphereSet& set)
{
    if (set.spheres.size() == 1) {
        // TODO: Maybe pick a new starting point?
        // Or let the teleportation fail like this and restart from the main loop?
        return;
    }

    std::sort(set.spheres.begin(), set.spheres.end(), [](const Sphere& lhs, const Sphere& rhs) {
        return lhs.radius < rhs.radius;
    });

    unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
    std::default_random_engine generator { seed };

    size_t numToReplace = std::ceil(set.spheres.size() / 8.0);
    for (size_t i = 0; i < numToReplace; ++i) {

        Sphere& sphere = set.spheres[i];
        sphere.origin = getRandomSphereLocationInsideMesh(*set.grids.inside, generator);

        // NOTE: A sphere expansion will happen after this is done, so we don't need to take care of that now
        sphere.radius = 0.0;
    }
}

double aabbVolume(const aabb3& aabb)
{
    glm::vec<3, double> dims = aabb.max - aabb.min;
    return dims.x * dims.y * dims.z;
}

std::optional<vec3> triangleRayIntersection(vec3 v0, vec3 v1, vec3 v2, vec3 rayOrigin, vec3 rayDirection, float& outT)
{
    // From GraphicsCodex

    const vec3& e1 = v1 - v0;
    const vec3& e2 = v2 - v0;

    vec3 N = normalize(cross(e1, e2));

    vec3 q = cross(rayDirection, e2);
    float a = dot(e1, q);

    // Close to the limit of precision?
    if (fabsf(a) <= 1e-4f) {
        return {};
    }

    const vec3& s = (rayOrigin - v0) / a;
    const vec3& r = cross(s, e1);

    // Barycentric coordinates
    float b[3];
    b[0] = dot(s, q);
    b[1] = dot(r, rayDirection);
    b[2] = 1.0f - b[0] - b[1];

    // Intersected inside triangle?
    float t = dot(e2, r);
    if ((b[0] >= 0) && (b[1] >= 0) && (b[2] >= 0) && t >= 0) {

        vec3 hitPoint = rayOrigin + t * rayDirection;

        // TODO: This worked on a previous project... What's up with the order though?
        vec3 barycentric { b[2], b[0], b[1] };

        outT = t;
        return barycentric;
    }

    return {};
}

vec3 raytraceMesh(const SimpleMesh& mesh, vec3 o, vec3 d)
{
    float closestT = INFINITE;
    int64_t closestTriangle = -1;
    vec3 closestBarycentric {};

    for (size_t ti = 0; ti < mesh.triangleCount(); ++ti) {

        vec3 v0, v1, v2;
        mesh.triangle(ti, v0, v1, v2);

        float t;
        std::optional<vec3> bary = triangleRayIntersection(v0, v1, v2, o, d, t);

        if (bary.has_value() && t < closestT) {
            closestBarycentric = bary.value();
            closestTriangle = ti;
            closestT = t;
        }
    }

    if (closestTriangle == -1) {
        return vec3(0, 0, 0);
    }

    vec2 uv0, uv1, uv2;
    mesh.triangleTexcoords(closestTriangle, uv0, uv1, uv2);

    vec3 b = closestBarycentric;
    vec2 uv = b[0] * uv0 + b[1] * uv1 + b[2] * uv2;

    assert(mesh.hasTexture());
    vec3 color = mesh.texture().sample(uv);

    return color;
}

SphericalHarmonics generateSphericalHarmonics(vec3 sampleCenter, size_t numSamples, const SimpleMesh& mesh)
{

    auto rgbFunction = [&](double phi, double theta) -> vec3 {
        Eigen::Vector3d v = sh::ToVector(phi, theta);
        vec3 direction = vec3(v.x(), v.y(), v.z());
        vec3 Lsrgb = raytraceMesh(mesh, sampleCenter, direction);
        return pow(Lsrgb, vec3(2.2f));
    };

    std::vector<double> r = *sh::ProjectFunction(
        2, [&](double phi, double theta) -> double {
            vec3 color = rgbFunction(phi, theta);
            return color.r;
        },
        numSamples);
    std::vector<double> g = *sh::ProjectFunction(
        2, [&](double phi, double theta) -> double {
            vec3 color = rgbFunction(phi, theta);
            return color.g;
        },
        numSamples);
    std::vector<double> b = *sh::ProjectFunction(
        2, [&](double phi, double theta) -> double {
            vec3 color = rgbFunction(phi, theta);
            return color.b;
        },
        numSamples);

    assert(r.size() == 9);
    assert(g.size() == 9);
    assert(b.size() == 9);

    SphericalHarmonics sh;

    sh.L00 = vec3(r[0], g[0], b[0]);

    sh.L1_1 = vec3(r[1], g[1], b[1]);
    sh.L10 = vec3(r[2], g[2], b[2]);
    sh.L11 = vec3(r[3], g[3], b[3]);

    sh.L2_2 = vec3(r[4], g[4], b[4]);
    sh.L2_1 = vec3(r[5], g[5], b[5]);
    sh.L20 = vec3(r[6], g[6], b[6]);
    sh.L21 = vec3(r[7], g[7], b[7]);
    sh.L22 = vec3(r[8], g[8], b[8]);

    return sh;
}

std::vector<SphericalHarmonics> generateSphericalHarmonicsForSphereColors(const SphereSet& set, size_t numSamples)
{
    std::vector<SphericalHarmonics> SHs;

    for (const Sphere& sphere : set.spheres) {
        SphericalHarmonics sh = generateSphericalHarmonics(sphere.origin, numSamples, set.mesh);
        SHs.push_back(sh);
    }

    return SHs;
}

void generateJsonOutput(const SphereSet& sphereSet, const std::string& outPath)
{
    using namespace nlohmann;

    json j;
    j["proxy"] = "sphere-set";
    j["spheres"] = {};

    for (size_t i = 0; i < sphereSet.spheres.size(); ++i) {

        const Sphere& sphere = sphereSet.spheres[i];
        const SphericalHarmonics& sh = sphereSet.sphereSH[i];

        json jsonSphere = {
            { "center", { sphere.origin.x, sphere.origin.y, sphere.origin.z } },
            { "radius", sphere.radius },
            { "sh",
              { { "L00", { sh.L00.x, sh.L00.y, sh.L00.z } },
                { "L1_1", { sh.L1_1.x, sh.L1_1.y, sh.L1_1.z } },
                { "L10", { sh.L10.x, sh.L10.y, sh.L10.z } },
                { "L11", { sh.L11.x, sh.L11.y, sh.L11.z } },
                { "L2_2", { sh.L2_2.x, sh.L2_2.y, sh.L2_2.z } },
                { "L2_1", { sh.L2_1.x, sh.L2_1.y, sh.L2_1.z } },
                { "L20", { sh.L20.x, sh.L20.y, sh.L20.z } },
                { "L21", { sh.L21.x, sh.L21.y, sh.L21.z } },
                { "L22", { sh.L22.x, sh.L22.y, sh.L22.z } } } }
        };
        j["spheres"].push_back(jsonSphere);
    }

    std::ofstream outstream(outPath);
    outstream << std::setw(4) << j << std::endl;
}

void generateOutput(const SphereSet& sphereSet, bool doPrint = true)
{
    size_t sphereCount = sphereSet.spheres.size();

    std::string result = "const vec4 spheres[] = vec4[](\n";
    for (size_t i = 0; i < sphereCount; ++i) {
        const Sphere& sphere = sphereSet.spheres[i];
        std::string line = fmt::format("\tvec4({}, {}, {}, {}){}\n",
                                       sphere.origin.x, sphere.origin.y, sphere.origin.z,
                                       sphere.radius, (i + 1 == sphereCount) ? "" : ",");
        result += line;
    }
    result += ");\n";

    if (doPrint) {
        fmt::print(result);
    }

#ifdef WIN32
    OpenClipboard(0);
    EmptyClipboard();
    HGLOBAL hg = GlobalAlloc(GMEM_MOVEABLE, result.size() + 1);
    if (!hg) {
        CloseClipboard();
        return;
    }
    memcpy(GlobalLock(hg), result.c_str(), result.size());
    GlobalUnlock(hg);
    SetClipboardData(CF_TEXT, hg);
    CloseClipboard();
    GlobalFree(hg);
#endif
}

void setApplicationWorkingDirectory(char* executableName, const std::string& workingDir)
{
#ifdef WIN32
    char fullPathBuf[_MAX_PATH] = {};
    assert(_fullpath(fullPathBuf, executableName, sizeof(fullPathBuf)));
    std::string fullPath { fullPathBuf };

    size_t startOfWorkingDirName = fullPath.find(workingDir);
    std::string newWorkingDir = fullPath.substr(0, startOfWorkingDirName + workingDir.length() + 1);
    assert(_chdir(newWorkingDir.c_str()) == 0);
#else
    char fullPathBuf[PATH_MAX] = {};
    assert(realpath(executableName, fullPathBuf));
    std::string fullPath { fullPathBuf };

    size_t startOfWorkingDirName = fullPath.find(workingDir);
    std::string newWorkingDir = fullPath.substr(0, startOfWorkingDirName + workingDir.length() + 1);
    assert(chdir(newWorkingDir.c_str()) == 0);
#endif
}

int main(int argc, char** argv)
{
    setApplicationWorkingDirectory(argv[0], "ProxyGen");

    std::string path = "assets/Barrel/barrel.gltf";
    std::string outPath = "assets/Barrel/barrel_spheres.json";

    constexpr unsigned numSpheres = 2;
    const size_t gridDimensions = 126;

    constexpr size_t numShSamples = 4096;

    fmt::print("=> loading model '{}'\n", path);
    auto [basePath, model] = GltfUtil::loadModel(path);
    std::vector<SimpleMesh> simpleMeshes {};
    GltfUtil::bakeDownModelToSimpleMeshes(model, basePath, simpleMeshes);
    if (simpleMeshes.size() > 1) {
        fmt::print("==> model has more than one meshes; only the first will be used!\n");
    }

    const SimpleMesh& mesh = simpleMeshes[0];
    SphereSet set { mesh };

    fmt::print("=> creating voxel grids\n");

    aabb3 meshBounds { vec3(+INFINITY), vec3(-INFINITY) };
    mesh.extendAABB(meshBounds.min, meshBounds.max);

    set.grids.shell = std::make_unique<VoxelGrid>(glm::ivec3(gridDimensions), meshBounds);
    set.grids.shell->insertMesh(mesh, true);
    set.grids.shell->quantizeColors(10);

    set.grids.filled = std::make_unique<VoxelGrid>(*set.grids.shell);
    set.grids.filled->fillVolumes();

    set.grids.inside = std::make_unique<VoxelGrid>(*set.grids.filled);
    set.grids.inside->subtractGrid(*set.grids.shell);

    fmt::print("=> assigning initial spheres\n");
    assignSpheresRandomlyInVolume(set, numSpheres);

    fmt::print("=> optimizing begin\n");

    bool didJustTeleport = false;
    double previousVolume = -std::numeric_limits<double>::infinity();

    double bestVolume = std::numeric_limits<double>::min();
    std::vector<Sphere> bestSolution {};

    constexpr int maxConsecutiveReverts = 3;
    int numConsecutiveReverts = 0;

    constexpr int maxIterations = 10'000;
    int iteration = 0;

    for (; iteration < maxIterations; ++iteration) {

        fmt::print("==> expanding spheres\n");
        expandSpheresMaximally(set);
        double newVolume = sphereSetUnionVolume(set);
        fmt::print("   volume: {}\n", newVolume);

        if (newVolume > bestVolume) {
            bestVolume = newVolume;
            bestSolution = set.spheres;
        }

        if (newVolume > previousVolume) {

            fmt::print("===> sphere fitting\n");
            sphereFitting(set);

            didJustTeleport = false;
            numConsecutiveReverts = 0;

        } else {

            if (didJustTeleport) {
                fmt::print("==> teleportation failed to increase volume, ");
                if (numConsecutiveReverts < maxConsecutiveReverts) {
                    fmt::print("reverting to known good solution\n");
                    set.spheres = bestSolution;
                    numConsecutiveReverts += 1;
                    //continue; (no, we have to teleport from this new stage first!)
                } else {
                    fmt::print("and max reverts reached, so aborting\n");
                    break;
                }
            }

            fmt::print("===> sphere teleportation\n");
            sphereTeleportation(set);
            didJustTeleport = true;
        }

        previousVolume = newVolume;

        if (iteration % 10 == 0) {
            double itDone = 100.0 * double(iteration) / double(maxIterations);
            fmt::print("==> {} iterations done ({:.1f}%)\n", iteration, itDone);
            generateOutput(set, false);
        }
    }

    if (iteration == maxIterations) {
        fmt::print("==> max iterations reached\n");
    }

    if (numConsecutiveReverts == maxConsecutiveReverts) {
        fmt::print("==> max consectutive reverts reached\n");
    }

    fmt::print("=> optimizing done\n");

    fmt::print("=> generating sphere output with volume {} (AABB volume {})\n", bestVolume, aabbVolume(meshBounds));
    set.spheres = bestSolution;

    fmt::print("=> generating SH color representations for spheres ({} samples)\n", numShSamples);
    std::vector<SphericalHarmonics> SHs = generateSphericalHarmonicsForSphereColors(set, numShSamples);
    set.sphereSH = SHs;

    generateOutput(set);
    generateJsonOutput(set, outPath);
}


/*
 * optimize_tnc.cpp
 *
 *  Created on: Feb 9, 2010
 *      Author: smitty
 */

#include "optimize_state_reconstructor_periods_nlopt.h"
#include <iostream>
#include <stdio.h>
#include <nlopt.hpp>
#include <math.h>
#include <vector>

#include "state_reconstructor.h"
#include "rate_model.h"

#include <armadillo>
using namespace arma;

StateReconstructor * nloptsr_periods;
vector<RateModel> * nloptrm_periods;
vector<mat> * nloptfree_variables_periods;

//double nlopt_sr(int n, const double *x, void *state) {
//double nlopt_sr(unsigned n, const double *x, double *grad, void *my_func_data);
double nlopt_sr_periods(unsigned n, const double *x, double *grad, void *my_func_data) {
    for (unsigned int k=0; k < nloptfree_variables_periods->size(); k++) {
        for (unsigned int i=0; i < (*nloptfree_variables_periods)[k].n_rows; i++) {
            for (unsigned int j=0; j < (*nloptfree_variables_periods)[k].n_cols; j++) {
                if (i != j) {
                    (*nloptrm_periods)[k].set_Q_cell(i,j,x[int(nloptfree_variables_periods->at(k)(i,j))]);
                    if ((*nloptrm_periods)[k].get_Q()(i,j) < 0 || (*nloptrm_periods)[k].get_Q()(i,j) >= 1000) {
                        return 1000000000000;
                    }
                }
            }
        }
    }
    double like;
    for (unsigned int i=0; i < nloptrm_periods->size(); i++) {
        nloptrm_periods->at(i).set_Q_diag();
    }
    like = nloptsr_periods->eval_likelihood();
    for (unsigned int i=0; i < nloptrm_periods->size(); i++) {
        if (nloptrm_periods->at(i).neg_p == true) {
            like = 10000000000000;
            break;
        }
    }
//    cout << like << endl;
    if (like < 0 || like == std::numeric_limits<double>::infinity()) {
        like = 10000000000000;
    }
    return like;
}

void optimize_sr_periods_nlopt(vector<RateModel> * _rm,StateReconstructor * _sr, vector<mat> * _free_mask, int _nfree) {
    nloptsr_periods = _sr;
    nloptrm_periods = _rm;
    nloptfree_variables_periods = _free_mask;

    nlopt::opt opt(nlopt::LN_NELDERMEAD, _nfree);
    //nlopt::opt opt(nlopt::LN_BOBYQA, _nfree);
    //nlopt::opt opt(nlopt::LN_PRAXIS, _nfree);
    //nlopt::opt opt(nlopt::LN_SBPLX, _nfree);
    //nlopt::opt opt(nlopt::LN_COBYLA, _nfree);
    //nlopt::opt opt(nlopt::LN_NEWUOA, _nfree);

    opt.set_lower_bounds(0.0000);
    opt.set_upper_bounds(100000);
    opt.set_min_objective(nlopt_sr_periods, NULL);
    opt.set_xtol_rel(0.001);
    opt.set_maxeval(10000);

    vector<double> x(_nfree,0);
    for (unsigned int k=0; k < _rm->size(); k++) {
        for (unsigned int i=0; i < _rm->at(k).get_Q().n_rows; i++) {
            for (unsigned int j=0; j < _rm->at(k).get_Q().n_cols; j++) {
                if (i != j) {
                    x[int((*_free_mask)[k](i, j))] = _rm->at(k).get_Q()(i, j);
                    //cout << x[int((*_free_mask)[k](i,j))] << " ";
                }
            }
            cout << endl;
        }
    }
    //double minf;
    vector<double> result = opt.optimize(x);
    for (unsigned int k=0; k < _rm->size(); k++) {
        for (unsigned int i=0; i < _rm->at(k).get_Q().n_rows; i++) {
            for (unsigned int j=0; j < _rm->at(k).get_Q().n_cols; j++) {
                if (i != j) {
                    (*_free_mask)[k](i, j) = result[int((*_free_mask)[k](i, j))];
                    //cout << x[int((*(*_free_mask)[k])(i,j))] << " ";
                }
            }
        //cout << endl;
        }
    }
    //if ((&minf) < 0) {
    //   printf("nlopt failed!\n");
    //}
    //else {
    //    printf("found minimum at %0.10g\n", x[0], x[1], minf);
    //}
}


#ifndef STAN_MATH_PRIM_SCAL_PROB_BERNOULLI_LOGIT_LPMF_HPP
#define STAN_MATH_PRIM_SCAL_PROB_BERNOULLI_LOGIT_LPMF_HPP

#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/scal/meta/operands_and_partials.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_bounded.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/fun/size_zero.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/inv_logit.hpp>
#include <stan/math/prim/scal/fun/log1m.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <boost/random/bernoulli_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <cmath>

namespace stan {
namespace math {

/**
 * Returns the log PMF of the logit-parametrized Bernoulli distribution. If
 * containers are supplied, returns the log sum of the probabilities.
 *
 * @tparam T_n type of integer parameter
 * @tparam T_prob type of chance of success parameter
 * @param n integer parameter
 * @param theta logit-transformed chance of success parameter
 * @return log probability or log sum of probabilities
 * @throw std::domain_error if theta is infinite.
 * @throw std::invalid_argument if container sizes mismatch.
 */
template <bool propto, typename T_n, typename T_prob>
typename return_type<T_prob>::type bernoulli_logit_lpmf(const T_n& n,
                                                        const T_prob& theta) {
  static const char* function = "bernoulli_logit_lpmf";
  typedef
      typename stan::partials_return_type<T_n, T_prob>::type T_partials_return;

  using std::exp;

  if (size_zero(n, theta))
    return 0.0;

  T_partials_return logp(0.0);

  check_bounded(function, "n", n, 0, 1);
  check_not_nan(function, "Logit transformed probability parameter", theta);
  check_consistent_sizes(function, "Random variable", n,
                         "Probability parameter", theta);

  if (!include_summand<propto, T_prob>::value)
    return 0.0;

  scalar_seq_view<T_n> n_vec(n);
  scalar_seq_view<T_prob> theta_vec(theta);
  size_t N = max_size(n, theta);
  operands_and_partials<T_prob> ops_partials(theta);

  for (size_t n = 0; n < N; n++) {
    const T_partials_return theta_dbl = value_of(theta_vec[n]);

    const int sign = 2 * n_vec[n] - 1;
    const T_partials_return ntheta = sign * theta_dbl;
    const T_partials_return exp_m_ntheta = exp(-ntheta);

    // Handle extreme values gracefully using Taylor approximations.
    static const double cutoff = 20.0;
    if (ntheta > cutoff)
      logp -= exp_m_ntheta;
    else if (ntheta < -cutoff)
      logp += ntheta;
    else
      logp -= log1p(exp_m_ntheta);

    if (!is_constant_struct<T_prob>::value) {
      if (ntheta > cutoff)
        ops_partials.edge1_.partials_[n] -= exp_m_ntheta;
      else if (ntheta < -cutoff)
        ops_partials.edge1_.partials_[n] += sign;
      else
        ops_partials.edge1_.partials_[n]
            += sign * exp_m_ntheta / (exp_m_ntheta + 1);
    }
  }
  return ops_partials.build(logp);
}

template <typename T_n, typename T_prob>
inline typename return_type<T_prob>::type bernoulli_logit_lpmf(
    const T_n& n, const T_prob& theta) {
  return bernoulli_logit_lpmf<false>(n, theta);
}

}  // namespace math
}  // namespace stan
#endif


/*
*  @file 		ex9.cpp
*  @details  	This file is the solution to exercise 9.
*  @author    	Alexander Rettkowski
*  @date      	12.07.2017
*/
#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_object.hpp>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/fusion/include/io.hpp>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include <boost/property_map/property_map.hpp>

#include <boost/timer/timer.hpp>
#include <boost/chrono.hpp>

#include <iostream>
#include <string>
#include <complex>
#include <fstream>
#include <queue>

#include <omp.h>

using namespace boost;

namespace exercise9
{
	namespace qi = boost::spirit::qi;
	namespace ascii = boost::spirit::ascii;
	struct edge
	{
		int startNode;
		int endNode;
		int length;
	};
}

BOOST_FUSION_ADAPT_STRUCT(
	exercise9::edge,
	(int, startNode)
	(int, endNode)
	(int, length)
)

namespace exercise9
{
	template <typename Iterator>
	struct line_parser : qi::grammar<Iterator, edge()>
	{
		line_parser() : line_parser::base_type(start)
		{
			using qi::int_;
			start %= int_ >> ' ' >> int_ >> ' ' >> int_;
		}

		qi::rule<Iterator, edge()> start;
	};
}

/**
* This method checks (in a naive way), if a given number is prime or not.
* @param number The number to check.
* @returns True, if number is prime; false otherwise.
*/
bool isPrime(int number) {
	if (number == 2)
		return true;
	if (number % 2 == 0)
		return false;
	for (int i = 3; (i*i) <= number; i += 2) {
		if (number % i == 0) return false;
	}
	return true;
}

/**
* The function that calculates the shortest paths using Dijkstra's method.
* @param numberOfNodes Number of nodes in the graph.
* @param graph The graph represented as a vector auf edge-vectors.
* @param startNode The id of the node where the search starts.
* @returns The weight of the Steiner Tree and the edge list of the tree coupled as a std::pair
*/
std::pair<int, std::list<std::pair<int, int>>>  steinerTree(int numberOfNodes, std::vector< std::vector< std::pair<int, int> > > graph, int startNode)
{
	std::vector<int> connectedVertices;
	connectedVertices.push_back(startNode);
	std::list<std::pair<int, int>> edgeList;

	std::vector<int> predecessors(numberOfNodes, -1);
	std::vector<int> distanceTo(numberOfNodes, INT_MAX);
	std::priority_queue< std::pair<int, int>, std::vector< std::pair<int, int> >, std::greater< std::pair<int, int> > > queue;
	queue.push(std::pair<int, int>(startNode, 0));
	distanceTo[startNode] = 0;
	//predecessors[startNode] = -1;
	int steinerWeight = 0;

	int currentNode, compareNode, compareNodeDistance, currentNodeDistance;

	while (!queue.empty()) {
		currentNode = queue.top().first;
		currentNodeDistance = queue.top().second;
		queue.pop();

		if (distanceTo[currentNode] < currentNodeDistance) continue;

		for (int i = 0; i < graph[currentNode].size(); i++) {
			compareNode = graph[currentNode][i].first;
			compareNodeDistance = graph[currentNode][i].second;
			if (distanceTo[compareNode] > distanceTo[currentNode] + compareNodeDistance) {
				if (isPrime(compareNode))
				{
					predecessors[compareNode] = currentNode;

					int backtracknode = compareNode;
					while (std::find(connectedVertices.begin(), connectedVertices.end(), backtracknode) == connectedVertices.end())
					{
						for (std::pair<int, int> edge : graph[predecessors[backtracknode]])
						{
							if (edge.first == backtracknode)
							{
								edgeList.push_back(std::pair<int, int>(predecessors[backtracknode], backtracknode));
								steinerWeight += edge.second;
							}
						}

						distanceTo[backtracknode] = 0;
						connectedVertices.push_back(backtracknode);
						backtracknode = predecessors[backtracknode];
						queue.push(std::pair<int, int>(backtracknode, 0));
					}
				}
				else
				{
					distanceTo[compareNode] = distanceTo[currentNode] + compareNodeDistance;
					queue.push(std::pair<int, int>(compareNode, distanceTo[compareNode]));
					predecessors[compareNode] = currentNode;
				}
			}
		}
	}

	return std::pair<int, std::list<std::pair<int, int>>>(steinerWeight, edgeList);
}

/**
* The main function that reads in a file and processes it.
* @param argc Number of command line arguments.
* @param *argv a pointer to the array of command line arguments.
*/
int main(int argc, char *argv[])
{
	timer::cpu_timer boostTimer;

	using boost::spirit::ascii::space;
	typedef std::string::const_iterator iterator_type;
	typedef exercise9::line_parser<iterator_type> line_parser;
	line_parser parser;
	std::string currentLine;
	std::ifstream file(argv[1]);

	// get number of nodes
	char delimiter = ' ';
	getline(file, currentLine, delimiter);
	const int numberOfNodes = stoi(currentLine);
	getline(file, currentLine);
	int constSub = 1;

	std::vector<std::vector<std::pair<int, int>>> edges(numberOfNodes);
	while (getline(file, currentLine))
	{
		exercise9::edge parsedLine;
		std::string::const_iterator currentPosition = currentLine.begin();
		std::string::const_iterator lineEnd = currentLine.end();
		bool parsingSucceeded = phrase_parse(currentPosition, lineEnd, parser, space, parsedLine);

		if (parsingSucceeded && currentPosition == lineEnd)
		{
			std::pair<int, int> *tempEdge = new std::pair<int, int>();
			tempEdge->first = parsedLine.endNode - constSub;
			tempEdge->second = parsedLine.length;
			edges[parsedLine.startNode - constSub].push_back(*tempEdge);

			std::pair<int, int> *reverseEdge = new std::pair<int, int>();
			reverseEdge->first = parsedLine.startNode - constSub;
			reverseEdge->second = parsedLine.length;
			edges[parsedLine.endNode - constSub].push_back(*reverseEdge);
		}
	}

	file.close();

	timer::cpu_timer runTimer;
	omp_set_dynamic(0);
	omp_set_num_threads(std::stoi(argv[2]));
	std::vector<int> terminalIds;
	for (int i = 0; i < numberOfNodes; i++)
	{
		if (isPrime(i))
			terminalIds.push_back(i);
	}
	std::vector<int> lengths(terminalIds.size());

#pragma omp parallel for
	for (int i = 0; i < terminalIds.size(); i++)
		{
		    std::pair<int, std::list<std::pair<int, int>>> currentRun = steinerTree(numberOfNodes, edges, terminalIds[i]);
			lengths[i] = currentRun.first;
			std::cout << "Length starting from " << i << ": " << lengths[i] << std::endl;
		}

	int minlength = INT32_MAX;
	int minId = -1;
	for (int i = 0; i < terminalIds.size(); i++)
	{
		if (lengths[i] < minlength)
		{
			minlength = lengths[i];
			minId = i;
		}
	}

	timer::cpu_times runtime = boostTimer.elapsed();
	std::list<std::pair<int, int>> edgelist = steinerTree(numberOfNodes, edges, minId).second;

	std::cout << "TLEN: " << minlength << std::endl;
	std::cout << "TREE: ";

	for (std::pair<int, int> edge : edgelist)
	{
		std::cout << "(" << edge.first << ", " << edge.second << ") ";
	}
	std::cout << std::endl;

	timer::cpu_times time = boostTimer.elapsed();
	std::cout << "TIME: " << (time.user + time.system) / 1e9 << "s\n";
	std::cout << "WALL: " << runtime.wall / 1e9 << "s\n";

	return 0;
}


/*
 [auto_generated]
 boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp

 [begin_description]
 Implementation of the Dormand-Prince 5(4) method. This stepper can also be used with the dense-output controlled stepper.
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 Mario Mulansky

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA_DOPRI5_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA_DOPRI5_HPP_INCLUDED

#include <boost/numeric/odeint/util/bind.hpp>

#include <boost/numeric/odeint/stepper/base/explicit_error_stepper_fsal_base.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>

#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/util/same_instance.hpp>

namespace boost {
namespace numeric {
namespace odeint {

template<
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = typename algebra_dispatcher< State >::algebra_type ,
class Operations = typename operations_dispatcher< State >::operations_type ,
class Resizer = initially_resizer
>
class runge_kutta_dopri5
#ifndef DOXYGEN_SKIP
: public explicit_error_stepper_fsal_base<
  runge_kutta_dopri5< State , Value , Deriv , Time , Algebra , Operations , Resizer > ,
  5 , 5 , 4 , State , Value , Deriv , Time , Algebra , Operations , Resizer >
#else
: public explicit_error_stepper_fsal_base
#endif
{

public :

    #ifndef DOXYGEN_SKIP
    typedef explicit_error_stepper_fsal_base<
    runge_kutta_dopri5< State , Value , Deriv , Time , Algebra , Operations , Resizer > ,
    5 , 5 , 4 , State , Value , Deriv , Time , Algebra , Operations , Resizer > stepper_base_type;
    #else
    typedef explicit_error_stepper_fsal_base< runge_kutta_dopri5< ... > , ... > stepper_base_type;
    #endif

    typedef typename stepper_base_type::state_type state_type;
    typedef typename stepper_base_type::value_type value_type;
    typedef typename stepper_base_type::deriv_type deriv_type;
    typedef typename stepper_base_type::time_type time_type;
    typedef typename stepper_base_type::algebra_type algebra_type;
    typedef typename stepper_base_type::operations_type operations_type;
    typedef typename stepper_base_type::resizer_type resizer_type;

    #ifndef DOXYGEN_SKIP
    typedef typename stepper_base_type::stepper_type stepper_type;
    typedef typename stepper_base_type::wrapped_state_type wrapped_state_type;
    typedef typename stepper_base_type::wrapped_deriv_type wrapped_deriv_type;
    #endif // DOXYGEN_SKIP

    runge_kutta_dopri5( const algebra_type &algebra = algebra_type() ) : stepper_base_type( algebra )
    { }

    template< class System , class StateIn , class DerivIn , class StateOut , class DerivOut >
    void do_step_impl( System system , const StateIn &in , const DerivIn &dxdt_in , time_type t ,
            StateOut &out , DerivOut &dxdt_out , time_type dt )
    {
        const value_type a2 = static_cast<value_type> ( 1 ) / static_cast<value_type>( 5 );
        const value_type a3 = static_cast<value_type> ( 3 ) / static_cast<value_type> ( 10 );
        const value_type a4 = static_cast<value_type> ( 4 ) / static_cast<value_type> ( 5 );
        const value_type a5 = static_cast<value_type> ( 8 )/static_cast<value_type> ( 9 );

        const value_type b21 = static_cast<value_type> ( 1 ) / static_cast<value_type> ( 5 );

        const value_type b31 = static_cast<value_type> ( 3 ) / static_cast<value_type>( 40 );
        const value_type b32 = static_cast<value_type> ( 9 ) / static_cast<value_type>( 40 );

        const value_type b41 = static_cast<value_type> ( 44 ) / static_cast<value_type> ( 45 );
        const value_type b42 = static_cast<value_type> ( -56 ) / static_cast<value_type> ( 15 );
        const value_type b43 = static_cast<value_type> ( 32 ) / static_cast<value_type> ( 9 );

        const value_type b51 = static_cast<value_type> ( 19372 ) / static_cast<value_type>( 6561 );
        const value_type b52 = static_cast<value_type> ( -25360 ) / static_cast<value_type> ( 2187 );
        const value_type b53 = static_cast<value_type> ( 64448 ) / static_cast<value_type>( 6561 );
        const value_type b54 = static_cast<value_type> ( -212 ) / static_cast<value_type>( 729 );

        const value_type b61 = static_cast<value_type> ( 9017 ) / static_cast<value_type>( 3168 );
        const value_type b62 = static_cast<value_type> ( -355 ) / static_cast<value_type>( 33 );
        const value_type b63 = static_cast<value_type> ( 46732 ) / static_cast<value_type>( 5247 );
        const value_type b64 = static_cast<value_type> ( 49 ) / static_cast<value_type>( 176 );
        const value_type b65 = static_cast<value_type> ( -5103 ) / static_cast<value_type>( 18656 );

        const value_type c1 = static_cast<value_type> ( 35 ) / static_cast<value_type>( 384 );
        const value_type c3 = static_cast<value_type> ( 500 ) / static_cast<value_type>( 1113 );
        const value_type c4 = static_cast<value_type> ( 125 ) / static_cast<value_type>( 192 );
        const value_type c5 = static_cast<value_type> ( -2187 ) / static_cast<value_type>( 6784 );
        const value_type c6 = static_cast<value_type> ( 11 ) / static_cast<value_type>( 84 );

        typename odeint::unwrap_reference< System >::type &sys = system;

        m_k_x_tmp_resizer.adjust_size( in , detail::bind( &stepper_type::template resize_k_x_tmp_impl<StateIn> , detail::ref( *this ) , detail::_1 ) );

        //m_x_tmp = x + dt*b21*dxdt
        stepper_base_type::m_algebra.for_each3( m_x_tmp.m_v , in , dxdt_in ,
                typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , dt*b21 ) );

        sys( m_x_tmp.m_v , m_k2.m_v , t + dt*a2 );
        // m_x_tmp = x + dt*b31*dxdt + dt*b32*m_k2
        stepper_base_type::m_algebra.for_each4( m_x_tmp.m_v , in , dxdt_in , m_k2.m_v ,
                typename operations_type::template scale_sum3< value_type , time_type , time_type >( 1.0 , dt*b31 , dt*b32 ));

        sys( m_x_tmp.m_v , m_k3.m_v , t + dt*a3 );
        // m_x_tmp = x + dt * (b41*dxdt + b42*m_k2 + b43*m_k3)
        stepper_base_type::m_algebra.for_each5( m_x_tmp.m_v , in , dxdt_in , m_k2.m_v , m_k3.m_v ,
                typename operations_type::template scale_sum4< value_type , time_type , time_type , time_type >( 1.0 , dt*b41 , dt*b42 , dt*b43 ));

        sys( m_x_tmp.m_v, m_k4.m_v , t + dt*a4 );
        stepper_base_type::m_algebra.for_each6( m_x_tmp.m_v , in , dxdt_in , m_k2.m_v , m_k3.m_v , m_k4.m_v ,
                typename operations_type::template scale_sum5< value_type , time_type , time_type , time_type , time_type >( 1.0 , dt*b51 , dt*b52 , dt*b53 , dt*b54 ));

        sys( m_x_tmp.m_v , m_k5.m_v , t + dt*a5 );
        stepper_base_type::m_algebra.for_each7( m_x_tmp.m_v , in , dxdt_in , m_k2.m_v , m_k3.m_v , m_k4.m_v , m_k5.m_v ,
                typename operations_type::template scale_sum6< value_type , time_type , time_type , time_type , time_type , time_type >( 1.0 , dt*b61 , dt*b62 , dt*b63 , dt*b64 , dt*b65 ));

        sys( m_x_tmp.m_v , m_k6.m_v , t + dt );
        stepper_base_type::m_algebra.for_each7( out , in , dxdt_in , m_k3.m_v , m_k4.m_v , m_k5.m_v , m_k6.m_v ,
                typename operations_type::template scale_sum6< value_type , time_type , time_type , time_type , time_type , time_type >( 1.0 , dt*c1 , dt*c3 , dt*c4 , dt*c5 , dt*c6 ));

        // the new derivative
        sys( out , dxdt_out , t + dt );
    }

    template< class System , class StateIn , class DerivIn , class StateOut , class DerivOut , class Err >
    void do_step_impl( System system , const StateIn &in , const DerivIn &dxdt_in , time_type t ,
            StateOut &out , DerivOut &dxdt_out , time_type dt , Err &xerr )
    {
        const value_type c1 = static_cast<value_type> ( 35 ) / static_cast<value_type>( 384 );
        const value_type c3 = static_cast<value_type> ( 500 ) / static_cast<value_type>( 1113 );
        const value_type c4 = static_cast<value_type> ( 125 ) / static_cast<value_type>( 192 );
        const value_type c5 = static_cast<value_type> ( -2187 ) / static_cast<value_type>( 6784 );
        const value_type c6 = static_cast<value_type> ( 11 ) / static_cast<value_type>( 84 );

        const value_type dc1 = c1 - static_cast<value_type> ( 5179 ) / static_cast<value_type>( 57600 );
        const value_type dc3 = c3 - static_cast<value_type> ( 7571 ) / static_cast<value_type>( 16695 );
        const value_type dc4 = c4 - static_cast<value_type> ( 393 ) / static_cast<value_type>( 640 );
        const value_type dc5 = c5 - static_cast<value_type> ( -92097 ) / static_cast<value_type>( 339200 );
        const value_type dc6 = c6 - static_cast<value_type> ( 187 ) / static_cast<value_type>( 2100 );
        const value_type dc7 = static_cast<value_type>( -1 ) / static_cast<value_type> ( 40 );

        /* ToDo: copy only if &dxdt_in == &dxdt_out ? */
        if( same_instance( dxdt_in , dxdt_out ) )
        {
            m_dxdt_tmp_resizer.adjust_size( in , detail::bind( &stepper_type::template resize_dxdt_tmp_impl<StateIn> , detail::ref( *this ) , detail::_1 ) );
            boost::numeric::odeint::copy( dxdt_in , m_dxdt_tmp.m_v );
            do_step_impl( system , in , dxdt_in , t , out , dxdt_out , dt );
            //error estimate
            stepper_base_type::m_algebra.for_each7( xerr , m_dxdt_tmp.m_v , m_k3.m_v , m_k4.m_v , m_k5.m_v , m_k6.m_v , dxdt_out ,
                                                    typename operations_type::template scale_sum6< time_type , time_type , time_type , time_type , time_type , time_type >( dt*dc1 , dt*dc3 , dt*dc4 , dt*dc5 , dt*dc6 , dt*dc7 ) );

        }
        else
        {
            do_step_impl( system , in , dxdt_in , t , out , dxdt_out , dt );
            //error estimate
            stepper_base_type::m_algebra.for_each7( xerr , dxdt_in , m_k3.m_v , m_k4.m_v , m_k5.m_v , m_k6.m_v , dxdt_out ,
                                                    typename operations_type::template scale_sum6< time_type , time_type , time_type , time_type , time_type , time_type >( dt*dc1 , dt*dc3 , dt*dc4 , dt*dc5 , dt*dc6 , dt*dc7 ) );

        }

    }

    /*
     * Calculates Dense-Output for Dopri5
     *
     * See Hairer, Norsett, Wanner: Solving Ordinary Differential Equations, Nonstiff Problems. I, p.191/192
     *
     * y(t+theta) = y(t) + h * sum_i^7 b_i(theta) * k_i
     *
     * A = theta^2 * ( 3 - 2 theta )
     * B = theta^2 * ( theta - 1 )
     * C = theta^2 * ( theta - 1 )^2
     * D = theta   * ( theta - 1 )^2
     *
     * b_1( theta ) = A * b_1 - C * X1( theta ) + D
     * b_2( theta ) = 0
     * b_3( theta ) = A * b_3 + C * X3( theta )
     * b_4( theta ) = A * b_4 - C * X4( theta )
     * b_5( theta ) = A * b_5 + C * X5( theta )
     * b_6( theta ) = A * b_6 - C * X6( theta )
     * b_7( theta ) = B + C * X7( theta )
     *
     * An alternative Method is described in:
     *
     * www-m2.ma.tum.de/homepages/simeon/numerik3/kap3.ps
     */
    template< class StateOut , class StateIn1 , class DerivIn1 , class StateIn2 , class DerivIn2 >
    void calc_state( time_type t , StateOut &x ,
                     const StateIn1 &x_old , const DerivIn1 &deriv_old , time_type t_old ,
                     const StateIn2 & /* x_new */ , const DerivIn2 &deriv_new , time_type t_new ) const
    {
        const value_type b1 = static_cast<value_type> ( 35 ) / static_cast<value_type>( 384 );
        const value_type b3 = static_cast<value_type> ( 500 ) / static_cast<value_type>( 1113 );
        const value_type b4 = static_cast<value_type> ( 125 ) / static_cast<value_type>( 192 );
        const value_type b5 = static_cast<value_type> ( -2187 ) / static_cast<value_type>( 6784 );
        const value_type b6 = static_cast<value_type> ( 11 ) / static_cast<value_type>( 84 );

        const time_type dt = ( t_new - t_old );
        const value_type theta = ( t - t_old ) / dt;
        const value_type X1 = static_cast< value_type >( 5 ) * ( static_cast< value_type >( 2558722523LL ) - static_cast< value_type >( 31403016 ) * theta ) / static_cast< value_type >( 11282082432LL );
        const value_type X3 = static_cast< value_type >( 100 ) * ( static_cast< value_type >( 882725551 ) - static_cast< value_type >( 15701508 ) * theta ) / static_cast< value_type >( 32700410799LL );
        const value_type X4 = static_cast< value_type >( 25 ) * ( static_cast< value_type >( 443332067 ) - static_cast< value_type >( 31403016 ) * theta ) / static_cast< value_type >( 1880347072LL ) ;
        const value_type X5 = static_cast< value_type >( 32805 ) * ( static_cast< value_type >( 23143187 ) - static_cast< value_type >( 3489224 ) * theta ) / static_cast< value_type >( 199316789632LL );
        const value_type X6 = static_cast< value_type >( 55 ) * ( static_cast< value_type >( 29972135 ) - static_cast< value_type >( 7076736 ) * theta ) / static_cast< value_type >( 822651844 );
        const value_type X7 = static_cast< value_type >( 10 ) * ( static_cast< value_type >( 7414447 ) - static_cast< value_type >( 829305 ) * theta ) / static_cast< value_type >( 29380423 );

        const value_type theta_m_1 = theta - static_cast< value_type >( 1 );
        const value_type theta_sq = theta * theta;
        const value_type A = theta_sq * ( static_cast< value_type >( 3 ) - static_cast< value_type >( 2 ) * theta );
        const value_type B = theta_sq * theta_m_1;
        const value_type C = theta_sq * theta_m_1 * theta_m_1;
        const value_type D = theta * theta_m_1 * theta_m_1;

        const value_type b1_theta = A * b1 - C * X1 + D;
        const value_type b3_theta = A * b3 + C * X3;
        const value_type b4_theta = A * b4 - C * X4;
        const value_type b5_theta = A * b5 + C * X5;
        const value_type b6_theta = A * b6 - C * X6;
        const value_type b7_theta = B + C * X7;

        // const state_type &k1 = *m_old_deriv;
        // const state_type &k3 = dopri5().m_k3;
        // const state_type &k4 = dopri5().m_k4;
        // const state_type &k5 = dopri5().m_k5;
        // const state_type &k6 = dopri5().m_k6;
        // const state_type &k7 = *m_current_deriv;

        stepper_base_type::m_algebra.for_each8( x , x_old , deriv_old , m_k3.m_v , m_k4.m_v , m_k5.m_v , m_k6.m_v , deriv_new ,
                typename operations_type::template scale_sum7< value_type , time_type , time_type , time_type , time_type , time_type , time_type >( 1.0 , dt * b1_theta , dt * b3_theta , dt * b4_theta , dt * b5_theta , dt * b6_theta , dt * b7_theta ) );
    }

    template< class StateIn >
    void adjust_size( const StateIn &x )
    {
        resize_k_x_tmp_impl( x );
        resize_dxdt_tmp_impl( x );
        stepper_base_type::adjust_size( x );
    }

private:

    template< class StateIn >
    bool resize_k_x_tmp_impl( const StateIn &x )
    {
        bool resized = false;
        resized |= adjust_size_by_resizeability( m_x_tmp , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_k2 , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_k3 , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_k4 , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_k5 , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_k6 , x , typename is_resizeable<deriv_type>::type() );
        return resized;
    }

    template< class StateIn >
    bool resize_dxdt_tmp_impl( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_dxdt_tmp , x , typename is_resizeable<deriv_type>::type() );
    }

    wrapped_state_type m_x_tmp;
    wrapped_deriv_type m_k2 , m_k3 , m_k4 , m_k5 , m_k6 ;
    wrapped_deriv_type m_dxdt_tmp;
    resizer_type m_k_x_tmp_resizer;
    resizer_type m_dxdt_tmp_resizer;
};

/************* DOXYGEN ************/
/**
 * \class runge_kutta_dopri5
 * \brief The Runge-Kutta Dormand-Prince 5 method.
 *
 * The Runge-Kutta Dormand-Prince 5 method is a very popular method for solving ODEs, see
 * .
 * The method is explicit and fulfills the Error Stepper concept. Step size control
 * is provided but continuous output is available which make this method favourable for many applications.
 *
 * This class derives from explicit_error_stepper_fsal_base and inherits its interface via CRTP (current recurring
 * template pattern). The method possesses the FSAL (first-same-as-last) property. See
 * explicit_error_stepper_fsal_base for more details.
 *
 * \tparam State The state type.
 * \tparam Value The value type.
 * \tparam Deriv The type representing the time derivative of the state.
 * \tparam Time The time representing the independent variable - the time.
 * \tparam Algebra The algebra type.
 * \tparam Operations The operations type.
 * \tparam Resizer The resizer policy type.
 */

    /**
     * \fn runge_kutta_dopri5::runge_kutta_dopri5( const algebra_type &algebra )
     * \brief Constructs the runge_kutta_dopri5 class. This constructor can be used as a default
     * constructor if the algebra has a default constructor.
     * \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
     */

    /**
     * \fn runge_kutta_dopri5::do_step_impl( System system , const StateIn &in , const DerivIn &dxdt_in , time_type t , StateOut &out , DerivOut &dxdt_out , time_type dt )
     * \brief This method performs one step. The derivative `dxdt_in` of `in` at the time `t` is passed to the
     * method. The result is updated out-of-place, hence the input is in `in` and the output in `out`. Furthermore,
     * the derivative is update out-of-place, hence the input is assumed to be in `dxdt_in` and the output in
     * `dxdt_out`.
     * Access to this step functionality is provided by explicit_error_stepper_fsal_base and
     * `do_step_impl` should not be called directly.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved. in is not modified in this method
     * \param dxdt_in The derivative of x at t. dxdt_in is not modified by this method
     * \param t The value of the time, at which the step should be performed.
     * \param out The result of the step is written in out.
     * \param dxdt_out The result of the new derivative at time t+dt.
     * \param dt The step size.
     */

    /**
     * \fn runge_kutta_dopri5::do_step_impl( System system , const StateIn &in , const DerivIn &dxdt_in , time_type t , StateOut &out , DerivOut &dxdt_out , time_type dt , Err &xerr )
     * \brief This method performs one step. The derivative `dxdt_in` of `in` at the time `t` is passed to the
     * method. The result is updated out-of-place, hence the input is in `in` and the output in `out`. Furthermore,
     * the derivative is update out-of-place, hence the input is assumed to be in `dxdt_in` and the output in
     * `dxdt_out`.
     * Access to this step functionality is provided by explicit_error_stepper_fsal_base and
     * `do_step_impl` should not be called directly.
     * An estimation of the error is calculated.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved. in is not modified in this method
     * \param dxdt_in The derivative of x at t. dxdt_in is not modified by this method
     * \param t The value of the time, at which the step should be performed.
     * \param out The result of the step is written in out.
     * \param dxdt_out The result of the new derivative at time t+dt.
     * \param dt The step size.
     * \param xerr An estimation of the error.
     */

    /**
     * \fn runge_kutta_dopri5::calc_state( time_type t , StateOut &x , const StateIn1 &x_old , const DerivIn1 &deriv_old , time_type t_old , const StateIn2 &  , const DerivIn2 &deriv_new , time_type t_new ) const
     * \brief This method is used for continuous output and it calculates the state `x` at a time `t` from the
     * knowledge of two states `old_state` and `current_state` at time points `t_old` and `t_new`. It also uses
     * internal variables to calculate the result. Hence this method must be called after two successful `do_step`
     * calls.
     */

    /**
     * \fn runge_kutta_dopri5::adjust_size( const StateIn &x )
     * \brief Adjust the size of all temporaries in the stepper manually.
     * \param x A state from which the size of the temporaries to be resized is deduced.
     */

} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA_DOPRI5_HPP_INCLUDED


#ifndef CANNON_ML_LLOYD_H
#define CANNON_ML_LLOYD_H

/*!
 * \file cannon/ml/lloyd.hpp
 * \brief File containing functions relating to Lloyd's algorithm for Voronoi
 * relaxation.
 */

#include <Eigen/Dense>

using namespace Eigen;

namespace cannon {
  namespace ml {

    /*!
     * \brief Enum class representing the different ways that the centroid
     * integral used by Lloyd's algorithm can be numerically approximated.
     */
    enum class LloydSamplingStrategy {
      Grid,
      Uniform,
    };

    /*!
     * \brief Function to compute a single iteration of the classic Lloyd's
     * algorithm in place within the specified bounds.
     *
     * \param pts The current points making up the Voronoi diagram.
     * \param strat Sampling strategy to use for approximating centroid
     * \param x_low X axis lower bound
     * \param x_high X axis upper bound
     * \param y_low Y axis lower bound
     * \param y_high Y axis upper bound
     */
    void do_lloyd_iteration(
        Matrix2Xd &pts,
        LloydSamplingStrategy strat = LloydSamplingStrategy::Grid,
        double x_low = -1.0, double x_high = 1.0, double y_low = -1.0,
        double y_high = 1.0);

    /*!
     * \brief Function to compute a single iteration of Lloyd's
     * algorithm with respect to the input weighting function in place within
     * the specified bounds.
     *
     * \param pts The current points making up the Voronoi diagram.
     * \param f The weighting function to use. Should be everywhere positive.
     * \param strat Sampling strategy to use for approximating centroid
     * \param x_low X axis lower bound
     * \param x_high X axis upper bound
     * \param y_low Y axis lower bound
     * \param y_high Y axis upper bound
     */
    void do_lloyd_iteration(
        Matrix2Xd &pts, std::function<double(const Vector2d&)> f,
        LloydSamplingStrategy strat = LloydSamplingStrategy::Grid,
        double x_low = -1.0, double x_high = 1.0, double y_low = -1.0,
        double y_high = 1.0);
  }
}

#endif /* ifndef CANNON_ML_LLOYD_H */


//
// $Id: base.hpp 7297 2015-03-12 05:30:33Z paragmallick $
//
//
// Original author: Witold Wolski <wewolski@gmail.com>
//
// Copyright : ETH Zurich
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//

#ifndef BASE_H
#define BASE_H

#include <math.h>
#include <algorithm>
#include <vector>
#include <functional>
#include <numeric>
#include <stdexcept>
#include <limits>
#include <iterator>
#include <cmath>
#include <string>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_integral.hpp>
#include "pwiz/utility/findmf/base/base/utilities/base.hpp"

namespace ralab
{
  namespace base
  {
    namespace base
    {

      /// generates the sequence from, from+/-1, ..., to (identical to from:to).
      template<typename TReal>
      void seq
      (
          TReal from, //!<[in] the starting  value of the sequence
          TReal to, //!<[in] the end value of the sequence
          std::vector<TReal> &result //!<[out] result sequence
          )
      {
        result.clear();
        typedef typename std::vector<TReal>::size_type size_type;
        if( from <= to )
        {
          size_type length = static_cast<size_type>(to - from) + 1;
          result.resize(length);
          std::generate(result.begin() , result.end() , utilities::SeqPlus<TReal>(from));
        }
        else
        {
          size_type length = static_cast<size_type>(from - to) + 1 ;
          result.resize(length);
          std::generate(result.begin() , result.end() , utilities::SeqMinus<TReal>(from));
        }
      }

      /// generates sequence: from, from+by, from + 2*by, ..., up to a sequence value less than, or equal than to.
      /// Specifying to < from and by of positive sign is an error.
      template<typename TReal>
      void seq(
          TReal from,//!<[in] the starting value of the sequence
          TReal to,//!<[in] the end value of the sequence
          TReal by, //!<[in] number: increment of the sequence
          std::vector<TReal> &result //!<[out] result sequence
          )
      {
        result.clear();
        typedef typename std::vector<TReal>::size_type size_type;
        size_type size = static_cast<size_type>(  (to - from)  / by ) + 1u ;
        result.reserve( size );

        if(from <= to)
        {
          if(!(by > 0)){
            throw std::logic_error(std::string( "by > 0" ));
          }
          for(; from <= to; from += by)
          {
            result.push_back(from);
          }
        }
        else
        {
          if(! (by < 0) ){
            throw std::logic_error(std::string( "by < 0" ));
          }
          for(; from >= to; from += by)
          {
            result.push_back(from);
          }
        }
      }

      /// generates sequence: from, to of length
      /// calls seq with \$[ by = ( ( to - from ) / (  length  - 1. ) ) \$]
      template<typename TReal>
      void seq_length(
          TReal from,//!<[in] the starting value of the sequence
          TReal to,//!<[in] the end value of the sequence
          unsigned int length, //!<[in] length of sequence
          std::vector<TReal> &result //!<[out] result sequence
          )
      {
        TReal by = ( ( to - from ) / ( static_cast<TReal>( length ) - 1. ) );
        seq(from, to, by, result);

        //this is required because of machine precision...
        // sometimes by does not add's up to precisely _to_ nad
        if(result.size() < length)
        {
          result.push_back(result[result.size()-1] + by );
        }
        else
        {
          result.resize(length);
        }
      }

      /// generates the sequence [1, 2, ..., length(ref)]
      /// (as if argument along.with had been specified),
      /// unless the argument is numeric of length 1 when it is interpreted as
      ///  1:from (even for seq(0) for compatibility with S).

      template< typename T1 , typename T2 >
      void seq
      (
          std::vector<T1> & ref, //!<[in] take the length from the length of this argument.
          std::vector<T2> & res //!<[out] result sequence
          )
      {
        T2 t(1);
        res.assign(ref.size() , t );
        std::partial_sum( res.begin() , res.end() , res.begin() );
      }

      /// Generates Sequence 1,2,3,....length .
      /// Generates 1, 2, ..., length unless length.out = 0, when it generates
      /// integer(0).

      template<typename TSize, typename TReal>
      typename boost::enable_if<boost::is_integral<TSize>, void>::type
      seq(
          TSize length, //!< [in] length of sequence
          std::vector<TReal> &res //!< [out] result sequence
          )
      {
        TReal t(1);
        res.assign(length , t );
        std::partial_sum(res.begin(),res.end(),res.begin());
      }

      /// MEAN Trimmed arithmetic mean.

      /// ## Default S3 method:
      /// mean(x, trim = 0, na.rm = FALSE, ...)

      /// Arguments
      /// x 	An R object. Currently there are methods for numeric data frames, numeric vectors and dates. A complex vector is allowed for trim = 0, only.
      /// trim 	the fraction (0 to 0.5) of observations to be trimmed from each end of x before the mean is computed. Values outside that range are taken as the nearest endpoint.
      /// na.rm 	a logical value indicating whether NA values should be stripped before the computation proceeds.
      /// ... 	further arguments passed to or from other methods.

      /// Value
      /// For a data frame, a named vector with the appropriate method being applied column by column.
      /// If trim is zero (the default), the arithmetic mean of the values in x is computed, as a numeric or complex vector of length one. If x is not logical (coerced to numeric), integer, numeric or complex, NA is returned, with a warning.
      /// If trim is non-zero, a symmetrically trimmed mean is computed with a fraction of trim observations deleted from each end before the mean is computed.

      template < typename InputIterator >
      inline
      typename std::iterator_traits<InputIterator>::value_type
      mean(
          InputIterator begin, //!< [in]
          InputIterator end //!< [in]
          )
      {
        typedef typename std::iterator_traits<InputIterator>::value_type TReal;
        TReal size = static_cast<TReal>(std::distance(begin,end));
        TReal sum = std::accumulate(begin , end, 0. );
        return(sum/size);
      }

      /// mean
      template <typename TReal>
      inline TReal mean(const std::vector<TReal> & x )
      {
        TReal size = static_cast<TReal>(x.size());
        TReal sum = std::accumulate(x.begin() , x.end(), 0. );
        return ( sum / size ) ;
      }

      /// mean
      template <typename TReal>
      TReal mean(
          const std::vector<TReal> & x, //!< [in]
          TReal trim  //!< [in] trim 0 - 0.5
          )
      {
        if(trim >= 0.5)
        {
          trim = 0.4999999;
        }
        TReal size = static_cast<TReal>(x.size());
        std::vector<TReal> wc(x); //working copy
        std::sort(wc.begin(),wc.end());
        size_t nrelemstrim = static_cast<size_t>(round( size * trim )) ;
        size_t nrelems = std::distance(wc.begin() + nrelemstrim, wc.end() - nrelemstrim );
        TReal sum = std::accumulate(wc.begin() + nrelemstrim , wc.end() - nrelemstrim, 0. );
        return ( sum / static_cast<TReal>( nrelems ) ); //static_cast will be required with boost::math::round
      }

      /// computes the mean
      template<class Iter_T>
      typename std::iterator_traits<Iter_T>::value_type geometricMean(Iter_T first, Iter_T last)
      {
        typedef typename std::iterator_traits<Iter_T>::value_type TReal;
        size_t cnt = distance(first, last);
        std::vector<TReal> copyOfInput(first, last);

        // ln(x)
        typename std::vector<TReal>::iterator inputIt;

        for(inputIt = copyOfInput.begin(); inputIt != copyOfInput.end() ; ++inputIt)
        {
          *inputIt = std::log(*inputIt);
        }

        // sum(ln(x))
        TReal sum( std::accumulate(copyOfInput.begin(), copyOfInput.end(), TReal() ));

        // e^(sum(ln(x))/N)
        TReal geomean( std::exp(sum / cnt) );
        return geomean;
      }

      /// Range of Values
      /// range returns a std::pair containing minimum and maximum of all the given values.
      template<typename TReal>
      void Range(
          const std::vector<TReal> & values, //!< [in] data
          std::pair<TReal,TReal> & range //!< [out] range
          )
      {
        TReal min = * std::min_element(values.begin(),values.end());
        TReal max = * std::max_element(values.begin(),values.end());
        range.first = min;
        range.second = max;
      }

      ///  maximum of 3 numbers
      template<typename T>
      inline double max3(T a, T b, T c)
      {
        T max;
        if(a>b)
          max=a;
        else
          max=b;
        if(max<c)
          max=c;
        return(max);
      }

      /// log base 2
      template<typename TReal>
      inline TReal log2(TReal test)
      {
        if(test==0)
          return(0);
        else
          return( log10( test ) / log10( static_cast<TReal>(2.) ));
      }

    }//namespace BASE ends here
  }//namespace base ends here
}//namespace ralab ends here

#endif // BASE_H


// Implementing the class that is defined in the header file: BarrierOption.hpp
//
// (c) Sudhansh Dua

#include "BarrierOption.hpp"
#include <string>
#include <boost/math/distributions.hpp>
#include <cmath>

using namespace std;
using namespace boost::math;

double BarrierOption::DownAndOutCallBarrier() const
{
	return ::DownAndOutCallBarrier(S, H, K, cr, T, r, sig, b, type, InOrOut);
}

double BarrierOption::DownAndOutPutBarrier() const
{
	return ::DownAndOutPutBarrier(S, H, K, cr, T, r, sig, b, type, InOrOut);
}

double BarrierOption::DownAndInCallBarrier() const
{
	return ::DownAndInCallBarrier(S, H, K, cr, T, r, sig, b, type, InOrOut);
}

double BarrierOption::DownAndInPutBarrier() const
{
	return ::DownAndInPutBarrier(S, H, K, cr, T, r, sig, b, type, InOrOut);
}

double BarrierOption::UpAndOutCallBarrier() const
{
	return ::UpAndOutCallBarrier(S, H, K, cr, T, r, sig, b, type, InOrOut);
}

double BarrierOption::UpAndOutPutBarrier() const
{
	return ::UpAndOutPutBarrier(S, H, K, cr, T, r, sig, b, type, InOrOut);
}

double BarrierOption::UpAndInCallBarrier() const
{
	return ::UpAndInCallBarrier(S, H, K, cr, T, r, sig, b, type, InOrOut);
}

double BarrierOption::UpAndInPutBarrier() const
{
	return ::UpAndInPutBarrier(S, H, K, cr, T, r, sig, b, type, InOrOut);
}

void BarrierOption::init()		// Initialising all the default values
{
	//	Default values
	T = 1;
	H = 105;
	cr = 3;
	r = 0.08;
	sig = 0.25;
	K = 100;
	S = 100;			//	Default stock price
	b = r;				//	Black - Scholes(1973) stock option model: b = r (i.e. non-dividend paying stock)

	type = "C";			//	Call option as default
	InOrOut = "In";			//	In Barrier as default
}

void BarrierOption::copy(const BarrierOption& option)
{
	H = option.H;
	T = option.T;
	InOrOut = option.InOrOut;
	cr = option.cr;
	r = option.r;
	sig = option.sig;
	K = option.K;
	b = option.b;
	type = option.type;
	S = option.S;
}

//	Constructors and destructor
//	Default Constructor
BarrierOption::BarrierOption() : Option()
{
	init();
}

//	Copy constructor
BarrierOption::BarrierOption(const BarrierOption& option) : Option(option)
{
	copy(option);
}

//	Constructor that accepts values
BarrierOption::BarrierOption(const double& S1, const double& H1, const double& K1, const double& cr1, const double& T1, const double& r1, const double& sig1,
	const double& b1, const string type1, const string InOrOut1) : Option(), S(S1), H(H1), K(K1), cr(cr1), T(T1), r(r1), sig(sig1), b(b1), type(type1), InOrOut(InOrOut1) {}

//	Destructor
BarrierOption::~BarrierOption() {}

//	Assignment Operator
BarrierOption& BarrierOption::operator = (const BarrierOption& option)
{
	if (this == &option)
	{
		return *this;		//	Self-assignment check!
	}
	Option::operator = (option);
	copy(option);
	return *this;
}

// Functions that calculate the option price
double BarrierOption::Price() const
{
	if (S >= H)
	{ 							// Down Barrier

		if (InOrOut == "In")						// Down and in barrier
		{

			if (type == "C")
			{												// Down and in call barrier
				return DownAndInCallBarrier();
			}
			else
			{												// Down and in put barrier
				return DownAndInPutBarrier();
			}
		}
		else
		{								// Down and out barrier
			if (type == "C")
			{												// Down and out call barrier
				return DownAndOutCallBarrier();
			}
			else
			{												// Down and out put barrier
				return DownAndOutPutBarrier();
			}
		}
	}

	else
	{							// Up Barrier

		if (InOrOut == "In")
		{								// Up and in barrier

			if (type == "C")
			{												// Up and in call barrier
				return UpAndInCallBarrier();
			}
			else
			{												// Up and in put barrier
				return UpAndInPutBarrier();
			}
		}
		else
		{								// Up and out barrier
			if (type == "C")
			{												// Up and out call barrier
				return UpAndOutCallBarrier();
			}
			else
			{												// Up and out put barrier
				return UpAndOutPutBarrier();
			}
		}
	}
}

// Modifier functions
void BarrierOption::toggle()			//	Change the option type
{
	type = ((type == "C") ? "P" : "C");
}

// Global Functions

double DownAndOutCallBarrier(const double S, const double H, const double K, const double cr, const double T, const double r, const double sig, const double b, const string type, const string InOrOut)
{
	double ita = 1;
	double phi = 1;

	double mu = (b - (sig * sig * 0.5)) / (sig * sig);
	double psi = sqrt((mu * mu) + (2 * r / (sig * sig)));
	double x1 = (log(S / K) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double x2 = (log(S / H) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double y1 = (log(H * H / (S * K)) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double y2 = (log(H / S)) / (sig * sqrt(T)) + ((1 + mu) * sig * sqrt(T));
	double z = (log(H / S) / (sig * sqrt(T))) + (psi * sig * sqrt(T));

	normal_distribution<> standard_normal(0.0, 1.0);
	double A = (phi * S * exp((b - r) * T) * cdf(standard_normal, phi * x1)) - (phi * K * exp(-r * T) * cdf(standard_normal, phi * (x1 - (sig * sqrt(T)))));
	double B = (phi * S * exp((b - r) * T) * cdf(standard_normal, phi * x2)) - (phi * K * exp(-r * T) * cdf(standard_normal, phi * (x2 - (sig * sqrt(T)))));
	double C = (phi * S * pow(H / S, 2 * (mu + 1)) * exp((b - r) * T) * cdf(standard_normal, ita * y1)) - (phi * K * exp(-r * T) * (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y1 - (sig * sqrt(T))))));
	double D = (phi * S * pow(H / S, 2 * (mu + 1)) * exp((b - r) * T) * cdf(standard_normal, ita * y2)) - (phi * K * exp(-r * T) * (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y2 - (sig * sqrt(T))))));
	double E = (cr * exp(-r * T) * ((cdf(standard_normal, ita * (x2 - (sig * sqrt(T))))) - (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y2 - (sig * sqrt(T)))))));
	double F = (cr * ((pow(H / S, mu + psi) * cdf(standard_normal, ita * z)) + (pow(H / S, mu - psi) * cdf(standard_normal, ita * (z - (2 * psi * sig * sqrt(T)))))));

	if (K > H)
	{
		return A - C + F;
	}
	else
		return B - D + F;
}

double DownAndOutPutBarrier(const double S, const double H, const double K, const double cr, const double T, const double r, const double sig, const double b, const string type, const string InOrOut)
{
	double ita = 1;
	double phi = -1;

	double mu = (b - (sig * sig * 0.5)) / (sig * sig);
	double psi = sqrt((mu * mu) + (2 * r / (sig * sig)));
	double x1 = (log(S / K) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double x2 = (log(S / H) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double y1 = (log(H * H / (S * K)) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double y2 = (log(H / S)) / (sig * sqrt(T)) + ((1 + mu) * sig * sqrt(T));
	double z = (log(H / S) / (sig * sqrt(T))) + (psi * sig * sqrt(T));

	normal_distribution<> standard_normal(0.0, 1.0);
	double A = phi * S * exp((b - r) * T) * cdf(standard_normal, phi * x1) - phi * K * exp(-r * T) * cdf(standard_normal, phi * (x1 - (sig * sqrt(T))));
	double B = phi * S * exp((b - r) * T) * cdf(standard_normal, phi * x2) - phi * K * exp(-r * T) * cdf(standard_normal, phi * (x2 - (sig * sqrt(T))));
	double C = (phi * S * pow(H / S, 2 * (mu + 1)) * exp((b - r) * T) * cdf(standard_normal, ita * y1)) - (phi * K * exp(-r * T) * (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y1 - (sig * sqrt(T))))));
	double D = (phi * S * pow(H / S, 2 * (mu + 1)) * exp((b - r) * T) * cdf(standard_normal, ita * y2)) - (phi * K * exp(-r * T) * (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y2 - (sig * sqrt(T))))));
	double E = (cr * exp(-r * T) * ((cdf(standard_normal, ita * (x2 - (sig * sqrt(T))))) - (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y2 - (sig * sqrt(T)))))));
	double F = (cr * ((pow(H / S, mu + psi) * cdf(standard_normal, ita * z)) + (pow(H / S, mu - psi) * cdf(standard_normal, ita * (z - (2 * psi * sig * sqrt(T)))))));

	if (K > H)
	{
		return A - B + C - D + F;
	}
	else
		return F;
}

double DownAndInCallBarrier(const double S, const double H, const double K, const double cr, const double T, const double r, const double sig, const double b, const string type, const string InOrOut)
{
	double ita = 1;
	double phi = 1;

	double mu = (b - (sig * sig * 0.5)) / (sig * sig);
	double psi = sqrt((mu * mu) + (2 * r / (sig * sig)));
	double x1 = (log(S / K) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double x2 = (log(S / H) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double y1 = (log(H * H / (S * K)) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double y2 = (log(H / S)) / (sig * sqrt(T)) + ((1 + mu) * sig * sqrt(T));
	double z = (log(H / S) / (sig * sqrt(T))) + (psi * sig * sqrt(T));

	normal_distribution<> standard_normal(0.0, 1.0);
	double A = phi * S * exp((b - r) * T) * cdf(standard_normal, phi * x1) - phi * K * exp(-r * T) * cdf(standard_normal, phi * (x1 - (sig * sqrt(T))));
	double B = phi * S * exp((b - r) * T) * cdf(standard_normal, phi * x2) - phi * K * exp(-r * T) * cdf(standard_normal, phi * (x2 - (sig * sqrt(T))));
	double C = (phi * S * pow(H / S, 2 * (mu + 1)) * exp((b - r) * T) * cdf(standard_normal, ita * y1)) - (phi * K * exp(-r * T) * (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y1 - (sig * sqrt(T))))));
	double D = (phi * S * pow(H / S, 2 * (mu + 1)) * exp((b - r) * T) * cdf(standard_normal, ita * y2)) - (phi * K * exp(-r * T) * (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y2 - (sig * sqrt(T))))));
	double E = (cr * exp(-r * T) * ((cdf(standard_normal, ita * (x2 - (sig * sqrt(T))))) - (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y2 - (sig * sqrt(T)))))));
	double F = (cr * ((pow(H / S, mu + psi) * cdf(standard_normal, ita * z)) + (pow(H / S, mu - psi) * cdf(standard_normal, ita * (z - (2 * psi * sig * sqrt(T)))))));

	if (K > H)
	{
		return C + E;
	}
	else
		return A - B + D + E;

}

double DownAndInPutBarrier(const double S, const double H, const double K, const double cr, const double T, const double r, const double sig, const double b, const string type, const string InOrOut)
{
	double ita = 1;
	double phi = -1;

	double mu = (b - (sig * sig * 0.5)) / (sig * sig);
	double psi = sqrt((mu * mu) + (2 * r / (sig * sig)));
	double x1 = (log(S / K) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double x2 = (log(S / H) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double y1 = (log(H * H / (S * K)) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double y2 = (log(H / S)) / (sig * sqrt(T)) + ((1 + mu) * sig * sqrt(T));
	double z = (log(H / S) / (sig * sqrt(T))) + (psi * sig * sqrt(T));

	normal_distribution<> standard_normal(0.0, 1.0);
	double A = phi * S * exp((b - r) * T) * cdf(standard_normal, phi * x1) - phi * K * exp(-r * T) * cdf(standard_normal, phi * (x1 - (sig * sqrt(T))));
	double B = phi * S * exp((b - r) * T) * cdf(standard_normal, phi * x2) - phi * K * exp(-r * T) * cdf(standard_normal, phi * (x2 - (sig * sqrt(T))));
	double C = (phi * S * pow(H / S, 2 * (mu + 1)) * exp((b - r) * T) * cdf(standard_normal, ita * y1)) - (phi * K * exp(-r * T) * (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y1 - (sig * sqrt(T))))));
	double D = (phi * S * pow(H / S, 2 * (mu + 1)) * exp((b - r) * T) * cdf(standard_normal, ita * y2)) - (phi * K * exp(-r * T) * (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y2 - (sig * sqrt(T))))));
	double E = (cr * exp(-r * T) * ((cdf(standard_normal, ita * (x2 - (sig * sqrt(T))))) - (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y2 - (sig * sqrt(T)))))));
	double F = (cr * ((pow(H / S, mu + psi) * cdf(standard_normal, ita * z)) + (pow(H / S, mu - psi) * cdf(standard_normal, ita * (z - (2 * psi * sig * sqrt(T)))))));

	if (K > H)
	{
		return B - C + D + E;
	}
	else
		return A + E;
}

double UpAndOutCallBarrier(const double S, const double H, const double K, const double cr, const double T, const double r, const double sig, const double b, const string type, const string InOrOut)
{
	double ita = -1;
	double phi = 1;

	double mu = (b - (sig * sig * 0.5)) / (sig * sig);
	double psi = sqrt((mu * mu) + (2 * r / (sig * sig)));
	double x1 = (log(S / K) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double x2 = (log(S / H) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double y1 = (log(H * H / (S * K)) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double y2 = (log(H / S)) / (sig * sqrt(T)) + ((1 + mu) * sig * sqrt(T));
	double z = (log(H / S) / (sig * sqrt(T))) + (psi * sig * sqrt(T));

	normal_distribution<> standard_normal(0.0, 1.0);
	double A = phi * S * exp((b - r) * T) * cdf(standard_normal, phi * x1) - phi * K * exp(-r * T) * cdf(standard_normal, phi * (x1 - (sig * sqrt(T))));
	double B = phi * S * exp((b - r) * T) * cdf(standard_normal, phi * x2) - phi * K * exp(-r * T) * cdf(standard_normal, phi * (x2 - (sig * sqrt(T))));
	double C = (phi * S * pow(H / S, 2 * (mu + 1)) * exp((b - r) * T) * cdf(standard_normal, ita * y1)) - (phi * K * exp(-r * T) * (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y1 - (sig * sqrt(T))))));
	double D = (phi * S * pow(H / S, 2 * (mu + 1)) * exp((b - r) * T) * cdf(standard_normal, ita * y2)) - (phi * K * exp(-r * T) * (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y2 - (sig * sqrt(T))))));
	double E = (cr * exp(-r * T) * ((cdf(standard_normal, ita * (x2 - (sig * sqrt(T))))) - (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y2 - (sig * sqrt(T)))))));
	double F = (cr * ((pow(H / S, mu + psi) * cdf(standard_normal, ita * z)) + (pow(H / S, mu - psi) * cdf(standard_normal, ita * (z - (2 * psi * sig * sqrt(T)))))));

	if (K > H)
	{
		return F;
	}
	else
		return A - B + C - D + F;

}

double UpAndOutPutBarrier(const double S, const double H, const double K, const double cr, const double T, const double r, const double sig, const double b, const string type, const string InOrOut)
{
	double ita = -1;
	double phi = -1;

	double mu = (b - (sig * sig * 0.5)) / (sig * sig);
	double psi = sqrt((mu * mu) + (2 * r / (sig * sig)));
	double x1 = (log(S / K) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double x2 = (log(S / H) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double y1 = (log(H * H / (S * K)) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double y2 = (log(H / S)) / (sig * sqrt(T)) + ((1 + mu) * sig * sqrt(T));
	double z = (log(H / S) / (sig * sqrt(T))) + (psi * sig * sqrt(T));

	normal_distribution<> standard_normal(0.0, 1.0);
	double A = phi * S * exp((b - r) * T) * cdf(standard_normal, phi * x1) - phi * K * exp(-r * T) * cdf(standard_normal, phi * (x1 - (sig * sqrt(T))));
	double B = phi * S * exp((b - r) * T) * cdf(standard_normal, phi * x2) - phi * K * exp(-r * T) * cdf(standard_normal, phi * (x2 - (sig * sqrt(T))));
	double C = (phi * S * pow(H / S, 2 * (mu + 1)) * exp((b - r) * T) * cdf(standard_normal, ita * y1)) - (phi * K * exp(-r * T) * (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y1 - (sig * sqrt(T))))));
	double D = (phi * S * pow(H / S, 2 * (mu + 1)) * exp((b - r) * T) * cdf(standard_normal, ita * y2)) - (phi * K * exp(-r * T) * (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y2 - (sig * sqrt(T))))));
	double E = (cr * exp(-r * T) * ((cdf(standard_normal, ita * (x2 - (sig * sqrt(T))))) - (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y2 - (sig * sqrt(T)))))));
	double F = (cr * ((pow(H / S, mu + psi) * cdf(standard_normal, ita * z)) + (pow(H / S, mu - psi) * cdf(standard_normal, ita * (z - (2 * psi * sig * sqrt(T)))))));

	if (K > H)
	{
		return B - D + F;
	}
	else
		return A - C + F;
}

double UpAndInCallBarrier(const double S, const double H, const double K, const double cr, const double T, const double r, const double sig, const double b, const string type, const string InOrOut)
{
	double ita = -1;
	double phi = 1;

	double mu = (b - (sig * sig * 0.5)) / (sig * sig);
	double psi = sqrt((mu * mu) + (2 * r / (sig * sig)));
	double x1 = (log(S / K) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double x2 = (log(S / H) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double y1 = (log(H * H / (S * K)) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double y2 = (log(H / S)) / (sig * sqrt(T)) + ((1 + mu) * sig * sqrt(T));
	double z = (log(H / S) / (sig * sqrt(T))) + (psi * sig * sqrt(T));

	normal_distribution<> standard_normal(0.0, 1.0);
	double A = phi * S * exp((b - r) * T) * cdf(standard_normal, phi * x1) - phi * K * exp(-r * T) * cdf(standard_normal, phi * (x1 - (sig * sqrt(T))));
	double B = phi * S * exp((b - r) * T) * cdf(standard_normal, phi * x2) - phi * K * exp(-r * T) * cdf(standard_normal, phi * (x2 - (sig * sqrt(T))));
	double C = (phi * S * pow(H / S, 2 * (mu + 1)) * exp((b - r) * T) * cdf(standard_normal, ita * y1)) - (phi * K * exp(-r * T) * (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y1 - (sig * sqrt(T))))));
	double D = (phi * S * pow(H / S, 2 * (mu + 1)) * exp((b - r) * T) * cdf(standard_normal, ita * y2)) - (phi * K * exp(-r * T) * (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y2 - (sig * sqrt(T))))));
	double E = (cr * exp(-r * T) * ((cdf(standard_normal, ita * (x2 - (sig * sqrt(T))))) - (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y2 - (sig * sqrt(T)))))));
	double F = (cr * ((pow(H / S, mu + psi) * cdf(standard_normal, ita * z)) + (pow(H / S, mu - psi) * cdf(standard_normal, ita * (z - (2 * psi * sig * sqrt(T)))))));

	if (K > H)
	{
		return A + E;
	}
	else
		return B - C + D + E;

}

double UpAndInPutBarrier(const double S, const double H, const double K, const double cr, const double T, const double r, const double sig, const double b, const string type, const string InOrOut)
{
	double ita = -1;
	double phi = -1;

	double mu = (b - (sig * sig * 0.5)) / (sig * sig);
	double psi = sqrt((mu * mu) + (2 * r / (sig * sig)));
	double x1 = (log(S / K) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double x2 = (log(S / H) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double y1 = (log(H * H / (S * K)) / (sig * sqrt(T))) + ((1 + mu) * sig * sqrt(T));
	double y2 = (log(H / S)) / (sig * sqrt(T)) + ((1 + mu) * sig * sqrt(T));
	double z = (log(H / S) / (sig * sqrt(T))) + (psi * sig * sqrt(T));

	normal_distribution<> standard_normal(0.0, 1.0);
	double A = phi * S * exp((b - r) * T) * cdf(standard_normal, phi * x1) - phi * K * exp(-r * T) * cdf(standard_normal, phi * (x1 - (sig * sqrt(T))));
	double B = phi * S * exp((b - r) * T) * cdf(standard_normal, phi * x2) - phi * K * exp(-r * T) * cdf(standard_normal, phi * (x2 - (sig * sqrt(T))));
	double C = (phi * S * pow(H / S, 2 * (mu + 1)) * exp((b - r) * T) * cdf(standard_normal, ita * y1)) - (phi * K * exp(-r * T) * (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y1 - (sig * sqrt(T))))));
	double D = (phi * S * pow(H / S, 2 * (mu + 1)) * exp((b - r) * T) * cdf(standard_normal, ita * y2)) - (phi * K * exp(-r * T) * (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y2 - (sig * sqrt(T))))));
	double E = (cr * exp(-r * T) * ((cdf(standard_normal, ita * (x2 - (sig * sqrt(T))))) - (pow(H / S, 2 * mu) * cdf(standard_normal, ita * (y2 - (sig * sqrt(T)))))));
	double F = (cr * ((pow(H / S, mu + psi) * cdf(standard_normal, ita * z)) + (pow(H / S, mu - psi) * cdf(standard_normal, ita * (z - (2 * psi * sig * sqrt(T)))))));

	if (K > H)
	{
		return A - B + D + E;
	}
	else
		return C + E;
}


#ifndef SKYLARK_INNER_HPP
#define SKYLARK_INNER_HPP

#include <boost/mpi.hpp>

namespace skylark { namespace base {

template<typename T>
inline El::Base<T> Nrm2(const El::Matrix<T>& x) {
    return El::Nrm2(x);
}

template<typename T>
inline El::Base<T> Nrm2(const El::DistMatrix<T>& x) {
    return El::Nrm2(x);
}

template<typename T>
inline El::Base<T> Nrm2(const El::DistMatrix<T, El::VC, El::STAR>& x) {
    boost::mpi::communicator comm(x.DistComm().comm, boost::mpi::comm_attach);
    T local = El::Nrm2(x.LockedMatrix());
    T snrm = boost::mpi::all_reduce(comm, local * local, std::plus<T>());
    return sqrt(snrm);
}

template<typename T>
inline El::Base<T> Nrm2(const El::DistMatrix<T, El::VR, El::STAR>& x) {
    boost::mpi::communicator comm(x.DistComm(), boost::mpi::comm_attach);
    T local = El::Nrm2(x.LockedMatrix());
    T snrm = boost::mpi::all_reduce(comm, local * local, std::plus<T>());
    return sqrt(snrm);
}

template<typename T>
inline El::Base<T> Nrm2(const El::DistMatrix<T, El::STAR, El::STAR>& x) {
    return El::Nrm2(x.LockedMatrix());
}

template<typename T>
inline void ColumnNrm2(const El::Matrix<T>& A,
    El::Matrix<El::Base<T> >& N) {

    N.Resize(A.Width(), 1);
    T *n = N.Buffer();
    const T *a = A.LockedBuffer();
    for(El::Int j = 0; j < A.Width(); j++) {
        n[j] = 0.0;
        for(El::Int i = 0; i < A.Height(); i++)
            n[j] += a[j * A.LDim() + i] * El::Conj(a[j * A.LDim() + i]);
        n[j] = sqrt(n[j]);
    }
}

template<typename T>
inline void ColumnNrm2(const El::DistMatrix<T, El::STAR, El::STAR>& A,
    El::DistMatrix<El::Base<T>, El::STAR, El::STAR>& N) {
    N.Resize(A.Width(), 1);
    T *n = N.Buffer();
    const T *a = A.LockedBuffer();
    for(El::Int j = 0; j < A.Width(); j++) {
        n[j] = 0.0;
        for(El::Int i = 0; i < A.LocalHeight(); i++)
            n[j] += a[j * A.LDim() + i] * El::Conj(a[j * A.LDim() + i]);
        n[j] = sqrt(n[j]);
    }
}

template<typename T, El::Distribution U, El::Distribution V>
inline void ColumnNrm2(const El::DistMatrix<T, U, V>& A,
    El::DistMatrix<El::Base<T>, El::STAR, El::STAR>& N) {

    std::vector<T> n(A.Width(), 1);
    std::fill(n.begin(), n.end(), 0.0);
    const El::Matrix<T> &Al = A.LockedMatrix();
    const T *a = Al.LockedBuffer();
    for(int j = 0; j < Al.Width(); j++)
        for(int i = 0; i < Al.Height(); i++)
            n[A.GlobalCol(j)] +=
                a[j * Al.LDim() + i] * El::Conj(a[j * Al.LDim() + i]);

    N.Resize(A.Width(), 1);
    El::Zero(N);
    boost::mpi::communicator comm(N.Grid().Comm().comm, boost::mpi::comm_attach);
    boost::mpi::all_reduce(comm, n.data(), A.Width(), N.Buffer(), std::plus<T>());
    for(int j = 0; j < A.Width(); j++)
        N.Set(j, 0, sqrt(N.Get(j, 0)));
}

template<typename T>
inline void ColumnNrm2(const El::AbstractDistMatrix<T>& A,
    El::DistMatrix<El::Base<T>, El::STAR, El::STAR>& N) {

    N.Resize(A.Width(), 1);

    if (A.Participating()) {
        std::vector<T> n(A.Width(), 1);
        std::fill(n.begin(), n.end(), 0.0);
        const El::Matrix<T> &Al = A.LockedMatrix();
        const T *a = Al.LockedBuffer();
        for(El::Int j = 0; j < Al.Width(); j++)
            for(El::Int i = 0; i < Al.Height(); i++)
                n[A.GlobalCol(j)] +=
                    a[j * Al.LDim() + i] * El::Conj(a[j * Al.LDim() + i]);

        El::Zero(N);
        El::mpi::AllReduce(n.data(), N.Buffer(), A.Width(), MPI_SUM,
            A.DistComm());
        for(El::Int j = 0; j < A.Width(); j++)
            N.Set(j, 0, sqrt(N.Get(j, 0)));
    }

    El::mpi::Broadcast(N.Buffer(), A.Width(), A.Root(), A.CrossComm());
}

template<typename T>
inline void ColumnDot(const El::Matrix<T>& A, const El::Matrix<T>& B,
    El::Matrix<T>& N) {

    // TODO just assuming sizes are OK for now.

    T *n = N.Buffer();
    const T *a = A.LockedBuffer();
    const T *b = B.LockedBuffer();
    for(El::Int j = 0; j < A.Width(); j++) {
        n[j] = 0.0;
        for(El::Int i = 0; i < A.Height(); i++)
            n[j] += a[j * A.LDim() + i] * El::Conj(b[j * B.LDim() + i]);
    }
}

template<typename T>
inline void ColumnDot(const El::DistMatrix<T, El::STAR, El::STAR>& A,
    const El::DistMatrix<T, El::STAR, El::STAR>& B,
    El::DistMatrix<T, El::STAR, El::STAR>& N) {

    // TODO just assuming sizes are OK for now.

    T *n = N.Buffer();
    const T *a = A.LockedBuffer();
    const T *b = B.LockedBuffer();
    for(El::Int j = 0; j < A.Width(); j++) {
        n[j] = 0.0;
        for(El::Int i = 0; i < A.LocalHeight(); i++)
            n[j] += a[j * A.LDim() + i] * El::Conj(b[j * B.LDim() + i]);
    }
}

template<typename T, El::Distribution U, El::Distribution V>
inline void ColumnDot(const El::DistMatrix<T, U, V>& A,
    const El::DistMatrix<T, U, V>& B,
    El::DistMatrix<T, El::STAR, El::STAR>& N) {

    // TODO just assuming sizes are OK for now, and grid aligned.
    std::vector<T> n(A.Width(), 1);
    std::fill(n.begin(), n.end(), 0);
    const El::Matrix<T> &Al = A.LockedMatrix();
    const T *a = Al.LockedBuffer();
    const El::Matrix<T> &Bl = B.LockedMatrix();
    const T *b = Bl.LockedBuffer();
    for(El::Int j = 0; j < Al.Width(); j++)
        for(El::Int i = 0; i < Al.Height(); i++)
            n[A.GlobalCol(j)] +=
                a[j * Al.LDim() + i] * El::Conj(b[j * Bl.LDim() + i]);

   N.Resize(A.Width(), 1);
   El::Zero(N);
   boost::mpi::communicator comm(N.Grid().Comm().comm, boost::mpi::comm_attach);
   boost::mpi::all_reduce(comm, n.data(), A.Width(), N.Buffer(), std::plus<T>());
}

template<typename T>
inline void RowDot(const El::Matrix<T>& A, const El::Matrix<T>& B,
    El::Matrix<T>& N) {

    // TODO just assuming sizes are OK for now.

    T *n = N.Buffer();
    const T *a = A.LockedBuffer();
    const T *b = B.LockedBuffer();
    for(El::Int i = 0; i < A.Height(); i++)
        n[i] = 0.0;

    for(El::Int j = 0; j < A.Width(); j++) {
        for(El::Int i = 0; i < A.Height(); i++)
            n[i] += a[j * A.LDim() + i] * El::Conj(b[j * B.LDim() + i]);
    }
}

} } // namespace skylark::base

#endif // SKYLARK_INNER_HPP