/*
 *  testClipTriangleJacobians.cpp
 *  Trogdor6
 *
 *  Created by Paul C Hansen on 8/11/11.
 *  Copyright 2011 Stanford University. All rights reserved.
 *
 *  This file is covered by the MIT license.  See LICENSE.txt.
 */

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE testClipTriangleJacobians

#include <boost/test/unit_test.hpp>

#include <iostream>
#include <cstdlib>
#include <ctime>
#include "Voxelize/ClipTriangle.h"
#include "SimpleMesh/Search.h"
#include "TimeWrapper.h"

using namespace std;
using namespace SimpleMesh;

//static ClippedTriangle sClip(const ClippedTriangle & ctri, Rect2d xyBounds);
static Matrix3d sDelta(const map<unsigned int, Matrix3<Vector3d> > & jacobian,
    Vector3d delta);

//#define CHECK_CLOSE(p0, p1, tol) BOOST_CHECK_SMALL(norm((p0)-(p1)), tol)
#define CHECK_CLOSE(p0, p1) BOOST_CHECK_SMALL(norm((p0)-(p1)), 1e-9)
#define CHECK_MATRIX_CLOSE(m0, m1) BOOST_CHECK_SMALL(vectorNorm((m0)-(m1)), 1e-9);

const void* NULLPTR = 0L;
const int X = 0, Y = 1, Z = 2;
//
//void printJacobian(const map<unsigned int, Vector3d> & dFdv)
//{
//    map<unsigned int, Vector3d>::const_iterator itr;
//    for (itr = dFdv.begin(); itr != dFdv.end(); itr++)
//    {
//        cerr << itr->first << ": " << itr->second << "\n";
//    }
//}
//void printJacobian(const map<unsigned int, Matrix3<Vector3d> > & dFdv)
//{
//    map<unsigned int, Matrix3<Vector3d> >::const_iterator itr;
//    for (itr = dFdv.begin(); itr != dFdv.end(); itr++)
//    {
//        cerr << itr->first << ": " << itr->second << "\n";
//    }
//}

BOOST_AUTO_TEST_CASE( IntegrateDz_Triangle )
{
    Vector3d o(0,0,0), x(1,0,0), y(0,1,0), z(0,0,1), xy(1,1,0);
    Triangle xyTriangle0(SimpleMesh::Test::oneTri(o + z/2, x + z/2, y + z/2));
    ClippedTriangle ctri0(xyTriangle0);

//    double vol0 = ctri0.volume(Z);
    double area0 = ctri0.area2d(Z);

    map<unsigned int, Vector3d> jacobian;
    ctri0.intDz(jacobian);

    for (int vv = 0; vv < 3; vv++)
    {
        Vector3d dFdv = jacobian[vv];
        BOOST_CHECK_EQUAL(dFdv[0], 0.0);
        BOOST_CHECK_EQUAL(dFdv[1], 0.0);
        BOOST_CHECK_CLOSE(dFdv[2], area0/3.0, 1e-7);
    }

    /*
    cerr << "Initial volume is " << vol0 << "\n";

    Vector3d dz = 1e-6*z;
    Triangle xyTriangle1(SimpleMesh::Test::oneTri(o + z/2 + dz, x + z/2, y + z/2));
    ClippedTriangle ctri1(xyTriangle1);

    double vol1 = ctri1.volume(Z);

    cerr << "New volume is " << vol1 << "\n";
    cerr << "deltaV = " << vol1 - vol0 << "\n";

    cerr << "Jacobian has " << jacobian.size() << " elements.\n";
    printJacobian(jacobian);
    */
}

BOOST_AUTO_TEST_CASE( IntegrateDz_ClippedTriangle2D )
{
//    cerr << "SKIPPING\n"; return;
    Vector3d o(0,0,0), x(1,0,0), y(0,1,0), z(0,0,1), xy(1,1,0);
    Triangle xyTriangle0(SimpleMesh::Test::oneTri(o + z/2, x + z/2, y + z/2));

    Rect2d clipBounds(0.2, 0.1, 0.9, 0.8);
//    ClippedTriangle ctri0 = sClip(ClippedTriangle(xyTriangle0), clipBounds);
    ClippedTriangle ctri0 = ClippedTriangle(xyTriangle0).partInside(clipBounds);

    double vol0 = ctri0.volume(Z);
//    double area0 = ctri0.area2d(Z);

    map<unsigned int, Vector3d> jacobian;
    ctri0.intDz(jacobian);

//    cerr << "Initial volume is " << vol0 << "\n";

    Vector3d dz = 1e-6*z;
    for (int vv = 0; vv < 3; vv++)
    {
        Triangle3d tri(o + z/2, x + z/2, y + z/2);
        tri[vv] += dz;
        Triangle xyTriangle1(SimpleMesh::Test::oneTri(tri[0], tri[1], tri[2]));
        ClippedTriangle ctri1 = ClippedTriangle(xyTriangle1).partInside(clipBounds);

        double vol1 = ctri1.volume(Z);

        double dVdv = (vol1-vol0)/norm(dz);

//        cerr << "dVdv = " << dVdv << "\n";
        BOOST_CHECK_EQUAL(jacobian[vv][X], 0.0);
        BOOST_CHECK_EQUAL(jacobian[vv][Y], 0.0);
        BOOST_CHECK_CLOSE(jacobian[vv][Z], dVdv, 1e-7);
    }

//    printJacobian(jacobian);
}

BOOST_AUTO_TEST_CASE( IntegrateZDhTriangle )
{
//    cerr << "SKIPPING\n"; return;
    Vector3d o(0,0,0), x(1,0,0), y(0,1,0), z(0,0,1), xy(1,1,0);
    Triangle xyTriangle0(SimpleMesh::Test::oneTri(o + z/2, x + z/2, y + z/2));
    ClippedTriangle ctri0(xyTriangle0);

//    double vol0 = ctri0.volume(Z);
//    double area0 = ctri0.area2d(Z);

    map<unsigned int, Vector3d> jacobian;
    ctri0.intZDh(jacobian, Z);

    //printJacobian(jacobian);

    // It's pretty easy to work out these answers on paper.

    Vector3d dFdv0(-0.25, -0.25, 0.0);
    Vector3d dFdv1(0.25, 0, 0);
    Vector3d dFdv2(0.0, 0.25, 0.0);

    CHECK_CLOSE(jacobian[0], dFdv0);
    CHECK_CLOSE(jacobian[1], dFdv1);
    CHECK_CLOSE(jacobian[2], dFdv2);
}

BOOST_AUTO_TEST_CASE( IntegrateZDhTriangle_ClipXY )
{
//    cerr << "SKIPPING\n"; return;
    Vector3d o(0,0,0), x(1,0,0), y(0,1,0), z(0,0,1), xy(1,1,0);
    Triangle3d tri0(o+z/2, x+z/2, y+z/2);
    Triangle xyTriangle0(SimpleMesh::Test::oneTri(tri0[0], tri0[1], tri0[2]));

    Rect2d clipBounds(0.2, 0.1, 0.9, 0.8);
    ClippedTriangle ctri0 = ClippedTriangle(xyTriangle0).partInside(clipBounds);

    double vol0 = ctri0.volume(Z);
    double area0 = ctri0.area2d(Z);

    map<unsigned int, Vector3d> jacobian;
    ctri0.intZDh(jacobian, Z);

    double eps = 1e-8; // roughly the most accurate point?  eh.
    for (int vv = 0; vv < 3; vv++)
    for (int xy = 0; xy < 2; xy++)
    {
        Triangle3d tri1(tri0);
        tri1[vv][xy] += eps;

        Triangle xyTriangle1(SimpleMesh::Test::oneTri(tri1[0], tri1[1], tri1[2]));
        ClippedTriangle ctri1 = ClippedTriangle(xyTriangle1).partInside(clipBounds);

        double vol1 = ctri1.volume(Z);

        double dFdv = (vol1-vol0)/eps;

        if (jacobian[vv][xy] == 0.0)
            BOOST_CHECK_SMALL(dFdv, 1e-3);
        else
            BOOST_CHECK_CLOSE(jacobian[vv][xy], dFdv, 1e-3);
    }
}

BOOST_AUTO_TEST_CASE( ClipZ )
{
//    cerr << "SKIPPING\n"; return;
    Vector3d o(0,0,0), x(1,0,0), y(0,1,0), z(0,0,1), xy(1,1,0);
    Triangle3d tri0(o, x+z, y);
    Triangle xyTriangle0(SimpleMesh::Test::oneTri(tri0[0], tri0[1], tri0[2]));

    Plane3d planeZ(Vector3d::unit(Z), -0.5); // z = 0.5

    SensitiveEdge newEdge(xyTriangle0, planeZ);

    ClippedTriangle ctri0(xyTriangle0);
    ctri0 = ctri0.partAbove(Z, 0.5, &newEdge);

    double vol0 = ctri0.volume(Z);
    double area0 = ctri0.area2d(Z);

    map<unsigned int, Vector3d> interiorJacobian, totalJacobian;
//    ctri0.intZDh(jacobian);
    ctri0.intDz(interiorJacobian);

    // Yes, these are magic numbers.  It took me two hours or so to derive them.
    // Just be careful and you'll be ok.  :-D
    BOOST_CHECK_CLOSE(interiorJacobian[0][0], -1.0/48, 1e-6);
    BOOST_CHECK_SMALL(interiorJacobian[0][1], 1e-6);
    BOOST_CHECK_CLOSE(interiorJacobian[0][2], 1.0/48, 1e-6);
    BOOST_CHECK_CLOSE(interiorJacobian[1][0], -1.0/12, 1e-6);
    BOOST_CHECK_SMALL(interiorJacobian[1][1], 1e-6);
    BOOST_CHECK_CLOSE(interiorJacobian[1][2], 1.0/12, 1e-6);
    BOOST_CHECK_CLOSE(interiorJacobian[2][0], -1.0/48, 1e-6);
    BOOST_CHECK_SMALL(interiorJacobian[2][1], 1e-6);
    BOOST_CHECK_CLOSE(interiorJacobian[2][2], 1.0/48, 1e-6);

    ctri0.intZDh(totalJacobian, Z);
    ctri0.intDz(totalJacobian);

//    printJacobian(totalJacobian);

    double eps = 1e-8; // roughly the most accurate point?  eh.
    for (int vv = 0; vv < 3; vv++)
    {
        Vector3d dFdv(0,0,0);
        for (int xyz = 0; xyz < 3; xyz++)
        {
            Triangle3d tri1(tri0);
            tri1[vv][xyz] += eps;

            Triangle xyTriangle1(SimpleMesh::Test::oneTri(tri1[0], tri1[1], tri1[2]));
            ClippedTriangle ctri1 = ClippedTriangle(xyTriangle1).partAbove(Z, 0.5);

            double vol1 = ctri1.volume(Z);

            dFdv[xyz] = (vol1-vol0)/eps;

            if (fabs(totalJacobian[vv][xyz]) < 1e-10)
                BOOST_CHECK_SMALL(dFdv[xyz], 1e-6);
            else
                BOOST_CHECK_CLOSE(totalJacobian[vv][xyz], dFdv[xyz], 1e-5);
        }
//        cerr << vv << ": " << dFdv << "\n";
    }
}

static void sIntegrateZDhTriangle_ClipZ(Triangle3d tri0)
{
//    cerr << "SKIPPING\n"; return;
    Triangle xyTriangle0(SimpleMesh::Test::oneTri(tri0[0], tri0[1], tri0[2]));

    Rect2d clipBounds(0.3, 0.2, 0.8, 0.7);
    Plane3d zHalf(Vector3d::unit(Z), -0.5);

    SensitiveEdge zEdge(xyTriangle0, zHalf);

    ClippedTriangle ctri0 = ClippedTriangle(xyTriangle0).partInside(clipBounds);
    ctri0 = ctri0.partAbove(Z, 0.5, &zEdge);

    double vol0 = ctri0.volume(Z);
    double area0 = ctri0.area2d(Z);

    map<unsigned int, Vector3d> jacobian;
    ctri0.intZDh(jacobian, Z);
    ctri0.intDz(jacobian);

    double eps = 1e-8; // roughly the most accurate point?  eh.
    for (int vv = 0; vv < 3; vv++)
    for (int xyz = 0; xyz < 3; xyz++)
    {
        Triangle3d tri1(tri0);
        tri1[vv][xyz] += eps;

        Triangle xyTriangle1(SimpleMesh::Test::oneTri(tri1[0], tri1[1], tri1[2]));
        ClippedTriangle ctri1 = ClippedTriangle(xyTriangle1).partInside(clipBounds);
        ctri1 = ctri1.partAbove(Z, 0.5);

        double vol1 = ctri1.volume(Z);

        double dFdv = (vol1-vol0)/eps;

        if (fabs(jacobian[vv][xyz]) < 1e-10)
            BOOST_CHECK_SMALL(dFdv, 1e-6);
        else
            BOOST_CHECK_CLOSE(jacobian[vv][xyz], dFdv, 1e-4);
    }
}

BOOST_AUTO_TEST_CASE( IntegrateZDhTriangle_ClipZ_Upfacing )
{
    Vector3d o(0,0,0), x(1,0,0), y(0,1,0), z(0,0,1), xy(1,1,0);
    Triangle3d tri0(o, x+z, y);
    sIntegrateZDhTriangle_ClipZ(tri0);
}

BOOST_AUTO_TEST_CASE( IntegrateZDhTriangle_ClipZ_Downfacing )
{
    Vector3d o(0,0,0), x(1,0,0), y(0,1,0), z(0,0,1), xy(1,1,0);
    Triangle3d tri0(o, y, x+z);
    sIntegrateZDhTriangle_ClipZ(tri0);
}

void sIntDNNT(Triangle3d tri0)
{
    Triangle xyTriangle0(SimpleMesh::Test::oneTri(tri0[0], tri0[1], tri0[2]));

    Rect2d clipBounds(0.1, 0.1, 0.4, 0.4);
    ClippedTriangle ctri0 = ClippedTriangle(xyTriangle0).partInside(clipBounds);

    Matrix3d nnT = outerProduct(ctri0.arealNormal(), unit(ctri0.arealNormal()));

    map<unsigned int, Matrix3<Vector3d> > jacobian;
    ctri0.intDnnT(jacobian);

//    cerr << "Orientation:\n" << nnT << "\n";
//    cerr << "Jacobian:\n";
//    printJacobian(jacobian);

    double eps = 1e-8; // roughly the most accurate point?  eh.
    for (int vv = 0; vv < 3; vv++)
    for (int xyz = 0; xyz < 3; xyz++)
    {
        Triangle3d tri1(tri0);
        tri1[vv][xyz] += eps;

        Triangle xyTriangle1(SimpleMesh::Test::oneTri(tri1[0], tri1[1], tri1[2]));
        ClippedTriangle ctri1 = ClippedTriangle(xyTriangle1).partInside(clipBounds);

        Matrix3d nnT1 = outerProduct(ctri1.arealNormal(),
            unit(ctri1.arealNormal()));

        Matrix3d deltaNNT = (nnT1 - nnT)/eps;

        for (int ii = 0; ii < 3; ii++)
        for (int jj = 0; jj < 3; jj++)
        if (jacobian[vv](ii,jj)[xyz] == 0.0)
            BOOST_CHECK_SMALL(deltaNNT(ii,jj), 1e-6);
        else
            BOOST_CHECK_CLOSE(jacobian[vv](ii,jj)[xyz], deltaNNT(ii,jj), 1e-3);
    }
}

// To test this we can clip the triangle down to have no boundaries left.
BOOST_AUTO_TEST_CASE( IntDNNT_Upfacing )
{
//    cerr << "SKIPPING\n"; return;
    Vector3d o(0,0,0), x(1,0,0), y(0,1,0), z(0,0,1);
//    Triangle3d tri0(o+z/2, x+z/2, y+z/2);
    Triangle3d tri0(o + 0.1*z, x - 0.2*z, y + 0.1*x + 0.3*z);

    sIntDNNT(tri0);
}

BOOST_AUTO_TEST_CASE( IntDNNT_Downfacing )
{
//    cerr << "SKIPPING\n"; return;
    Vector3d o(0,0,0), x(1,0,0), y(0,1,0), z(0,0,1);
//    Triangle3d tri0(o+z/2, x+z/2, y+z/2);
    Triangle3d tri0(o + 0.1*z, y + 0.1*x + 0.3*z, x - 0.2*z);

    sIntDNNT(tri0);
}

BOOST_AUTO_TEST_CASE( IntNNTDh_ClipRight_Unflipped )
{
//    cerr << "SKIPPING\n"; return;
    cerr << "\n==== IntNNTDh_CLipRight_Unflipped\n\n";
    Vector3d o(0,0,0), x(1,0,0), y(0,1,0), z(0,0,1);
    Triangle3d tri0(o+z/2, x+z/2, y+z/2);
    Triangle xyTriangle0(SimpleMesh::Test::oneTri(tri0[0], tri0[1], tri0[2]));

    ClippedTriangle ctri0 = ClippedTriangle(xyTriangle0).partBelow(X, 0.5);
//    cerr << "Tri: \n" << ctri0 << "\n";

    Matrix3d nnTA = ctri0.nnTA(); //outerProduct(ctri0.arealNormal(), unit(ctri0.arealNormal()));

    map<unsigned int, Matrix3<Vector3d> > jacobian;
    ctri0.intNNTDh(jacobian);

//    cerr << "Orientation:\n" << nnTA << "\n";
//    cerr << "Jacobian:\n";
//    printJacobian(jacobian);

    // The integral contributions along each edge can be worked out in yer head.
    // Here they are:

    // Edge 0:
    // dA/dp0 = [0 -3/8 0]
    // dA/dp1 = [0 -1/8 0]
    // dA/dp2 = 0

    // Edge 1:
    // dA/dp0 = 0
    // dA/dp1 = [1/8 1/8 0]
    // dA/dp2 = [3/8 3/8 0]

    // Edge 2:
    // dA/dp0 = [-1/2 0 0]
    // dA/dp1 = 0
    // dA/dp2 = [-1/2 0 0]

    // Total:
    // dA/dp0 = [-1/2 -3/8 0]
    // dA/dp1 = [1/8 0 0]
    // dA/dp2 = [-1/8 3/8 0]

    // The sensitivity in question is just the orientation factor times the
    // areal sensitivity.  When the normal vector is unit(z), the answer is:
    CHECK_CLOSE(jacobian[0](Z,Z), Vector3d(-0.5, -0.375, 0));
    CHECK_CLOSE(jacobian[1](Z,Z), Vector3d(0.125, 0, 0));
    CHECK_CLOSE(jacobian[2](Z,Z), Vector3d(-0.125, 0.375, 0));

    // make sure all the parts that should be zero ARE zero.
    for (int vert = 0; vert < 3; vert++)
    for (int ii = 0; ii < 3; ii++)
    for (int jj = 0; jj < 3; jj++)
    if (ii != Z || jj != Z)
    {
        BOOST_CHECK_SMALL(norm(jacobian[vert](ii,jj)), 1e-6);
    }

    Vector3d delta(1e-6, 0, 0);
    Triangle3d tri1 = tri0 + delta;
    Triangle xyTriangle1(SimpleMesh::Test::oneTri(tri1[0], tri1[1], tri1[2]));
    ClippedTriangle ctri1 = ClippedTriangle(xyTriangle1).partBelow(X, 0.5);
//    cerr << "Tri perturbed:\n" << ctri1 << "\n";

    Matrix3d nnTA1 = ctri1.nnTA(); //outerProduct(ctri1.arealNormal(), unit(ctri1.arealNormal()));

    Matrix3d deltaNNT_expected = sDelta(jacobian, delta);

//    cerr << "Delta:\n" << nnTA1 - nnTA << "\n";
//    cerr << "Delta expected:\n" << deltaNNT_expected << "\n";
}

BOOST_AUTO_TEST_CASE( IntNNTDh_ClipRight_Tilty )
{
    cerr << "SKIPPING\n"; return;
    cerr << "\n\n==== IntNNTDh_CLipRight_Unflipped\n\n";
    Vector3d o(0,0,0), x(1,0,0), y(0,1,0), z(0,0,1);
    Triangle3d tri0(o+z/2, x+z/2, y+0.49*z);
    Triangle xyTriangle0(SimpleMesh::Test::oneTri(tri0[0], tri0[1], tri0[2]));

    ClippedTriangle ctri0 = ClippedTriangle(xyTriangle0).partBelow(X, 0.5);
//    cerr << "Tri: \n" << ctri0 << "\n";

//    Matrix3d nnT = outerProduct(ctri0.arealNormal(), unit(ctri0.arealNormal()))
//        / unit(ctri0.arealNormal())[Z];

    Matrix3d nnTdA = ctri0.triangle()->nnTdA();

    map<unsigned int, Matrix3<Vector3d> > jacobian;
    ctri0.intNNTDh(jacobian);

//    cerr << "Orientation:\n" << nnTdA << "\n";
//    cerr << "Jacobian:\n";
//    printJacobian(jacobian);

    // The integral contributions along each edge can be worked out in yer head.
    // Here they are:

    // Edge 0:
    // dA/dp0 = [0 -3/8 0]
    // dA/dp1 = [0 -1/8 0]
    // dA/dp2 = 0

    // Edge 1:
    // dA/dp0 = 0
    // dA/dp1 = [1/8 1/8 0]
    // dA/dp2 = [3/8 3/8 0]

    // Edge 2:
    // dA/dp0 = [-1/2 0 0]
    // dA/dp1 = 0
    // dA/dp2 = [-1/2 0 0]

    // Total:
    // dA/dp0 = [-1/2 -3/8 0]
    // dA/dp1 = [1/8 0 0]
    // dA/dp2 = [-1/8 3/8 0]

    // The sensitivity in question is just the orientation factor times the
    // areal sensitivity.

//    for (int ii = 0; ii < 3; ii++)
//    for (int jj = 0; jj < 3; jj++)
//    {
//        Vector3d jacobian0 = nnTdA(ii,jj) * Vector3d(-0.5, -0.375, 0);
//
//        Vector3d jacobian1 = nnTdA(ii,jj) * Vector3d(0.125, 0, 0);
//
//        Vector3d jacobian2 = nnTdA(ii,jj) * Vector3d(-0.125, 0.375, 0.0);
//
//        cerr << "jacobian0 " << jacobian0 << " vs " << jacobian[0](ii,jj) << "\n";
//        cerr << "jacobian1 " << jacobian1 << " vs " << jacobian[1](ii,jj) << "\n";
//        cerr << "jacobian2 " << jacobian2 << " vs " << jacobian[2](ii,jj) << "\n";
//    }

    Vector3d delta(1e-6, 0, 0);
    Triangle3d tri1 = tri0 + delta;
    Triangle xyTriangle1(SimpleMesh::Test::oneTri(tri1[0], tri1[1], tri1[2]));
    ClippedTriangle ctri1 = ClippedTriangle(xyTriangle1).partBelow(X, 0.5);
//    cerr << "Tri perturbed:\n" << ctri1 << "\n";

    Matrix3d nnT1dA = ctri1.triangle()->nnTdA();

    Matrix3d deltaNNT = ctri1.triangle()->nnTdA()*ctri1.area2d(Z) -
        ctri0.triangle()->nnTdA()*ctri0.area2d(Z);
    Matrix3d deltaNNT_expected = sDelta(jacobian, delta);

//    cerr << "Delta:\n" << deltaNNT << "\n";
//    cerr << "Delta expected:\n" << deltaNNT_expected << "\n";
}

static ClippedTriangle sBugsy(vector<ControlVertex> cvs)
{
    Triangle3d ctri0(cvs[0].point(), cvs[1].point(), cvs[2].point());
    SimpleMesh::Triangle* meshTri0 = new SimpleMesh::Triangle(ctri0);
    meshTri0->controlVertices(cvs[0], cvs[1], cvs[2]);
    meshTri0->edgeControlVertices(cvs[0], cvs[1], cvs[2]);
    meshTri0->cacheSensitivity();

    ClippedTriangle clipt0(*meshTri0);
//    cerr << "Pre-clipped:\n" << clipt0 << "\n";

    // Clipping:
    // y in [-0.5 0.5]
    // z in [-0.5 0.5]
    // x in [0 1]

//    Rect3d pb(-0.5, 0, -0.5, 0.5, 1, 0.5);
    Rect3d pb(1, -2.0/3, -1, 3, 2.0/3, -1.0/3);

    Plane3d bottomPlane(Vector3d::unit(0), -pb.p1[0]);
    Plane3d topPlane(Vector3d::unit(0), -pb.p2[0]);

    Plane3d topY(Vector3d::unit(Y), -pb.p2[1]),
        botY(Vector3d::unit(Y), -pb.p1[1]),
        topZ(Vector3d::unit(Z), -pb.p2[2]),
        botZ(Vector3d::unit(Z), -pb.p1[2]);

//    cerr << "Planes:\n"
//        << botY << "\n" << topY << "\n" << botZ << "\n" << topZ << "\n";

    SensitiveEdge* edgeTopY = new SensitiveEdge(*clipt0.triangle(), topY);
    SensitiveEdge* edgeBotY = new SensitiveEdge(*clipt0.triangle(), botY);

    clipt0 = clipt0.partAbove(Y, pb.p1[1], edgeBotY);
    clipt0 = clipt0.partBelow(Y, pb.p2[1], edgeTopY);

    SensitiveEdge* edgeAbove = new SensitiveEdge(*clipt0.triangle(), topPlane);
    SensitiveEdge* edgeBelow = new SensitiveEdge(*clipt0.triangle(), bottomPlane);

//    cerr << "Above: " << topPlane << " edge " << edgeAbove->line()
//        << "\n";
//    cerr << "Below: " << bottomPlane << " edge " << edgeBelow->line() << "\n";

    clipt0 = clipt0.partAbove(X, pb.p1[0], edgeBelow);
    clipt0 = clipt0.partBelow(X, pb.p2[0], edgeAbove);

    return clipt0;
}

// Stick a triangle up so its top vertex lies exactly on a clipping boundary.
// Make sure that the Jacobians are ok.
//BOOST_AUTO_TEST_CASE( VertexOnBoundary )
//{
//    // Triangle faces down.  This is a regression test and I'm imitating
//    // the problem there.
//
//    Vector3d cv0(0,0,0);
//    Vector3d cv1(0,1,0);
//    Vector3d cv2(1,0,0);
//
//    vector<ControlVertex> cvs0, cvs1;
//    cvs0.push_back(ControlVertex(cv0, 0));
//    cvs0.push_back(ControlVertex(cv1, 1));
//    cvs0.push_back(ControlVertex(cv2, 4));
//
//    SimpleMesh::Triangle tri( Triangle3d(cv0, cv1, cv2) );
//    tri.controlVertices(cvs0);
//
//    // Now clip the triangle to the y = 1 plane.
//
//    ClippedTriangle preClipped(tri);
//    ClippedTriangle clipped = preClipped.partBelow(1, 1.0);
//}

BOOST_AUTO_TEST_CASE( BugChaser )
{
    cerr << "SKIPPING\n"; return;
    cerr << "\n\n===== BugChaser\n\n";

    // First triangle
//    Vector3d cv0(-0.1830127018922193, 0.81698729810778059, 0);
//    Vector3d cv1(0.68301270189221941, 0.3169872981077807, 0);
//    Vector3d cv4(-0.1830127018922193, 0.81698729810778059, 1);

    // Second triangle
//    Vector3d cv0(0.683013, 0.316987, 0);
//    Vector3d cv1(0.683013, 0.316987, 1);
//    Vector3d cv4(-0.183013, 0.816987, 1);

    // Third triangle
//    Vector3d cv0(1.18301, 1.18301, 0);
//    Vector3d cv1(1.18301, 1.18301, 1);
//    Vector3d cv4(0.683013, 0.316987, 1);

    // Fourth triangle
//    Vector3d cv0(0.683013, 0.316987, 0);
//    Vector3d cv1(1.18301, 1.18301, 0);
//    Vector3d cv4(0.683013, 0.316987, 1);

    // From standard block, Triangle 1
    Vector3d cv0(-2, -1.33333, -0.666667);
    Vector3d cv1(-2, 1.33333, -0.666667);
    Vector3d cv4(2, -1.33333, -0.666667);

//    Vector3d delta(1e-5, 0, 0);
    Vector3d delta(0, 1e-5, 0);

    vector<ControlVertex> cvs0, cvs1;
    cvs0.push_back(ControlVertex(cv0, 0));
    cvs0.push_back(ControlVertex(cv1, 1));
    cvs0.push_back(ControlVertex(cv4, 4));

    ClippedTriangle clipt0 = sBugsy(cvs0);
    cerr << "Clipped triangle:\n"
        << "quickPatch(" << clipt0 << ", 'g');\n";

    cvs1 = cvs0;
    for (int nn = 0; nn < 3; nn++)
    {
        cvs1[nn].point(cvs1[nn].point() + delta);
    }
    ClippedTriangle clipt1 = sBugsy(cvs1);
    cerr << "New one:\n"
        << "quickPatch(" << clipt1 << ", 'g');\n";

    map<unsigned int, Matrix3<Vector3d> > DNNT, NNTDh, ADnnT;
    // = clipt0.DNNT(X);
    DNNT = clipt0.DNNT(clipt0.triangle()->upAxis());
    clipt0.intNNTDh(NNTDh);
    clipt0.intDnnT(ADnnT);

    cerr << "DNNT = \n";
    printJacobian(DNNT);
    cerr << "\nNNTDh =\n";
    printJacobian(NNTDh);
    cerr << "\nADnnT =\n";
    printJacobian(ADnnT);

    Matrix3d nnTA0 = clipt0.nnTA();
    Matrix3d nnTA1 = clipt1.nnTA();

    Matrix3d deltaNNT = clipt1.nnTA() - clipt0.nnTA();
    Matrix3d deltaNNT_expected = sDelta(DNNT, delta);

    cerr << "deltaNNT =\n" << deltaNNT << "\n";
    cerr << "expected\n" << deltaNNT_expected << "\n";
    cerr << "\tareal\n" << sDelta(ADnnT, delta) << "\n";
    cerr << "\tbdy\n" << sDelta(NNTDh, delta) << "\n";

    Matrix3d DNNT_meas = deltaNNT/norm(delta);
    Matrix3d DNNT_exp = deltaNNT_expected/norm(delta);

    cerr << "DNNT_meas =\n" << DNNT_meas << "\n";
    cerr << "DNNT_exp =\n" << DNNT_exp << "\n";

    if (vectorNorm(deltaNNT_expected) > 1e-5)
    {
        CHECK_MATRIX_CLOSE(DNNT_meas, DNNT_exp);
    }

    BOOST_CHECK(true);
}

BOOST_AUTO_TEST_CASE( SimpleBugChaser )
{
    cerr << "SKIPPING\n"; return;
//    cerr << "\n\n===== SimpleBugChaser\n\n";
    // Normal along Y.  Fails.
    Vector3d cv0(0,0.8,0);
    Vector3d cv1(1.0,0,0);
    Vector3d cv4(0,0.8,1);

    // Normal along X. Succeeds.
//    Vector3d cv0(0,1.0,0);
//    Vector3d cv1(0.8,0,0);
//    Vector3d cv4(0,1.0,1);

    Vector3d delta(1e-2, 0, 0);

    vector<ControlVertex> cvs0, cvs1;
    cvs0.push_back(ControlVertex(cv0, 0));
    cvs0.push_back(ControlVertex(cv1, 1));
    cvs0.push_back(ControlVertex(cv4, 4));

    ClippedTriangle clipt0 = sBugsy(cvs0);
//    cerr << "Clipped triangle:\n"
//        << "quickPatch(" << clipt0 << ", 'g');\n";
//    cerr << "NNTA =\n" << clipt0.nnTA() << "\n";

    cvs1 = cvs0;
    for (int nn = 0; nn < 3; nn++)
    {
        cvs1[nn].point(cvs1[nn].point() + delta);
    }
    ClippedTriangle clipt1 = sBugsy(cvs1);
//    cerr << "New one:\n"
//        << "quickPatch(" << clipt1 << ", 'g');\n";
//    cerr << "NNTA =\n" << clipt1.nnTA() << "\n";

    map<unsigned int, Matrix3<Vector3d> > DNNT, NNTDh, ADnnT;
    // = clipt0.DNNT(X);
    DNNT = clipt0.DNNT(clipt0.triangle()->upAxis());
    clipt0.intNNTDh(NNTDh);
    clipt0.intDnnT(ADnnT);

//    cerr << "DNNT = \n";
//    printJacobian(DNNT);
//    cerr << "\nNNTDh =\n";
//    printJacobian(NNTDh);
//    cerr << "\nADnnT =\n";
//    printJacobian(ADnnT);

    Matrix3d nnTA0 = clipt0.nnTA();
    Matrix3d nnTA1 = clipt1.nnTA();

    Matrix3d deltaNNT = clipt1.nnTA() - clipt0.nnTA();
    Matrix3d deltaNNT_expected = sDelta(DNNT, delta);

//    cerr << "deltaNNT =\n" << deltaNNT << "\n";
//    cerr << "expected\n" << deltaNNT_expected << "\n";
//    cerr << "\tareal\n" << sDelta(ADnnT, delta) << "\n";
//    cerr << "\tbdy\n" << sDelta(NNTDh, delta) << "\n";

    BOOST_CHECK(true);
}

/*
BOOST_AUTO_TEST_CASE( IntNNTDh_ClipRight_Flipped )
{
    cerr << "\n==== IntNNTDh_ClipRight_Flipped\n\n";
    Vector3d o(0,0,0), x(1,0,0), y(0,1,0), z(0,0,1);
    Triangle3d tri0(o+z/2, x+z/2, y+z/2);
    Triangle xyTriangle0(sOneFlippedTri(tri0[0], tri0[1], tri0[2]));

    ClippedTriangle ctri0 = ClippedTriangle(xyTriangle0).partBelow(X, 0.5);
    cerr << "Tri: \n" << ctri0 << "\n";

    Matrix3d nnT = outerProduct(ctri0.arealNormal(), unit(ctri0.arealNormal()));

    map<unsigned int, Matrix3<Vector3d> > jacobian;
    ctri0.intNNTDh(jacobian);

    cerr << "Orientation:\n" << nnT << "\n";
    cerr << "Jacobian:\n";
    printJacobian(jacobian);

    // The integral contributions are the same as in the unflipped case, but
    // should have p1 and p2 reversed.

    // Edge 0:
    // dA/dp0 = [0 -3/8 0]
    // dA/dp1 = 0
    // dA/dp2 = [0 -1/8 0]

    // Edge 1:
    // dA/dp0 = 0
    // dA/dp1 = [3/8 3/8 0]
    // dA/dp2 = [1/8 1/8 0]

    // Edge 2:
    // dA/dp0 = [-1/2 0 0]
    // dA/dp1 = [-1/2 0 0]
    // dA/dp2 = 0

    // Total:
    // dA/dp0 = [-1/2 -3/8 0]
    // dA/dp1 = [-1/8 3/8 0]
    // dA/dp2 = [1/8 0 0]

    // The sensitivity in question is just the orientation factor times the
    // areal sensitivity.  When the normal vector is unit(z), the answer is:
    CHECK_CLOSE(jacobian[0](Z,Z), Vector3d(-0.5, -0.375, 0));
    CHECK_CLOSE(jacobian[1](Z,Z), Vector3d(-0.125, 0.375, 0));
    CHECK_CLOSE(jacobian[2](Z,Z), Vector3d(0.125, 0, 0));

    // make sure all the parts that should be zero ARE zero.
    for (int vert = 0; vert < 3; vert++)
    for (int ii = 0; ii < 3; ii++)
    for (int jj = 0; jj < 3; jj++)
    if (ii != Z || jj != Z)
    {
        BOOST_CHECK_SMALL(norm(jacobian[vert](ii,jj)), 1e-6);
    }
}
*/

static Matrix3d sDelta(const map<unsigned int, Matrix3<Vector3d> > & jacobian,
    Vector3d delta)
{
    Matrix3d out;

    map<unsigned int, Matrix3<Vector3d> >::const_iterator itr;
    for (itr = jacobian.begin(); itr != jacobian.end(); itr++)
    {
        for (int mm = 0; mm < 9; mm++)
            out[mm] += dot(itr->second[mm], delta);
    }
    return out;
}


// Dlib lenet implementation, from dlib/examples
#include <dlib/dnn.h>
#include <iostream>
#include <dlib/data_io.h>

using namespace std;
using namespace dlib;

int main(int argc, char** argv) try
{
    // Parse path to mnist
    if (argc != 2)
    {
        cout << "This example needs the MNIST dataset to run!" << endl;
        cout << "You can get MNIST from http://yann.lecun.com/exdb/mnist/" << endl;
        cout << "Download the 4 files that comprise the dataset, decompress them, and" << endl;
        cout << "put them in a folder.  Then give that folder as input to this program." << endl;
        return 1;
    }

    // containers for ubyte mnist
    std::vector<matrix<unsigned char>> training_images;
    std::vector<unsigned long>         training_labels;
    std::vector<matrix<unsigned char>> testing_images;
    std::vector<unsigned long>         testing_labels;
    // load the data using the provided function from dlib
    load_mnist_dataset(argv[1], training_images, training_labels, testing_images, testing_labels);

    // net type definition
    using net_type = loss_multiclass_log<
                                fc<10,
                                relu<fc<84,
                                relu<fc<120,
                                max_pool<2,2,2,2,relu<con<16,5,5,1,1,
                                max_pool<2,2,2,2,relu<con<6,5,5,1,1,
                                input<matrix<unsigned char>>
                                >>>>>>>>>>>>;

    // use the net_type we just created to initiate an instance of net
    net_type net;
    // trainer with sgd, parameter setup
    dnn_trainer<net_type> trainer(net);
    trainer.set_learning_rate(0.01);
    trainer.set_min_learning_rate(0.00001);
    trainer.set_mini_batch_size(128);
    trainer.set_max_num_epochs(2);
    trainer.be_verbose();
    // this line does state saving based on seconds so we dont lose the results
    trainer.set_synchronization_file("mnist_sync", std::chrono::seconds(20));
    // training start
    trainer.train(training_images, training_labels);

    // clean the net from any data and save it
    net.clean();
    serialize("mnist_network.dat") << net;
    // Now if we later wanted to recall the network from disk we can simply say:
    // deserialize("mnist_network.dat") >> net;

    // predict using the model and see how many of the images were classified correctly
    std::vector<unsigned long> predicted_labels = net(training_images);
    int num_right = 0;
    int num_wrong = 0;
    // And then let's see if it classified them correctly.
    for (size_t i = 0; i < training_images.size(); ++i)
    {
        if (predicted_labels[i] == training_labels[i])
            ++num_right;
        else
            ++num_wrong;

    }
    // calc accuracy
    cout << "training num_right: " << num_right << endl;
    cout << "training num_wrong: " << num_wrong << endl;
    cout << "training accuracy:  " << num_right / static_cast<double>(num_right + num_wrong) << endl;

    // test metrics
    predicted_labels = net(testing_images);
    num_right = 0;
    num_wrong = 0;
    for (size_t i = 0; i < testing_images.size(); ++i)
    {
        if (predicted_labels[i] == testing_labels[i])
            ++num_right;
        else
            ++num_wrong;

    }
    cout << "testing num_right: " << num_right << endl;
    cout << "testing num_wrong: " << num_wrong << endl;
    cout << "testing accuracy:  " << num_right / static_cast<double>(num_right + num_wrong) << endl;

    // save as xml, this can later be used to convert this to caffe
    net_to_xml(net, "net.xml");
}
// print the error exception if any happens
catch(std::exception& e)
{
    cout << e.what() << endl;
}


// Copyright Louis Dionne 2016
// Distributed under the Boost Software License, Version 1.0.

#include <boost/hana/tuple.hpp>

#include <cassert>
#include <string>

#include "hana.hpp"
namespace hana = boost::hana;

int main() {
    hana::tuple<int, char, float> ts{1, '2', 3.3f};
    hana::tuple<std::string, double> us{"foobar", 4.4};
    auto cat = ::tuple_cat(ts, us);

    assert(hana::at_c<0>(cat) == 1);
    assert(hana::at_c<1>(cat) == '2');
    assert(hana::at_c<2>(cat) == 3.3f);
    assert(hana::at_c<3>(cat) == "foobar");
    assert(hana::at_c<4>(cat) == 4.4);
}


#ifndef PARMCB_DETAIL_SPANNING_FOREST_HPP_
#define PARMCB_DETAIL_SPANNING_FOREST_HPP_

//    Copyright (C) Dimitrios Michail 2019 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#include <map>
#include <queue>
#include <set>

#include <boost/graph/adjacency_list.hpp>

namespace parmcb {

    namespace detail {

        template<class Graph, class OutputIterator>
        std::size_t spanning_forest(const Graph &g, OutputIterator spanning_forest_edges) {

            if (boost::num_vertices(g) == 0)
                return 0;

            typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
            typedef typename boost::graph_traits<Graph>::vertex_iterator VertexIt;
            typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;

            BOOST_CONCEPT_ASSERT(( boost::VertexListGraphConcept<Graph> ));
            BOOST_CONCEPT_ASSERT(( boost::OutputIteratorConcept<OutputIterator, Edge> ));

            std::queue<Vertex> queue;
            std::unordered_set<Vertex> unreached;
            VertexIt ui, uiend;
            for (boost::tie(ui, uiend) = boost::vertices(g); ui != uiend; ++ui) {
                unreached.insert(*ui);
            }

            std::size_t c = 0;
            while (!unreached.empty()) {
                auto vi = unreached.begin();
                auto v = *vi;
                unreached.erase(vi);
                queue.push(v);

                while (!queue.empty()) {
                    auto u = queue.front();
                    queue.pop();

                    auto eiRange = boost::out_edges(u, g);
                    for (auto ei = eiRange.first; ei != eiRange.second; ++ei) {
                        auto e = *ei;
                        auto w = boost::target(e, g);
                        if (w == u) {
                            // ignore self-loop
                            continue;
                        }

                        auto wit = unreached.find(w);
                        if (wit == unreached.end()) {
                            continue;
                        }
                        unreached.erase(wit);
                        *spanning_forest_edges++ = e;
                        queue.push(w);
                    }
                }
                c++;
            }

            return c;
        }

    } // detail

} // parmcb

#endif


// Copyright © 2016 Venture Media Labs.
//
// This file is part of mxml. The full mxml copyright notice, including
// terms governing use, modification, and redistribution, is contained in the
// file LICENSE at the root of the source code distribution tree.

#include <mxml/geometry/Geometry.h>
#include <mxml/StreamOperators.h>
#include <boost/test/unit_test.hpp>

using namespace mxml;

BOOST_AUTO_TEST_CASE(defaultValues) {
    Geometry geometry;
    BOOST_CHECK_EQUAL(geometry.location(), Point(0.0, 0.0));
    BOOST_CHECK_EQUAL(geometry.size(), Size(0.0, 0.0));
    BOOST_CHECK_EQUAL(geometry.origin(), Point(0.0, 0.0));
    BOOST_CHECK_EQUAL(geometry.center(), Point(0.0, 0.0));
    BOOST_CHECK_EQUAL(geometry.frame(), Rect(Point(0.0, 0.0), Size(0.0, 0.0)));
    BOOST_CHECK_EQUAL(geometry.contentOffset(), Point(0.0, 0.0));
}

// The following test cases use this geometry placement
//
//   0 1 2 3 4 5 6 7 8 9 0
// 0┌────────────────────
// 1│  ┌──────────────┐
// 2│  │              │
// 3│  │              │
// 4│  │              │
// 5│  │              │
// 6│  │              │
// 7│  │   *          │
// 8│  └──────────────┘
// 9│
//

void configureGeometry(Geometry& geometry) {
    geometry.setHorizontalAnchorPointValues(0.25, 0);
    geometry.setVerticalAnchorPointValues(0.75, 0);
    geometry.setLocation({30, 70});
    geometry.setSize({80, 80});
}

BOOST_AUTO_TEST_CASE(placement) {
    Geometry geometry;
    configureGeometry(geometry);

    BOOST_CHECK_EQUAL(geometry.origin(), Point(10, 10));
    BOOST_CHECK_EQUAL(geometry.center(), Point(50, 50));
    BOOST_CHECK_EQUAL(geometry.frame(), Rect(Point(10, 10), Size(80, 80)));
}

BOOST_AUTO_TEST_CASE(convertToParent) {
    Geometry parent;
    parent.setSize({200, 200});

    Geometry child;
    configureGeometry(child);

    BOOST_CHECK_EQUAL(child.convertToParent({0, 0}), child.origin());
    BOOST_CHECK_EQUAL(child.convertToParent({80, 80}), child.frame().max());
}

BOOST_AUTO_TEST_CASE(convertFromParent) {
    Geometry parent;
    parent.setSize({200, 200});

    Geometry child;
    configureGeometry(child);

    BOOST_CHECK_EQUAL(child.convertFromParent(child.origin()), Point(0, 0));
    BOOST_CHECK_EQUAL(child.convertFromParent(child.frame().max()), Point(80, 80));
}

BOOST_AUTO_TEST_CASE(convertToParentWithContentOffset) {
    Geometry parent;
    parent.setSize({200, 200});

    Geometry child;
    configureGeometry(child);
    child.setContentOffset({-10, 20});

    BOOST_CHECK_EQUAL(child.convertToParent({0, 0}), Point(20, -10));
}

BOOST_AUTO_TEST_CASE(convertFromParentWithContentOffset) {
    Geometry parent;
    parent.setSize({200, 200});

    Geometry child;
    configureGeometry(child);
    child.setContentOffset({-10, 20});

    BOOST_CHECK_EQUAL(child.convertFromParent({20, -10}), Point(0, 0));
}

BOOST_AUTO_TEST_CASE(convertPointToGeometryNull) {
    Geometry root;
    root.setSize({200, 200});

    Geometry* child;
    {
        std::unique_ptr<Geometry> geom(new Geometry);
        child = geom.get();
        configureGeometry(*child);
        geom->setContentOffset({-10, 20});
        root.addGeometry(std::move(geom));
    }

    Point result = child->convertToGeometry({0, 0}, nullptr);
    BOOST_CHECK_EQUAL(result, child->convertToParent({0, 0}));
}

BOOST_AUTO_TEST_CASE(convertPointToGeometrySelf) {
    Geometry root;
    root.setSize({200, 200});

    Geometry* child;
    {
        std::unique_ptr<Geometry> geom(new Geometry);
        child = geom.get();
        configureGeometry(*child);
        geom->setContentOffset({-10, 20});
        root.addGeometry(std::move(geom));
    }

    Point result = child->convertToGeometry({0, 0}, child);
    BOOST_CHECK_EQUAL(result, Point(0, 0));
}

BOOST_AUTO_TEST_CASE(convertPointToGeometryParent) {
    Geometry root;
    root.setSize({200, 200});

    Geometry* child;
    {
        std::unique_ptr<Geometry> geom(new Geometry);
        child = geom.get();
        configureGeometry(*child);
        geom->setContentOffset({-10, 20});
        root.addGeometry(std::move(geom));
    }

    Point result = child->convertToGeometry({0, 0}, &root);
    BOOST_CHECK_EQUAL(result, child->convertToParent({0, 0}));
}

BOOST_AUTO_TEST_CASE(convertPointToGeometrySibling) {
    Geometry root;
    root.setSize({200, 200});

    Geometry* child1;
    {
        std::unique_ptr<Geometry> geom(new Geometry);
        child1 = geom.get();
        configureGeometry(*child1);
        root.addGeometry(std::move(geom));
    }

    Geometry* child2;
    {
        std::unique_ptr<Geometry> geom(new Geometry);
        child2 = geom.get();
        configureGeometry(*child2);
        geom->setLocation({120, 70});
        root.addGeometry(std::move(geom));
    }

    Point result = child1->convertToGeometry({0, 0}, child2);
    BOOST_CHECK_EQUAL(result, Point(-90, 0));
}


/*
 * Space_Run.cpp
 *
 */
#include <complex>
#include "space.h"
#include <boost/lexical_cast.hpp>

void CDelphiSpace::run()
{
    const char* infoString = " Info> ";

    delphi_integer i, j, k, ix, iy, iz, ic, ico, natom2;
    SGrid <delphi_integer> epstmp;
    SGrid <delphi_real> xl, xr;

    sgrid_temp_real.nX = 0.; sgrid_temp_real.nY = 0.; sgrid_temp_real.nZ = 0.;
    sgrid_temp_int.nX  = 0;   sgrid_temp_int.nY = 0;   sgrid_temp_int.nZ = 0;

    // initialize bDebMap
    get_pt3d <char>(idebmap, iGrid.nX + 1, iGrid.nY + 1, iGrid.nZ + 1);
    for (ix = 1; ix <= iGrid.nX; ix++)
        for (iy = 1; iy <= iGrid.nY; iy++)
            for (iz = 1; iz <= iGrid.nZ; iz++)
                idebmap[ix][iy][iz] = true;

    // (ARGO) initialize zetaSurfMap
    get_pt3d <char>(zetaSurfMap, iGrid.nX + 1, iGrid.nY + 1, iGrid.nZ + 1);
    for (ix = 1; ix <= iGrid.nX; ix++)
        for (iy = 1; iy <= iGrid.nY; iy++)
            for (iz = 1; iz <= iGrid.nZ; iz++)
                zetaSurfMap[ix][iy][iz] = true;

    if (iGaussian == 0)
        get_pt3d <SGrid <delphi_integer> >(iepsmp, global_iGrid.nX + 1, global_iGrid.nY + 1, global_iGrid.nZ + 1);
    else if (iGaussian == 1)
    {
        get_pt3d <SGrid <delphi_real> >(gepsmp, iGrid.nX + 1, iGrid.nY + 1, iGrid.nZ + 1);
        get_pt3d <SGrid <delphi_real> >(gepsmp2, iGrid.nX + 1, iGrid.nY + 1, iGrid.nZ + 1);
        get_pt3d <delphi_real>(gDensityMapOnGridPoint, iGrid.nX + 1, iGrid.nY + 1, iGrid.nZ + 1);
    }

    if (iGaussian == 1 && inhomo == 0 && logs) //for 2nd Gaussian run
        for (i = 1; i <= iGrid.nX; i++)
            for (j = 1; j <= iGrid.nY; j++)
                for (k = 1; k <= iGrid.nZ; k++)
                    gepsmp[i][j][k] = fGepsMap_v[(i - 1)*iGrid.nY*iGrid.nZ + (j - 1)*iGrid.nY + k - 1];

    xn1 = &xn1_v[0]-1;
    xn2 = &xn2_v[0]-1;
    fRadPrb = &fRadPrb_v[0]-1;

    // for focusing
    if (ibctyp == 3)
    {
        SGrid <delphi_real> halfl;
        SGrid <delphi_real> edge_low, edge_high, xyz_temp;
        vector <CAtomPdb> delphipdb2; //delphipdb2 for focusing

        halfl     = (iGrid - delphi_integer(1)) / fScale / 2.0;
        edge_low  = acenter - halfl - 3.5;
        edge_high = acenter + halfl + 3.5;
        natom2    = 0;

		cout << "space_run> half1 = " << halfl << endl;
		cout << "space_run> acenter =  " << acenter.nX <<" " << acenter.nY << " " << acenter.nZ <<  endl;

        for (i = 0; i < iNatom; i++)
        {
            xyz_temp = delphipdb[i].getPose();
            if ( !(optORGT(xyz_temp, edge_high) || optORLT(xyz_temp, edge_low) ) )
            {
                delphipdb2.push_back(delphipdb[i]);
                natom2++;
            }
        }

        delphipdb.resize(natom2);

        for (i = 0; i < natom2; i++)
            delphipdb[i] = delphipdb2[i];

        iNatom = delphipdb.size();

        vector <CAtomPdb>().swap(delphipdb2);

        /*
         * write updated values back to global data container
         */
        pdc->getKey_Ref<delphi_integer>("natom")         = iNatom;
        pdc->getKey_Ref<vector <CAtomPdb> >("delphipdb") = delphipdb;
        myStart = {0,0,0};
        epsdim  = iNatom + iNObject + 2;
    }
    else
    {
        split();

        SGrid <delphi_real> xyz_temp;
        vector <CAtomPdb> delphipdb2; //temperary atom information storage

        natom2 = 0;

        for (i = 0; i < iNatom; i++)
        {
            xyz_temp = delphipdb[i].getPose();
            if ( !(optORGT(xyz_temp, myEndCoor) || optORLT(xyz_temp, myStartCoor) ) )
            {
                delphipdb2.push_back(delphipdb[i]);
                globalAtomIndex.push_back(i);
                natom2++;
            }
        }

        // update the atoms handled by current process
        delphipdb.swap(delphipdb2);
        iNatom = natom2;

        SExtrema<delphi_real> tmpExtrema = { {0,0,0},{0,0,0} };
        if (iNatom > 0)
            tmpExtrema = { delphipdb[0].getPose(), delphipdb[0].getPose() };

        for (int i = 0; i < iNatom; i++)
        {
            tmpExtrema.nMin = optMin<delphi_real>(tmpExtrema.nMin, delphipdb[i].getPose());
            tmpExtrema.nMax = optMax<delphi_real>(tmpExtrema.nMax, delphipdb[i].getPose());
        }
        sLimObject[0]=tmpExtrema;

    }

    sDelPhiPDB = new delphipdb_struc[iNatom + 1];

    for (int i = 0; i <= iNatom - 1; i++)
    {
        sDelPhiPDB[i + 1].radius = delphipdb[i].getRadius();
        sDelPhiPDB[i + 1].xyz = delphipdb[i].getPose();
        sDelPhiPDB[i + 1].charge = delphipdb[i].getCharge();
        sDelPhiPDB[i + 1].atom_resname = delphipdb[i].getAtResname(); //Argo
    }

    for (int i = 1; i <= iNatom; i++)
    {
        xn1[i] = sDelPhiPDB[i].xyz;
        if (ibctyp != 3)
            xn2[i] = (sDelPhiPDB[i].xyz - global_cOldMid)*fScale + fRMid - optCast<delphi_real,delphi_integer>(myStart);
        else
            xn2[i] = (sDelPhiPDB[i].xyz - global_cOldMid)*fScale + (iGrid+1)/2.0;
    }

    epsmak(); //Lin Li reset

    // Now start crgarr
    if (isolv)
    {
        // increased the starting dimension of crgatn and..
        extracrg = 0;
        if (ndistr > 0) extracrg = iGrid.nX*iGrid.nY*iGrid.nZ;

        /*
         * 2011-05-30 Allocation of the arrays below is moved to the body of crgarr void,
         * arrays are accessible via pointers module. Sizes of arrays are determined before
         * allocation inside the crgarr void
         */
        crgarr(); //Lin Li reset

        xl = myStartCoor;
        xr = myEndCoor;

        if (logs || lognl)
        {
            ico = 0;
            for (ic = 1; ic <= nqass; ic++)
            {
                if (optORLT(chgpos[ic], xl) || optORGT(chgpos[ic], xr))
                {
                    #ifdef VERBOSE
                    if (crgatn[ic] < 0)
                        cout << "!WARNING: distribution " << -crgatn[ic] << "outside the box" << endl;
                    else
                    {
                        if (crgatn[ic] > iNatom)
                            cout << "WARNING:crg " << ic << "object " << crgatn[ic] - iNatom << "outside the box " << chgpos[ic] << endl;
                        else
                            cout << "//!! WARNING : charge " << delphipdb[crgatn[ic] - 1].getAtInf() << "outside the box" << endl;
                    }
                    #endif

                    ico = 1;
                }
            }

            if (ico > 0 && ibctyp != 3)
            {
                cout << "CHARGES OUTSIDE THE BOX AND NOT DOING FOCUSSING << THEREFORE STOP" << endl;
                exit(0);
            }
        }
    }

    // initialize iepsmp
    bDebMap_v.assign(iGrid.nX*iGrid.nY*iGrid.nZ, true);
    zetaSurfMap_v.assign(iGrid.nX*iGrid.nY*iGrid.nZ, true);
    if (iGaussian == 0)

        iEpsMap_v.assign(iGrid.nX*iGrid.nY*iGrid.nZ, { repsout,repsout ,repsout });
    else if (iGaussian == 1)
    {
        fGepsMap_v.assign(iGrid.nX*iGrid.nY*iGrid.nZ, sgrid_temp_real);
        fGepsMap2_v.assign(iGrid.nX*iGrid.nY*iGrid.nZ, sgrid_temp_real);
    }

    // -------gepsmp file: --------
//    {
//        ofstream densfile;
//        densfile.open ("test_space_gaussian_gepsmp0.txt");
//
//        densfile << fixed << setprecision(7);
//        for(ix=1; ix<=iGrid.nX; ix++)
//            for(iy=1; iy<=iGrid.nY; iy++)
//                for(iz=1; iz<=iGrid.nZ; iz++)
//                    densfile << setw(6) << right << ix << " "
//                             << setw(6) << right << iy << " "
//                             << setw(6) << right << iz << " "
//                             << setw(8) << right << gepsmp[ix][iy][iz].nX << " "
//                             << setw(8) << right << gepsmp[ix][iy][iz].nY << " "
//                             << setw(8) << right << gepsmp[ix][iy][iz].nZ << endl;
//        densfile.close();
//    }

    if (iGaussian == 0)
        for (k = 1; k <= iGrid.nZ; k++)
            for (j = 1; j <= iGrid.nY; j++)
                for (i = 1; i <= iGrid.nX; i++)
                    iEpsMap_v[(k - 1)*iGrid.nY*iGrid.nX + (j - 1)*iGrid.nX + i - 1] = iepsmp[k][j][i];
    else if (iGaussian == 1)
        for (k = 1; k <= iGrid.nZ; k++)
            for (j = 1; j <= iGrid.nY; j++)
                for (i = 1; i <= iGrid.nX; i++)
                {
                    //fGepsMap2_v[(k - 1)*iGrid.nY*iGrid.nX + (j - 1)*iGrid.nX + i - 1] = gepsmp2[i][j][k];
                    //fGepsMap_v[(k - 1)*iGrid.nY*iGrid.nX + (j - 1)*iGrid.nX + i - 1] = gepsmp[i][j][k];
                    fGepsMap2_v[(i-1)*iGrid.nY*iGrid.nZ+(j-1)*iGrid.nZ+k-1]=gepsmp2[i][j][k];
                    fGepsMap_v[(i-1)*iGrid.nY*iGrid.nZ+(j-1)*iGrid.nZ+k-1]=gepsmp[i][j][k];
                }

    // Gaussian Density Map
    if ( !(iGaussian == 1 && inhomo == 0 && logs) )
    {
        fGDensityMap_v.assign(iGrid.nX*iGrid.nY*iGrid.nZ, 0.0);

        if (fIonStrenth > fZero)
        {
            if (iGaussian != 0)
                for (k = 1; k <= iGrid.nZ; k++)
                    for (j = 1; j <= iGrid.nY; j++)
                        for (i = 1; i <= iGrid.nX; i++)
                            fGDensityMap_v[(k - 1)*iGrid.nY*iGrid.nX + (j - 1)*iGrid.nX + i - 1] = gDensityMapOnGridPoint[i][j][k];
            else
                for (k = 1; k <= iGrid.nZ; k++)
                    for (j = 1; j <= iGrid.nY; j++)
                        for (i = 1; i <= iGrid.nX; i++)
                            if (!idebmap[i][j][k]) fGDensityMap_v[(k - 1)*iGrid.nY*iGrid.nX + (j - 1)*iGrid.nX + i - 1] = 1.0;

        }
    }

    for (k = 1; k <= iGrid.nZ; k++)
    {
        for (j = 1; j <= iGrid.nY; j++)
        {
            for (i = 1; i <= iGrid.nX; i++)
            {
                //ARGO commented the ifdef in original. Place ifdef-KJI later
                #ifdef IKJ
                bDebMap_v[(i - 1)*iGrid*iGrid + (j - 1)*iGrid + (k - 1)] = idebmap[k][j][i];
                if (zetaOn == 1) zetaSurfMap_v[(i - 1)*iGrid*iGrid + (j - 1)*iGrid + (k - 1)] = zetaSurfMap[k][j][i]; //ARGO
                #endif // IKJ

                bDebMap_v[(k - 1)*iGrid.nY*iGrid.nX + (j - 1)*iGrid.nX + i - 1] = idebmap[k][j][i];
                if (zetaOn == 1) zetaSurfMap_v[(k - 1)*iGrid.nY*iGrid.nX + (j - 1)*iGrid.nX + i - 1] = zetaSurfMap[i][j][k]; //ARGO
            }
        }
    }

    if (iepsmp != NULL) free_pt3d(iepsmp, global_iGrid.nX + 1, global_iGrid.nY + 1, global_iGrid.nZ + 1);
    if (idebmap != NULL) free_pt3d(idebmap, iGrid.nX + 1, iGrid.nY + 1, iGrid.nZ + 1);
    if (gepsmp != NULL) free_pt3d(gepsmp, iGrid.nX + 1, iGrid.nY + 1, iGrid.nZ + 1);
    if (gepsmp2 != NULL) free_pt3d(gepsmp2, iGrid.nX + 1, iGrid.nY + 1, iGrid.nZ + 1);
    if (gDensityMapOnGridPoint != NULL) free_pt3d(gDensityMapOnGridPoint, iGrid.nX + 1, iGrid.nY + 1, iGrid.nZ + 1);
    if (zetaSurfMap != NULL) free_pt3d(zetaSurfMap, iGrid.nX + 1, iGrid.nY + 1, iGrid.nZ + 1); //ARGO
}


/*
 (c) 2014 Glen Joseph Fernandes
 glenjofe at gmail dot com

 Distributed under the Boost Software
 License, Version 1.0.
 http://boost.org/LICENSE_1_0.txt
*/
#ifndef BOOST_ALIGN_DETAIL_IS_ALIGNMENT_HPP
#define BOOST_ALIGN_DETAIL_IS_ALIGNMENT_HPP

#include <boost/config.hpp>
#include <cstddef>

namespace boost {
    namespace alignment {
        namespace detail {
            BOOST_CONSTEXPR inline bool is_alignment(std::size_t
                value) BOOST_NOEXCEPT
            {
                return (value > 0) && ((value & (value - 1)) == 0);
            }
        }
    }
}

#endif


//==================================================================
// Author       : Pointner Sebastian
// Company      : Johannes Kepler University
// Name         : SMT Macro Placer
// Workfile     : kmeans.hpp
//
// Date         : 19. March 2020
// Compiler     : gcc version 9.2.1 (GCC)
// Copyright    : Johannes Kepler University
// Description  : KMeans Clustering
//==================================================================
#ifndef KMEANS_HPP
#define KMEANS_HPP

#include <object.hpp>
#include <centroid.hpp>
#include <cluster.hpp>

#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/generator_iterator.hpp>

namespace Placer {

/**
 * @class KMeans
 *
 * @brief Perform KMeans Clustering for Macros
 */
class KMeans: public virtual Object
{
public:

    KMeans();

    virtual ~KMeans();

    void set_problem(std::vector<Component*> const & components,
                     size_t const cluster);

    void initialize();

    void run(size_t const iterations);

    void dump(std::ostream & stream = std::cout);

    void plot();

    std::vector<Cluster*> get_cluster();

private:

    void generate_initial_centroid_positions();
    void map_macros_to_cluster();

    std::vector<Component*> m_components;
    std::vector<Cluster*> m_cluster;
    size_t m_number_of_cluster;
    size_t m_iterations;

    size_t m_max_height;
    size_t m_max_width;
};

} /* namespace Placer */

#endif /* KMEANS_HPP */


#ifndef TEST_UNIT_MATH_PRIM_PROB_HMM_UTIL
#define TEST_UNIT_MATH_PRIM_PROB_HMM_UTIL
#include <stan/math/prim/prob/hmm_marginal.hpp>
#include <boost/math/distributions.hpp>
#include <boost/random.hpp>
#include <test/unit/math/test_ad.hpp>
#include <test/unit/util.hpp>
#include <gtest/gtest.h>
#include <limits>
#include <vector>

/**
 * Wrapper around hmm_marginal_density which passes rho and
 * Gamma without the last element of each column. We recover
 * the last element using the fact each column sums to 1.
 * The purpose of this function is to do finite diff benchmarking,
 * without breaking the simplex constraint.
 */
template <typename T_omega, typename T_Gamma, typename T_rho>
inline stan::return_type_t<T_omega, T_Gamma, T_rho> hmm_marginal_test_wrapper(
    const Eigen::Matrix<T_omega, Eigen::Dynamic, Eigen::Dynamic>& log_omegas,
    const Eigen::Matrix<T_Gamma, Eigen::Dynamic, Eigen::Dynamic>&
        Gamma_unconstrained,
    const std::vector<T_rho>& rho_unconstrained) {
  using stan::math::row;
  using stan::math::sum;
  int n_states = log_omegas.rows();

  Eigen::Matrix<T_Gamma, Eigen::Dynamic, Eigen::Dynamic> Gamma(n_states,
                                                               n_states);
  for (int i = 0; i < n_states; i++) {
    Gamma(i, n_states - 1) = 1 - sum(row(Gamma_unconstrained, i + 1));
    for (int j = 0; j < n_states - 1; j++) {
      Gamma(i, j) = Gamma_unconstrained(i, j);
    }
  }

  Eigen::Matrix<T_rho, Eigen::Dynamic, 1> rho(n_states);
  rho(1) = 1 - sum(rho_unconstrained);
  for (int i = 0; i < n_states - 1; i++)
    rho(i) = rho_unconstrained[i];

  return stan::math::hmm_marginal(log_omegas, Gamma, rho);
}

/**
 * In the proposed example, the latent state x determines
 * the observational distribution:
 *  0: normal(mu, sigma)
 *  1: normal(-mu, sigma)
 */
double state_lpdf(double y, double abs_mu, double sigma, int state) {
  int x = state == 0 ? 1 : -1;
  double chi = (y - x * abs_mu) / sigma;
  return -0.5 * chi * chi - 0.5 * std::log(2 * M_PI) - std::log(sigma);
}

class hmm_test : public ::testing::Test {
 protected:
  void SetUp() override {
    n_states_ = 2;
    p1_init_ = 0.65;
    gamma1_ = 0.7;
    gamma2_ = 0.45;
    n_transitions_ = 10;
    abs_mu_ = 1;
    sigma_ = 1;

    Eigen::VectorXd rho(n_states_);
    rho << p1_init_, 1 - p1_init_;
    rho_ = rho;

    Eigen::MatrixXd Gamma(n_states_, n_states_);
    Gamma << gamma1_, 1 - gamma1_, gamma2_, 1 - gamma2_;
    Gamma_ = Gamma;

    Eigen::VectorXd obs_data(n_transitions_ + 1);
    obs_data << -0.3315914, -0.1655340, -0.7984021, 0.2364608, -0.4489722,
        2.1831438, -1.4778675, 0.8717423, -1.0370874, 0.1370296, 1.9786208;
    obs_data_ = obs_data;

    Eigen::MatrixXd log_omegas(n_states_, n_transitions_ + 1);
    for (int n = 0; n < n_transitions_ + 1; n++) {
      log_omegas.col(n)[0] = state_lpdf(obs_data[n], abs_mu_, sigma_, 0);
      log_omegas.col(n)[1] = state_lpdf(obs_data[n], abs_mu_, sigma_, 1);
    }
    log_omegas_ = log_omegas;
    log_omegas_zero_ = log_omegas.block(0, 0, n_states_, 1);

    std::vector<double> rho_unconstrained(n_states_ - 1);
    for (int i = 0; i < rho.size() - 1; i++)
      rho_unconstrained[i] = rho(i);
    rho_unconstrained_ = rho_unconstrained;

    Gamma_unconstrained_ = Gamma.block(0, 0, n_states_, n_states_ - 1);
  }

  int n_states_, n_transitions_;
  double abs_mu_, sigma_, p1_init_, gamma1_, gamma2_;

  Eigen::VectorXd rho_;
  Eigen::MatrixXd Gamma_;
  Eigen::VectorXd obs_data_;
  Eigen::MatrixXd log_omegas_;
  Eigen::MatrixXd log_omegas_zero_;

  // Construct "unconstrained" versions of rho and Gamma, without
  // the final element which can be determnied using the fact
  // the columns sum to 1. This allows us to do finite diff tests,
  // without violating the simplex constraint of rho and Gamma.
  std::vector<double> rho_unconstrained_;
  Eigen::MatrixXd Gamma_unconstrained_;
  stan::test::ad_tolerances tols_;
};
#endif


/*
 * Copyright (c) 2011-2019, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

// Algorithm details and publications: http://www.golems.org/node/1570

#ifndef DART_PLANNING_PATHFOLLOWINGTRAJECTORY_HPP_
#define DART_PLANNING_PATHFOLLOWINGTRAJECTORY_HPP_

#include <Eigen/Core>
#include "dart/planning/Path.hpp"
#include "dart/planning/Trajectory.hpp"

namespace dart {
namespace planning {

class PathFollowingTrajectory : public Trajectory
{
public:
  PathFollowingTrajectory(
      const Path& path,
      const Eigen::VectorXd& maxVelocity,
      const Eigen::VectorXd& maxAcceleration);
  ~PathFollowingTrajectory(void);

  bool isValid() const;
  double getDuration() const;
  Eigen::VectorXd getPosition(double time) const;
  Eigen::VectorXd getVelocity(double time) const;
  double getMaxAccelerationError();

private:
  struct TrajectoryStep
  {
    TrajectoryStep()
    {
    }
    TrajectoryStep(double pathPos, double pathVel)
      : pathPos(pathPos), pathVel(pathVel), time(0.0)
    {
    }
    double pathPos;
    double pathVel;
    double time;
  };

  bool getNextSwitchingPoint(
      double pathPos,
      TrajectoryStep& nextSwitchingPoint,
      double& beforeAcceleration,
      double& afterAcceleration);
  bool getNextAccelerationSwitchingPoint(
      double pathPos,
      TrajectoryStep& nextSwitchingPoint,
      double& beforeAcceleration,
      double& afterAcceleration);
  bool getNextVelocitySwitchingPoint(
      double pathPos,
      TrajectoryStep& nextSwitchingPoint,
      double& beforeAcceleration,
      double& afterAcceleration);
  bool integrateForward(
      std::list<TrajectoryStep>& trajectory, double acceleration);
  void integrateBackward(
      std::list<TrajectoryStep>& trajectory,
      std::list<TrajectoryStep>& startTrajectory,
      double acceleration);
  double getMinMaxPathAcceleration(
      double pathPosition, double pathVelocity, bool max);
  double getMinMaxPhaseSlope(
      double pathPosition, double pathVelocity, bool max);
  double getAccelerationMaxPathVelocity(double pathPos);
  double getVelocityMaxPathVelocity(double pathPos);
  double getAccelerationMaxPathVelocityDeriv(double pathPos);
  double getVelocityMaxPathVelocityDeriv(double pathPos);

  TrajectoryStep getIntersection(
      const std::list<TrajectoryStep>& trajectory,
      std::list<TrajectoryStep>::iterator& it,
      const TrajectoryStep& linePoint1,
      const TrajectoryStep& linePoint2);
  inline double getSlope(
      const TrajectoryStep& point1, const TrajectoryStep& point2);
  inline double getSlope(std::list<TrajectoryStep>::const_iterator lineEnd);

  std::list<TrajectoryStep>::const_iterator getTrajectorySegment(
      double time) const;

  Path path;
  Eigen::VectorXd maxVelocity;
  Eigen::VectorXd maxAcceleration;
  unsigned int n;
  bool valid;
  std::list<TrajectoryStep> trajectory;

  static const double eps;
  static const double timeStep;

  mutable double cachedTime;
  mutable std::list<TrajectoryStep>::const_iterator cachedTrajectorySegment;
};

} // namespace planning
} // namespace dart

#endif // DART_PLANNING_PATHFOLLOWINGTRAJECTORY_HPP_


#include "StaticSound.h"

#include <cassert>
#include <boost/scoped_ptr.hpp>

#include "../../include/gameaudio/IFileReader.h"
#include "../../include/gameaudio/Error.h"

#include "WavDecoder.h"
#include "OggVorbisDecoder.h"

using namespace gameaudio;

StaticSound::StaticSound(boost::shared_ptr<IFileReader> reader, encoding_type encoding) {
	assert(reader != 0);

	alGenBuffers(1, &_alBuffer);

	boost::scoped_ptr<IDecoder> decoder(createDecoder(reader, encoding));

	ALenum format = decoder->getFormat();
	if (format == -1) throw Error("Format which OpenAL does not support");

	uint64 size = decoder->getSizeByBytes();
	if (size > 0xFFFFFFFF) throw Error("Too large file for non-streaming sound");
	char* data = new char[size];
	unsigned read_size = decoder->read(data, 0, size);
	alBufferData(_alBuffer, format, data, read_size, decoder->getFrequency());
	delete[] data;

	alSourcei(_alSource, AL_BUFFER, _alBuffer);

	_frequency = decoder->getFrequency();
	_size = read_size / (decoder->getBitNum() / 8 * decoder->getChannelsNum());
}

uint64 StaticSound::getLengthBySamples() const {
	return _size;
}

float StaticSound::getLengthBySecs() const {
	return (float)((double)_size / (double)_frequency);
}

void StaticSound::update() {
	if (getPlayPositionBySamples() >= _size) {
		setPlayPositionBySamples(0);
	}
}

float StaticSound::getPlayPositionBySecs() const {
	return getPlayPositionBySamples() / (float)_frequency;
}

void StaticSound::setPlayPositionBySecs(float v) {
	setPlayPositionBySamples(v * _frequency);
}

unsigned StaticSound::getFrequency() const {
	return _frequency;
}


/**
 * @file astro_test.cpp
 *
 * @author Samuel Farrens
 *
 * Unit tests for the methods of the Astro class.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE MasterTestSuite
#include <boost/test/unit_test.hpp>
#include <boost/test/parameterized_test.hpp>
#include "../../src/astro.hpp"
#include "../../src/exceptions.hpp"

using namespace boost::unit_test;
Astro astro;

BOOST_AUTO_TEST_SUITE (Master_test_suite)

/**
 * Unit test for Astro::find_bin.
 */
BOOST_AUTO_TEST_CASE( find_bin )
{
  BOOST_CHECK( astro.find_bin(1.0, 1.0, 1.0) == 0 );
  BOOST_CHECK( astro.find_bin(2.0, 1.0, 1.0) == 1.0 );
  BOOST_CHECK_THROW( astro.find_bin(1.0, 2.0, 1.0) == 0, BadArgumentException );
}

/**
 * Unit test for Astro::num_bins.
 */
BOOST_AUTO_TEST_CASE( num_bins )
{
  BOOST_CHECK( astro.num_bins(1.0, 2.0, 1.0) == 1.0 );
  BOOST_CHECK_THROW( astro.num_bins(2.0, 1.0, 1.0) == 0, BadArgumentException );
}

/**
 * Unit test for Astro::within.
 */
BOOST_AUTO_TEST_CASE( within )
{
  BOOST_CHECK( astro.within(1.5, 1.0, 2.0) == 1.0 );
  BOOST_CHECK( astro.within(0.5, 1.0, 2.0) == 0.0 );
  BOOST_CHECK_THROW( astro.within(1.0, 2.0, 1.0) == 0, BadArgumentException );
}

/**
 * Unit test for Astro::angsep.
 */
BOOST_AUTO_TEST_CASE( angsep )
{
  BOOST_CHECK( astro.angsep(0.0, -90.0, 0.0, -90.0) == 0.0 );
  BOOST_CHECK( astro.angsep(360.0, 90.0, 360.0, 90.0) == 0.0 );
  BOOST_CHECK_THROW( astro.angsep( -10.0 , 60.0, 20.0, 60.0) == 0, BadArgumentException );
  BOOST_CHECK_THROW( astro.angsep( 20.0 , 60.0, 400.0, 60.0) == 0, BadArgumentException );
  BOOST_CHECK_THROW( astro.angsep( 20.0 , 100.0, 20.0, 60.0) == 0, BadArgumentException );
  BOOST_CHECK_THROW( astro.angsep( 20.0 , 60.0, 20.0, -100.0) == 0, BadArgumentException );
}

/**
 * Unit test for Astro::mean.
 */
BOOST_AUTO_TEST_CASE( mean )
{
  std::vector<double> test;
  BOOST_CHECK_THROW( astro.mean(test) == 0, BadArgumentException );
  for (int i = 0; i < 10; i++) test.push_back(i);
  BOOST_CHECK( std::abs(astro.mean(test) - 4.5)
	       <= std::numeric_limits<double>::epsilon() );
}

/**
 * Unit test for Astro::median.
 */
BOOST_AUTO_TEST_CASE( median )
{
  std::vector<double> test;
  BOOST_CHECK_THROW( astro.median(test) == 0, BadArgumentException );
  for (int i = 0; i < 10; i++) test.push_back(i);
  BOOST_CHECK( std::abs(astro.median(test) - 4.5)
	       <= std::numeric_limits<double>::epsilon() );
}

/**
 * Unit test for Astro::variance.
 */
BOOST_AUTO_TEST_CASE( variance )
{
  std::vector<double> test;
  BOOST_CHECK_THROW( astro.variance(test) == 0, BadArgumentException );
  for (int i = 0; i < 10; i++) test.push_back(i);
  BOOST_CHECK( std::abs(astro.variance(test) - 8.25)
	       <= std::numeric_limits<double>::epsilon() );

}

/**
 * Unit test for Astro::stdev.
 */
BOOST_AUTO_TEST_CASE( stdev )
{
  std::vector<double> test;
  BOOST_CHECK_THROW( astro.stdev(test) == 0, BadArgumentException );
  for (int i = 0; i < 10; i++) test.push_back(1.0);
  BOOST_CHECK( astro.stdev(test) == 0.0 );
}

/**
 * Unit test for Astro::stderr_mean.
 */
BOOST_AUTO_TEST_CASE( stderr_mean )
{
  std::vector<double> test;
  BOOST_CHECK_THROW( astro.stderr_mean(test) == 0, BadArgumentException );
  for (int i = 0; i < 10; i++) test.push_back(1.0);
  BOOST_CHECK( astro.stderr_mean(test) == 0.0 );
}

/**
 * Unit test for Astro::stderr_median.
 */
BOOST_AUTO_TEST_CASE( stderr_median )
{
  std::vector<double> test;
  BOOST_CHECK_THROW( astro.stderr_median(test) == 0, BadArgumentException );
  for (int i = 0; i < 10; i++) test.push_back(1.0);
  BOOST_CHECK( astro.variance(test) == 0.0 );
}

/**
 * Unit test for Astro::min.
 */
BOOST_AUTO_TEST_CASE( min )
{
  std::vector<double> test;
  BOOST_CHECK_THROW( astro.min(test) == 0, BadArgumentException );
  for (int i = 0; i < 10; i++) test.push_back(i);
  BOOST_CHECK( astro.min(test) == 0 );
}

/**
 * Unit test for Astro::max.
 */
BOOST_AUTO_TEST_CASE( max )
{
  std::vector<double> test;
  BOOST_CHECK_THROW( astro.max(test) == 0, BadArgumentException );
  for (int i = 0; i < 10; i++) test.push_back(i);
  BOOST_CHECK( astro.max(test) == 9 );
}

BOOST_AUTO_TEST_SUITE_END ()


// External libraries
#include <stdio.h>
#include <boost/property_tree/ptree.hpp>

// Local libraries
#include <fmath/constants.h>
#include <fmath/RungeKutta.h>
#include <fparameters/Dimension.h>
#include <fparameters/parameters.h>
#include <fparameters/SpaceIterator.h>

// Local headers
#include "comptonScattMatrix.h"
#include "modelParameters.h"
#include "globalVariables.h"
#include "adafFunctions.h"
#include "write.h"
#include "messages.h"

extern "C" {
	#include <nrMath/random.h>
}
#define RANDOM_GENERATOR gsl_rng_r250 /* R250 random number generator from GNU Scientific Library. */

#define new_max(x,y) ((x) >= (y)) ? (x) : (y)
#define new_min(x,y) ((x) <= (y)) ? (x) : (y)
#define new_abs(x,y) ((x) >= (y)) ?(x-y) : (y-x)

void comptonScattMatrixWrite()
{
	matrixWrite("scattAA.dat", scattAA, nR, nR);
	matrixWrite("scattDA.dat", scattDA, nRcd, nR);
	matrixWrite("reachAD.dat", reachAD, nR, nRcd);
	matrixWrite("reachDA.dat", reachDA, nRcd, nR);
	matrixWrite("reachAA.dat", reachAA, nR, nR);
	vectorWrite("escapeAi.dat", escapeAi, nR);
	vectorWrite("escapeDi.dat", escapeDi, nRcd);
}

void comptonScattMatrix(State& st)
{
	show_message(msgStart,Module_comptonScattMatrix);

	size_t nPhot = GlobalConfig.get<size_t>("scatt.nRandomPhot");
	size_t nTheta = GlobalConfig.get<size_t>("scatt.nTheta");

	double captureRadius = 0.5*sqrt(27.0)*schwRadius;

	Vector rCellsBoundaries(nR+1,0.0), rCellsBoundariesCD(nRcd+1,0.0);
	rCellsBoundaries[0] = st.denf_e.ps[DIM_R][0]/sqrt(paso_r);
	rCellsBoundariesCD[0] = rTr;
    for(size_t iR=1;iR<=nR;iR++) rCellsBoundaries[iR]=rCellsBoundaries[iR-1]*paso_r;
	for(size_t iRcd=1;iRcd<=nRcd;iRcd++) rCellsBoundariesCD[iRcd]=rCellsBoundariesCD[iRcd-1]*paso_rCD;

	double rBound = max(rCellsBoundaries[nR],rCellsBoundariesCD[nRcd]);
	matrixInit(scattAA,nR,nR,0.0);
	matrixInit(scattDA,nRcd,nR,0.0);
	matrixInit(reachAD,nR,nRcd,0.0);
	matrixInit(reachAA,nR,nR,0.0);
	matrixInit(reachDA,nRcd,nR,0.0);
    escapeAi.resize(nR,0.0);
	escapeDi.resize(nRcd,0.0);

    InitialiseRandom(RANDOM_GENERATOR);
	//ADAF SCATTERING MATRIX

	double pasoprim = min(pow(rCellsBoundaries[1]/rCellsBoundaries[0],1.0/10.0),
						1.0+height_fun(st.denf_i.ps[DIM_R][0])/st.denf_i.ps[DIM_R][0]);

	#pragma omp parallel for
	for (int iR=0;iR<nR;iR++) {
		double r0 = st.denf_e.ps[DIM_R][iR];
		double thetaMin;
		if (height_method == 0)
			thetaMin = st.thetaH.get({0,iR,0});
		else
			thetaMin = atan(r0/height_fun(r0));
		double dyaux=(1.0-sin(thetaMin))/nTheta;
        for(size_t kTh=1;kTh<=nTheta;kTh++) {
            double yaux=sin(thetaMin)+kTh*dyaux;  // Theta distributed uniformly in sin(theta).
            double theta0=asin(yaux);
			double y0,z0;
			if (height_method == 0) {
				y0=r0*sin(theta0);
				z0=r0*cos(theta0);
			} else {
				y0 = r0;
				z0 = r0/tan(theta0);
			}
			for(size_t jPh=1;jPh<=nPhot;jPh++) {
				double random_number = gsl_rng_uniform(RandomNumberGenerator);
				double phiprim = 2.0*pi*random_number;
				random_number = gsl_rng_uniform(RandomNumberGenerator);
				double thetaprim = acos(1.0-2.0*random_number);   // Photon directions
																  // distributed
																  // isotropically.
				double pescap = 1.0;
				double z1ant = z0;
				double drprim=r0*(pasoprim-1.0);       // Step.
				double rprim=drprim;
				vector<size_t> countAA(nR,0);
				double r1 = r0;
				double z1 = z0;
				double r1aux = r1;
                do {
					double xprim=rprim*sin(thetaprim)*cos(phiprim);
					double yprim=rprim*sin(thetaprim)*sin(phiprim);
					double zprim=rprim*cos(thetaprim);
					double x1=xprim;
					double y1=y0+yprim;
					z1=z0+zprim;
					r1=sqrt(x1*x1+y1*y1+z1*z1);
					r1aux = (height_method == 0) ? r1 : sqrt(x1*x1+y1*y1);
					double theta1=atan(sqrt(x1*x1+y1*y1)/abs(z1));
					double ne = electronDensityTheta(r1,theta1);
					double exptau = exp(-ne*thomson*drprim);
                    double psc = 1.0-exptau;    // Probability of scattering.
					if (r1aux > rTr && z1*z1ant < 0.0) {
						for(size_t jRcd=0;jRcd<=nRcd;jRcd++) {
							if(r1aux > rCellsBoundariesCD[jRcd] && r1aux < rCellsBoundariesCD[jRcd+1]) {
								reachAD[iR][jRcd] += pescap; 		// Add the probability of
																	// absorption by the ring
																	// jRcd of the thin disk.
								goto LOOP;
							}
						}
                    }
					z1ant = z1;
					for(size_t jR=0;jR<nR;jR++) {

						if(r1aux > rCellsBoundaries[jR] && r1aux < rCellsBoundaries[jR+1]) {

							scattAA[iR][jR] += psc*pescap;
							if (countAA[jR] == 0) {
								reachAA[iR][jR] += pescap;
								countAA[jR]++;
							}
						}
                    }
					pescap *= exptau;
                    drprim=r1*(pasoprim-1.0);
                    rprim += drprim;
                } while(r1aux < rBound && z1 < rBound && r1 > schwRadius);
				LOOP:;
            }
        }
        for(size_t jR=0;jR<nR;jR++) {
            scattAA[iR][jR] /= (nPhot*nTheta);     // Dividing by the number of photons launched.
			reachAA[iR][jR] /= (nPhot*nTheta);
        }
		for (size_t jRcd=0;jRcd<nRcd;jRcd++) {
			reachAD[iR][jRcd] /= (nPhot*nTheta);
		}
	}

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

	// ADAF ESCAPE PHOTONS
	#pragma omp parallel for
	for (int iR=0;iR<nR;iR++) {
		double r0 = st.thetaH.ps[DIM_R][iR];
		double drprim=r0*(pasoprim-1.0);
		double thetaMin;
		if (height_method == 0)
			thetaMin = st.thetaH.get({0,iR,0});
		else
			thetaMin = atan(r0/height_fun(r0));
		double dyaux=(1.0-sin(thetaMin))/nTheta;
        for(size_t kTh=1;kTh<=nTheta;kTh++) {
            double yaux=sin(thetaMin)+kTh*dyaux;  // Theta distributed uniformly in sin(theta).
            double theta0 = (yaux < 1.0 ? asin(yaux) : pi/2.0);
			double phi0 = 0.0;
			double dPhi = 2.0*pi/nPhot;

			double x0,y0,z0;
			if (height_method == 0) {
				x0 = r0*sin(theta0)*cos(phi0);
				y0 = r0*sin(theta0)*cos(phi0);
				z0 = r0*cos(theta0);
			} else {
				x0 = r0*sin(phi0);
				y0 = r0*cos(phi0);
				z0 = r0/tan(theta0);
			}
			for(size_t jPh=1;jPh<=nPhot;jPh++) {
				double thetaprim = inclination*(pi/180.0);
				double rprim=drprim;
				double r1 = r0;
				double z1 = z0;
				double r1aux = r1;
				double pescap = 1.0;
				do {
					double xprim=rprim*sin(thetaprim);
					double zprim=rprim*cos(thetaprim);
					double x1=x0+xprim;
					z1=z0+zprim;
					r1=sqrt(x1*x1+y0*y0+z1*z1);
					double theta1=atan(sqrt(x1*x1+y0*y0)/abs(z1));
					r1aux = (height_method == 0) ? r1 : sqrt(x1*x1+y0*y0);
					double ne = electronDensityTheta(r1,theta1);
					pescap *= exp(-ne*thomson*drprim);
					drprim=r1*(pasoprim-1.0);
					rprim += drprim;
				} while (r1aux < rCellsBoundaries[nR] && z1 < rCellsBoundaries[nR]);
				escapeAi[iR] += pescap;
				phi0 += dPhi;
            }
        }
		escapeAi[iR] /= (nPhot*nTheta);     // Dividing by the number of photons launched.
	}

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

	// COLD DISK SCATTERING MATRIX
	#pragma omp parallel for
	for (int iRcd=0;iRcd<nRcd;iRcd++) {
		double r0cd = st.denf_e.ps[DIM_Rcd][iRcd];
		for(size_t jPh=1;jPh<=nPhot;jPh++) {
			double drprim = r0cd*(pasoprim-1.0);             // Step for the photon path.
			double random_number = gsl_rng_uniform(RandomNumberGenerator);
			double phiprim = 2.0*pi*random_number;
			random_number = gsl_rng_uniform(RandomNumberGenerator);
			double thetaprim = 0.5*acos(1.0-2.0*random_number);   // Photon directions
																  // distributed
			double rprim=drprim; 	                           	  // proportional to cos(theta)sin(theta).
			double pescap = 1.0;
			double r1 = r0cd;
			double r1aux = r1;
			double z1 = 0.0;
			vector<size_t> countDA(nR,0);
			do {
				double xprim=rprim*sin(thetaprim)*cos(phiprim);
				double yprim=rprim*sin(thetaprim)*sin(phiprim);
				double zprim=rprim*cos(thetaprim);
				double x1=xprim;
				double y1=r0cd+yprim;
				z1=zprim;
				r1=sqrt(x1*x1+y1*y1+z1*z1);
				double theta1=atan(sqrt(x1*x1+y1*y1)/abs(z1));
				r1aux = (height_method == 0) ? r1 : sqrt(x1*x1+y1*y1);
				double exptau = 1.0;
				if (r1aux < rCellsBoundaries[nR]) {
					double ne = electronDensityTheta(r1aux,theta1);
					exptau = exp(-ne*thomson*drprim);
					double psc = 1.0-exptau;    // Probability of scattering.
					for(size_t jR=0;jR<=nR;jR++) {
						if(r1aux > rCellsBoundaries[jR] && r1aux < rCellsBoundaries[jR+1]) {
							scattDA[iRcd][jR] += psc*pescap;
							if (countDA[jR] == 0) {
								reachDA[iRcd][jR] += pescap;
								countDA[jR]++;
							}
						}
					}
				}
				drprim=r1aux*(pasoprim-1.0);
				rprim += drprim;
				pescap *= exptau;                // Probability that a photon reaches
												 // the previous position.
			} while(r1aux < rBound && z1 < rBound && r1 > schwRadius);
        }
        for(size_t jR=0;jR<nR;jR++) {
            scattDA[iRcd][jR] /= nPhot;     // Dividing by the number of photons launched.
			reachDA[iRcd][jR] /= nPhot;
        }
	}

	//COLD DISK ESCAPE PHOTONS
	#pragma omp parallel for
	for (int iRcd=0;iRcd<nRcd;iRcd++) {
		double r0cd = st.denf_e.ps[DIM_Rcd][iRcd];
		double drprim=r0cd*(pasoprim-1.0);
		for(size_t jPh=1;jPh<=nPhot;jPh++) {
			double random_number = gsl_rng_uniform(RandomNumberGenerator);
			double phiprim = 2.0*pi*random_number;
			double thetaprim = inclination*(pi/180.0);
			double rprim=drprim;
			double r1 = r0cd;
			double z1 = 0.0;
			double r1aux = r1;
			double pescap = 1.0;
			do {
				double xprim=rprim*sin(thetaprim)*cos(phiprim);
				double yprim=rprim*sin(thetaprim)*sin(phiprim);
				double zprim=rprim*cos(thetaprim);
				double x1=xprim;
				double y1=r0cd+yprim;
				z1=zprim;
				r1=sqrt(x1*x1+y1*y1+z1*z1);
				double theta1=atan(sqrt(x1*x1+y1*y1)/abs(z1));
				r1aux = (height_method == 0) ? r1 : sqrt(x1*x1+y1*y1);
				double ne = electronDensityTheta(r1,theta1);
				pescap *= exp(-ne*thomson*drprim);
				drprim=r1*(pasoprim-1.0);
				rprim += drprim;
			} while(r1aux < rBound && z1 < rBound);     // Escape from the region.
			escapeDi[iRcd] += pescap;
        }
        escapeDi[iRcd] /= nPhot;
	}

    FinaliseRandom();
	comptonScattMatrixWrite();

	show_message(msgEnd,Module_comptonScattMatrix);
}

void comptonScattMatrixRead(State& st)
{
	matrixInit(scattAA,nR,nR,0.0);
	matrixInit(scattDA,nRcd,nR,0.0);
	matrixInit(reachAD,nR,nRcd,0.0);
	matrixInit(reachAA,nR,nR,0.0);
	matrixInit(reachDA,nRcd,nR,0.0);
    escapeAi.resize(nR,0.0);
	escapeDi.resize(nRcd,0.0);

	matrixRead("scattAA.dat",scattAA,nR,nR);
	matrixRead("scattDA.dat",scattDA,nRcd,nR);
	matrixRead("reachAD.dat",reachAD,nR,nRcd);
	matrixRead("reachAA.dat",reachAA,nR,nR);
	matrixRead("reachDA.dat",reachDA,nRcd,nR);
	vectorRead("escapeAi.dat",escapeAi,nR);
	vectorRead("escapeDi.dat",escapeDi,nRcd);
}


/*
gibbsOutput.cpp - This file is part of the Bayesembler (v1.1.1)

The MIT License (MIT)

Copyright (c) 2014 Lasse Maretty and Jonas Andreas Sibbesen

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include <gibbsOutput.h>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <fstream>
#include <sstream>
#include <bitset>
#include <math.h>

using namespace std;

GibbsOutput::GibbsOutput(int num_iterations_in, int num_transcripts_in) {

    num_iterations = num_iterations_in;
    num_transcripts = num_transcripts_in;

    marginal_occurence = Eigen::VectorXi::Zero(num_transcripts);
    marginal_mean_cumulant = Eigen::VectorXd::Zero(num_transcripts);
    marginal_var_cumulant = Eigen::VectorXd::Zero(num_transcripts);

}

void GibbsOutput::addSample(vector<int> plus, vector<double> values) {

    Eigen::VectorXd plus_values(plus.size());

	sort(plus.begin(), plus.end());

    for (int i=0; i < plus.size(); i++) {

        int idx = plus[i];
        marginal_occurence(idx)++;

        // If first occurence
        if (marginal_occurence(idx) == 1) {

            marginal_mean_cumulant(idx) = values[idx];

        } else {

            double delta = values[idx] - marginal_mean_cumulant(idx);
            marginal_mean_cumulant(idx) += delta/marginal_occurence(idx);
            marginal_var_cumulant(idx) += delta * (values[idx] - marginal_mean_cumulant(idx));
        }
    }

}

Ensemble GibbsOutput::getTopMarginals(double marginal_threshold) {

    double count_threshold = marginal_threshold * num_iterations;
    vector<int> indices;

    for (int i=0; i < num_transcripts; i++) {

        if (marginal_occurence(i) >= count_threshold) {

            indices.push_back(i);
        }
    }

    Eigen::VectorXd means(indices.size());
    Eigen::VectorXd sds(indices.size());

    for (int i=0; i < indices.size(); i++) {

        means(i) = marginal_mean_cumulant(indices[i]);
        sds(i) = sqrt(marginal_var_cumulant(indices[i])/(marginal_occurence(indices[i])-1));
    }

    Ensemble top_marginals;

    top_marginals.indices = indices;
    top_marginals.means = means;
    top_marginals.sd = sds;

    return top_marginals;
}

string GibbsOutput::writeEnsembleGtf(Ensemble top_ensemble, vector<Candidate> candidates, tr1::unordered_map<int,pair<int,double> > new_idx_to_old_idx_effective_length, int total_num_fragments, double count_threshold) {

    stringstream output_string;

    for (int i=0; i < top_ensemble.indices.size(); i++) {

        int new_idx = top_ensemble.indices[i];
        int old_idx = new_idx_to_old_idx_effective_length[new_idx].first;
        double expected_count = top_ensemble.means(i) * new_idx_to_old_idx_effective_length[new_idx].second * total_num_fragments/1e9;

        if (expected_count < count_threshold) {

            continue;

        }

        Candidate current_candidate = candidates[old_idx];

        assert(current_candidate.idx == old_idx);

        if (!current_candidate.isPreMrna) {

            vector<Vertex> current_vertices = current_candidate.vertices;

            for (int j=0; j < current_vertices.size(); j++) {

                output_string << current_vertices[j].reference << "\tBayesembler\texon\t" << current_vertices[j].start << "\t" << current_vertices[j].end << "\t.\t" << current_vertices[j].strand << "\t.\tgene_id \"" << current_candidate.graph_name << "\"; transcript_id \"" << current_candidate.name << "\"; transcript_confidence \"" << ((double) marginal_occurence(new_idx)/num_iterations) << "\"; FPKM \"" << top_ensemble.means(i) << "\"; FPKM_sd \"" << top_ensemble.sd(i) << "\"; expected_count \"" << expected_count << "\";" << endl;
            }
        }
    }

    return output_string.str();
}

string GibbsOutput::writeCandidateGtf(vector<Candidate> candidates, tr1::unordered_map<int,pair<int,double> > new_idx_to_old_idx_effective_length, int total_num_fragments) {

    stringstream output_string;

    for (int i=0; i < num_transcripts; i++) {

        int old_idx = new_idx_to_old_idx_effective_length[i].first;
        double expected_count = marginal_mean_cumulant(i) * new_idx_to_old_idx_effective_length[i].second * total_num_fragments/1e9;

        Candidate current_candidate = candidates[old_idx];

        assert(current_candidate.idx == old_idx);

        vector<Vertex> current_vertices = current_candidate.vertices;

        for (int j=0; j < current_vertices.size(); j++) {

            output_string << current_vertices[j].reference << "\tBayesembler\texon\t" << current_vertices[j].start << "\t" << current_vertices[j].end << "\t.\t" << current_vertices[j].strand << "\t.\tgene_id \"" << current_candidate.graph_name << "\"; transcript_id \"" << current_candidate.name << "\"; transcript_confidence \"" << ((double) marginal_occurence(i)/num_iterations) << "\"; FPKM \"" << marginal_mean_cumulant(i) << "\"; FPKM_sd \"" << sqrt(marginal_var_cumulant(i)/(marginal_occurence(i) - 1)) << "\"; expected_count \"" << expected_count << "\";" << endl;
        }
    }

    return output_string.str();

}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SIMD_COMMON_REM_PIO2_STRAIGHT_HPP_INCLUDED
#define NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SIMD_COMMON_REM_PIO2_STRAIGHT_HPP_INCLUDED

#include <nt2/toolbox/trigonometric/functions/rem_pio2_straight.hpp>
#include <nt2/toolbox/trigonometric/constants.hpp>
#include <nt2/toolbox/constant/include/constants/zero.hpp>
#include <nt2/toolbox/constant/include/constants/one.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <boost/fusion/tuple.hpp>

namespace nt2 { namespace ext
{

  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::rem_pio2_straight_, boost::simd::tag::simd_,
                      (A0)(X),
                      ((simd_ < floating_<A0>,X > ))
                    )
  {
    typedef typename meta::as_integer<A0>::type            itype;
    typedef boost::fusion::tuple<A0,A0,itype>        result_type;

    inline result_type operator()(A0 const& a0) const
    {
      result_type res;
      boost::fusion::at_c<2>(res) =
        nt2::rem_pio2_straight(a0,
                               boost::fusion::at_c<0>(res),
                               boost::fusion::at_c<1>(res)
                              );
      return res;
    }
  };

  /////////////////////////////////////////////////////////////////////////////
  // reference based Implementation when real
  /////////////////////////////////////////////////////////////////////////////
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::rem_pio2_straight_, boost::simd::tag::simd_,
                      (A0)(X),
                      ((simd_ < floating_<A0>, X > ))
                      ((simd_ < floating_<A0>, X > ))
                      ((simd_ < floating_<A0>, X > ))
                    )
  {

    typedef typename meta::as_integer<A0>::type result_type;
    inline result_type operator()(A0 const& x, A0 & xr, A0& xc) const
    {
      xr = x-Pio2_1<A0>();
      xr -= Pio2_2<A0>();
      xr -= Pio2_3<A0>();
      xc = nt2::Zero<A0>();
      return nt2::One<result_type>();
    }
  };
} }
#endif