//
// This file is part of the libWetHair open source project
//
// The code is licensed solely for academic and non-commercial use under the
// terms of the Clear BSD License. The terms of the Clear BSD License are
// provided below. Other licenses may be obtained by contacting the faculty
// of the Columbia Computer Graphics Group or a Columbia University licensing officer.
//
// The Clear BSD License
//
// Copyright 2017 Yun (Raymond) Fei, Henrique Teles Maia, Christopher Batty,
// Changxi Zheng, and Eitan Grinspun
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted (subject to the limitations in the disclaimer
// below) provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice, this
//  list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright notice,
//  this list of conditions and the following disclaimer in the documentation
//  and/or other materials provided with the distribution.
//
// * Neither the name of the copyright holder nor the names of its contributors may be used
//  to endorse or promote products derived from this software without specific
//  prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
// LICENSE. 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.

#include "CTCD.h"
#include <vector>
#include "rpoly.h"
#include <Eigen/Geometry>
#include <iostream>

using namespace Eigen;
using namespace std;

bool TimeInterval::overlap(const TimeInterval &t1, const TimeInterval &t2)
{
    return !(t1.l > t2.u || t2.l > t1.u);
}

bool TimeInterval::overlap(const std::vector<TimeInterval> &intervals)
{
    for(std::vector<TimeInterval>::const_iterator it1 = intervals.begin(); it1 != intervals.end(); ++it1)
    {
        std::vector<TimeInterval>::const_iterator it2 = it1;
        for(++it2; it2 != intervals.end(); ++it2)
            if(!overlap(*it1, *it2))
                return false;
    }
    return true;
}

TimeInterval TimeInterval::intersect(const std::vector<TimeInterval> &intervals)
{
    TimeInterval isect(0.0, 1.0);
    for(std::vector<TimeInterval>::const_iterator it = intervals.begin(); it != intervals.end(); ++it)
    {
        isect.l = max(it->l, isect.l);
        isect.u = min(it->u, isect.u);
    }
    return isect;
}

int CTCD::getQuadRoots(double a, double b, double c, double &t0, double &t1) {
    int roots = 0;
    int sign = 1;

    if (b < 0)
        sign = -1;

    double D = b * b - 4 * a * c;
    if (D >= 0)
    {
        roots = 2;
        double q = -0.5 * (b + sign * sqrt(D));
        t0 = q / a;
        t1 = c / q;
        if (t0 > t1)
            std::swap(t1, t0);
    }
    return roots;
}

void CTCD::checkInterval(double t1, double t2, double * op, int degree, vector<TimeInterval> &intervals, bool pos)
{
    // clamp values
    t1 = max(0.0, t1);
    t2 = max(0.0, t2);
    t1 = min(1.0, t1);
    t2 = min(1.0, t2);

    double tmid = (t2 + t1) / 2;
    double f = op[0];
    for(int i=1; i<=degree; i++)
    {
        f *= tmid;
        f += op[i];
    }

    if (pos && f >= 0)
        intervals.push_back(TimeInterval(t1, t2));
    else if (!pos && f <= 0)
        intervals.push_back(TimeInterval(t1, t2));
}

bool CTCD::couldHaveRoots(double *op, int degree, bool pos) {
    double result = 0;
    if ((pos && op[0] > 0) || (!pos && op[0] < 0))
        result = op[0];
    for (int i = 1; i < degree; i++)
    {
        result *= 1.0;
        if ((pos && op[i] > 0) || (!pos && op[i] < 0))
            result += op[i];
    }
    result *= 1.0;
    result += op[degree];
    return !((pos && result < 0) || (!pos && result > 0));
}

void CTCD::findIntervals(double *op, int n, vector<TimeInterval> & intervals, bool pos)
{
    int roots=0;
    int reducedDegree=n;

    if(n>6)
    {
        assert(!"Polynomials of degree > 6 not supported");
        return;
    }

    double time[6];
    // We don't care one bit about these imaginary roots
    double zeroi[6];

    // normalize
    double maxval = 0;
    for(int i=0; i<=n; i++)
    maxval = std::max(maxval, fabs(op[i]));
    if(maxval != 0)
    for(int i=0; i<=n; i++)
        op[i] /= maxval;

    for (int i = 0; i < n; i++)
    {
        if (op[i] == 0)
            reducedDegree--;
        else
            break;
    }

    if (reducedDegree < n)
    {
        for (int i = 0; i <= reducedDegree; i++)
            op[i] = op[i + n - reducedDegree];
    }

    if (reducedDegree > 2) {
        if (!couldHaveRoots(op, reducedDegree, pos))
            return;

        RootFinder rf;

        roots = rf.rpoly(op, reducedDegree, time, zeroi);
    }
    else if (reducedDegree == 2)
    {
        roots = getQuadRoots(op[0], op[1], op[2], time[0], time[1]);
    }
    else if (reducedDegree == 1)
    {
        time[0] = -op[1] / op[0];
        roots = 1;
    }
    else
    {
        // both points stationary -- check if colliding at t=0
        if ((!pos && op[0] <= 0) || (pos && op[0] >= 0))
            intervals.push_back(TimeInterval(0, 1.0));
        return;
    }

    // check intervals
    if (roots > 0)
    {
        std::sort(time, time + roots);
        if (time[0] >= 0)
            checkInterval(0, time[0], op, reducedDegree, intervals, pos);
        for (int i = 0; i < roots - 1; i++) {
            if (!((time[i] < 0 && time[i + 1] < 0) || (time[i] > 1.0 && time[i + 1] > 1.0)))
                checkInterval(time[i], time[i + 1], op, reducedDegree, intervals, pos);
        }
        if (time[roots - 1] <= 1.0)
            checkInterval(time[roots - 1], 1.0, op, reducedDegree, intervals, pos);
    }
    else
    {
        checkInterval(0.0, 1.0, op, reducedDegree, intervals, pos);
    }
}

void CTCD::barycentricPoly3D(const Vector3d &x10,
                             const Vector3d &x20,
                             const Vector3d &x30,
                             const Vector3d &v10,
                             const Vector3d &v20,
                             const Vector3d &v30,
                             vector<TimeInterval> &result)
{
    // alpha > 0
    double A = x10.dot(x10);
    double B = 2 * x10.dot(v10);
    double C = (v10).dot(v10);
    // e0.e1
    double D = (x20).dot(x10);
    double E = (x20).dot(v10) + (v20).dot(x10);
    double F = (v20).dot(v10);
    //(q0-q1).e0
    double G = (x30).dot(x20);
    double H = (x30).dot(v20) + (v30).dot(x20);
    double I = (v30).dot(v20);
    //(q0-q1).e1
    double J = (x30).dot(x10);
    double K = (x30).dot(v10) + (v30).dot(x10);
    double L = (v30).dot(v10);

    double op[5];

    op[0] = F * L - C * I;
    op[1] = F * K + E * L - C * H - B * I;
    op[2] = F * J + D * L + E * K - C * G - A * I - B * H;
    op[3] = D * K + E * J - A * H - B * G;
    op[4] = D * J - A * G;

    findIntervals(op, 4, result, true);
}

void CTCD::planePoly3D(const Vector3d &x10,
                       const Vector3d &x20,
                       const Vector3d &x30,
                       const Vector3d &v10,
                       const Vector3d &v20,
                       const Vector3d &v30,
                       vector<TimeInterval> &result)
{
    double op[4];
    op[0] = v10.dot(v20.cross(v30));
    op[1] = x10.dot(v20.cross(v30)) + v10.dot(x20.cross(v30)) + v10.dot(v20.cross(x30));
    op[2] = x10.dot(x20.cross(v30)) + x10.dot(v20.cross(x30)) + v10.dot(x20.cross(x30));
    op[3] = x10.dot(x20.cross(x30));
    findIntervals(op, 3, result, true);
}

void CTCD::distancePoly3D(const Vector3d &x10,
                          const Vector3d &x20,
                          const Vector3d &x30,
                          const Vector3d &v10,
                          const Vector3d &v20,
                          const Vector3d &v30,
                          double minDSquared,
                          vector<TimeInterval> &result)
{
    double A = v10.dot(v20.cross(v30));
    double B = x10.dot(v20.cross(v30)) + v10.dot(x20.cross(v30)) + v10.dot(v20.cross(x30));
    double C = x10.dot(x20.cross(v30)) + x10.dot(v20.cross(x30)) + v10.dot(x20.cross(x30));
    double D = x10.dot(x20.cross(x30));
    Vector3d E = x20.cross(x30);
    Vector3d F = x20.cross(v30) + v20.cross(x30);
    Vector3d G = v20.cross(v30);

    double op[7];
    op[0] = A * A;
    op[1] = 2 * A * B;
    op[2] = B * B + 2 * A * C - G.dot(G) * minDSquared;
    op[3] = 2 * A * D + 2 * B * C - 2 * G.dot(F) * minDSquared;
    op[4] = 2 * B * D + C * C - (2 * G.dot(E) + F.dot(F)) * minDSquared;
    op[5] = 2 * C * D - 2 * F.dot(E) * minDSquared;
    op[6] = D * D - E.dot(E) * minDSquared;
    findIntervals(op, 6, result, false);
}

bool CTCD::edgeEdgeCTCD(const Eigen::Vector3d &q0start,
                        const Eigen::Vector3d &p0start,
                        const Eigen::Vector3d &q1start,
                        const Eigen::Vector3d &p1start,
                        const Eigen::Vector3d &q0end,
                        const Eigen::Vector3d &p0end,
                        const Eigen::Vector3d &q1end,
                        const Eigen::Vector3d &p1end,
                        double eta,
                        double &t)
{
    double minD = eta * eta;

    // time intervals during which v is colinear with the edge, on the side of e1 towards e2, and on the side of e2 towards e1
    std::vector<TimeInterval> rawcoplane, a0, a1, b0, b1;

    Vector3d x10 = p0start - p1start;
    Vector3d x20 = p0start - q0start;
    Vector3d x30 = p1start - q1start;

    Vector3d vp0 = p0end-p0start;
    Vector3d vp1 = p1end-p1start;
    Vector3d vq0 = q0end-q0start;
    Vector3d vq1 = q1end-q1start;

    Vector3d v10 = vp0 - vp1;
    Vector3d v20 = vp0 - vq0;
    Vector3d v30 = vp1 - vq1;

    distancePoly3D(x10, x20, x30, v10, v20, v30, minD, rawcoplane);

    // check for parallel edges
    std::vector<TimeInterval> coplane;
    std::vector<TimeInterval> parallel;

    for(size_t i=0; i<rawcoplane.size(); i++)
    {
        double midt = (rawcoplane[i].u + rawcoplane[i].l) / 2;
        x10 = (q0start - p0start) + midt * (vq0 - vp0);
        x20 = (q1start - p1start) + midt * (vq1 - vp1);

        if (x10.cross(x20).norm() < 1e-8)
        {
            // handle parallel edges via VertexEdgeCTCD
            // parallel.push_back(rawcoplane[i]);
            // std::cout << "parallel edges detected! in CTCD, edgeEdgeCTCD" <<std::endl;
            // std::exit( EXIT_FAILURE );

            // [H] my attempt to rectify parallel edge collisions
            if( vertexEdgeCTCD( q0start, q1start, p1start, q0end, q1end, p1end, eta, t ) ){
                return true;
            }
            else{
                return vertexEdgeCTCD( p0start, q1start, p1start, p0end, q1end, p1end, eta, t );
            }

        }
        else
    {
            coplane.push_back(rawcoplane[i]);
    }
    }

    if(coplane.empty())
        return false;

    x10 = p1start - q1start;
    v10 = vp1 - vq1;
    x20 = p0start - q0start;
    v20 = vp0 - vq0;
    x30 = q0start - q1start;
    v30 = vq0 - vq1;
    barycentricPoly3D(x10, x20, x30, v10, v20, v30, a0);
    if(a0.empty())
        return false;

    x20 = q0start - p0start;
    v20 = vq0 - vp0;
    x30 = p0start - q1start;
    v30 = vp0 - vq1;
    barycentricPoly3D(x10, x20, x30, v10, v20, v30, a1);
    if(a1.empty())
        return false;

    x10 = p0start - q0start;
    v10 = vp0 - vq0;
    x20 = p1start - q1start;
    v20 = vp1 - vq1;
    x30 = q1start - q0start;
    v30 = vq1 - vq0;
    barycentricPoly3D(x10, x20, x30, v10, v20, v30, b0);
    if(b0.empty())
        return false;

    //x10 = p0 - q0;
    //v10 = vp0 - vq0;
    x20 = q1start - p1start;
    v20 = vq1 - vp1;
    x30 = p1start - q0start;
    v30 = vp1 - vq0;

    barycentricPoly3D(x10, x20, x30, v10, v20, v30, b1);
    if(b1.empty())
        return false;

    // check intervals for overlap
    bool col = false;
    double mint = 1.0;
    for (int i = 0; i < (int) coplane.size(); i++)
    {
        for (int j = 0; j < (int) a0.size(); j++)
        {
            for (int k = 0; k < (int) a1.size(); k++)
            {
                for (int l = 0; l < (int) b0.size(); l++)
                {
                    for (int m = 0; m < (int) b1.size(); m++)
                    {
                        vector<TimeInterval> intervals;
                        intervals.push_back(coplane[i]);
                        intervals.push_back(a0[j]);
                        intervals.push_back(a1[k]);
                        intervals.push_back(b0[l]);
                        intervals.push_back(b1[m]);
                        if (TimeInterval::overlap(intervals) )
                        {
                            TimeInterval isect = TimeInterval::intersect(intervals);
                bool skip = false;
                for(int p = 0; p < (int)parallel.size(); p++)
                {
                vector<TimeInterval> pcheck;
                pcheck.push_back(isect);
                pcheck.push_back(parallel[p]);
                if(TimeInterval::overlap(pcheck) )
                {
                    skip = true;
                    break;
                }
                }
                if(!skip)
                {
                                mint = min(mint, isect.l);
                                col = true;
                }
                        }
                    }
                }
            }
        }
    }

    if(col)
    {
        t = mint;
        return true;
    }
    return false;
}

bool CTCD::vertexFaceCTCD(const Vector3d &q0start,
                          const Vector3d &q1start,
                          const Vector3d &q2start,
                          const Vector3d &q3start,
                          const Vector3d &q0end,
                          const Vector3d &q1end,
                          const Vector3d &q2end,
                          const Vector3d &q3end,
                          double eta, double &t)
{
    double minD = eta * eta;
    Vector3d v0 = q0end - q0start;
    Vector3d v1 = q1end - q1start;
    Vector3d v2 = q2end - q2start;
    Vector3d v3 = q3end - q3start;

    // time intervals during which v is colinear with the edge, on the side of e1 towards e2, and on the side of e2 towards e1
    vector<TimeInterval> coplane, e1, e2, e3;

    // check p.((axb)xb)
    Vector3d x10 = q0start - q1start;
    Vector3d v10 = v0 - v1;
    Vector3d x20 = (q3start - q1start).cross(q2start - q1start);
    Vector3d v20 = (v3 - v1).cross(v2 - v1);
    Vector3d x30 = q3start - q1start;
    Vector3d v30 = v3 - v1;
    planePoly3D(x10, x20, x30, v10, v20, v30, e1);

    if(e1.empty())
        return false;

    x10 = q0start - q2start;
    v10 = v0 - v2;
    x20 = (q1start - q2start).cross(q3start - q2start);
    v20 = (v1 - v2).cross(v3 - v2);
    x30 = q1start - q2start;
    v30 = v1 - v2;
    planePoly3D(x10, x20, x30, v10, v20, v30, e2);

    if(e2.empty())
        return false;

    x10 = q0start - q3start;
    v10 = v0 - v3;
    x20 = (q2start - q3start).cross(q1start - q3start);
    v20 = (v2 - v3).cross(v1 - v3);
    x30 = q2start - q3start;
    v30 = v2 - v3;
    planePoly3D(x10, x20, x30, v10, v20, v30, e3);

    if(e3.empty())
        return false;

    x10 = q0start - q1start;
    x20 = q2start - q1start;
    x30 = q3start - q1start;
    v10 = v0 - v1;
    v20 = v2 - v1;
    v30 = v3 - v1;
    distancePoly3D(x10, x20, x30, v10, v20, v30, minD, coplane);

    if(coplane.empty())
        return false;

    bool col = false;
    double mint = 1.0;
    for (int i = 0; i < (int) coplane.size(); i++)
    {
        for (int j = 0; j < (int) e1.size(); j++)
        {
            for (int k = 0; k < (int) e2.size(); k++)
            {
                for (int l = 0; l < (int) e3.size(); l++)
                {
                    vector<TimeInterval> intervals;
                    intervals.push_back(coplane[i]);
                    intervals.push_back(e1[j]);
                    intervals.push_back(e2[k]);
                    intervals.push_back(e3[l]);
                    if(TimeInterval::overlap(intervals))
                    {
                        mint = std::min(TimeInterval::intersect(intervals).l, mint);
                        col = true;
                    }
                }
            }
        }
    }

    if(col)
    {
        t = mint;
        return true;
    }
    return false;
}

bool CTCD::vertexEdgeCTCD(const Vector3d &q0start,
                          const Vector3d &q1start,
                          const Vector3d &q2start,
                          const Vector3d &q0end,
                          const Vector3d &q1end,
                          const Vector3d &q2end,
                          double eta,
                          double &t)
{
    double op[5];
    double minD = eta*eta;
    Vector3d v0 = q0end-q0start;
    Vector3d v1 = q1end-q1start;
    Vector3d v2 = q2end-q2start;

    // time intervals during which v is colinear with the edge, on the side of e1 towards e2, and on the side of e2 towards e1
    vector<TimeInterval> colin, e1, e2;

    Vector3d ab = q2start - q1start;
    Vector3d ac = q0start - q1start;
    Vector3d cb = q2start - q0start;
    Vector3d vab = v2 - v1;
    Vector3d vac = v0 - v1;
    Vector3d vcb = v2 - v0;

    double c = ab.dot(ac);
    double b = ac.dot(vab) + ab.dot(vac);
    double a = vab.dot(vac);
    op[0] = a;
    op[1] = b;
    op[2] = c;
    findIntervals(op, 2, e1, true);
    if(e1.empty())
        return false;

    c = ab.dot(cb);
    b = cb.dot(vab) + ab.dot(vcb);
    a = vab.dot(vcb);

    op[0] = a;
    op[1] = b;
    op[2] = c;
    findIntervals(op, 2, e2, true);
    if(e2.empty())
        return false;

    double A = ab.dot(ab);
    double B = 2 * ab.dot(vab);
    double C = vab.dot(vab);
    double D = ac.dot(ac);
    double E = 2 * ac.dot(vac);
    double F = vac.dot(vac);
    double G = ac.dot(ab);
    double H = vab.dot(ac) + vac.dot(ab);
    double I = vab.dot(vac);
    op[4] = A * D - G * G - minD * A;
    op[3] = B * D + A * E - 2 * G * H - minD * B;
    op[2] = B * E + A * F + C * D - H * H - 2 * G * I - minD * C;
    op[1] = B * F + C * E - 2 * H * I;
    op[0] = C * F - I * I;
    findIntervals(op, 4, colin, false);
    if(colin.empty())
        return false;

    double mint = 1.0;
    bool col = false;
    for (int i = 0; i < (int) colin.size(); i++)
    {
        for (int j = 0; j < (int) e1.size(); j++)
        {
            for (int k = 0; k < (int) e2.size(); k++)
            {
                vector<TimeInterval> intervals;
                intervals.push_back(colin[i]);
                intervals.push_back(e1[j]);
                intervals.push_back(e2[k]);
                if(TimeInterval::overlap(intervals))
                {
                    mint = std::min(TimeInterval::intersect(intervals).l, mint);
                    col = true;
                }
            }
        }
    }

    if(col)
    {
        t = mint;
        return true;
    }
    return false;
}

bool CTCD::vertexVertexCTCD(const Vector3d &q1start,
                            const Vector3d &q2start,
                            const Vector3d &q1end,
                            const Vector3d &q2end,
                            double eta, double &t)
{
    int roots = 0;
    double min_d = eta*eta;
    double t1 = 0, t2 = 0;
    Vector3d v1 = q1end-q1start;
    Vector3d v2 = q2end-q2start;

    // t^2 term
    double a = v1.dot(v1) + v2.dot(v2) - 2 * v1.dot(v2);
    // t term
    double b = 2 * (v1.dot(q1start) - v2.dot(q1start) - v1.dot(q2start) + v2.dot(q2start));
    // current distance - min_d
    double c = q1start.dot(q1start) + q2start.dot(q2start) - 2 * q1start.dot(q2start) - min_d;
    if (a != 0)
    {
        roots = getQuadRoots(a, b, c, t1, t2);
    }
    else if (b != 0)
    {
        t1 = -c / b;
        roots = 1;
    }
    else
    {
        if(c<=0)
        {
            t = 0;
            return true;
        }
        return false;
    }

    double op[3];
    op[0] = a;
    op[1] = b;
    op[2] = c;
    vector<TimeInterval> interval;

    if (roots == 2)
    {
        checkInterval(0, t1, op, 2, interval, false);
        if(!interval.empty())
        {
            t = 0;
            return true;
        }
        checkInterval(t1, t2, op, 2, interval, false);
        if(!interval.empty())
        {
            t = t1;
            return true;
        }
        checkInterval(t2, 1.0, op, 2, interval, false);
        if(!interval.empty())
        {
            t = t2;
            return true;
        }
        return false;
    }
    else if (roots == 1)
    {
        checkInterval(0, t1, op, 2, interval, false);
        if(!interval.empty())
        {
            t = 0;
            return true;
        }
        checkInterval(t1, 1.0, op, 2, interval, false);
        if(!interval.empty())
        {
            t = t1;
            return true;
        }
        return false;
    }
    checkInterval(0, 1.0, op, 2, interval, false);
    if(!interval.empty())
    {
        t = 0;
        return true;
    }
    return false;
}

bool CTCD::checkEEContact( const Eigen::Vector3d &q0start,
                                const Eigen::Vector3d &p0start,
                                const Eigen::Vector3d &q1start,
                                const Eigen::Vector3d &p1start,
                                const Eigen::Vector3d &q0end,
                                const Eigen::Vector3d &p0end,
                                const Eigen::Vector3d &q1end,
                                const Eigen::Vector3d &p1end,
                                double eta,
                                double &t)
{

    if( edgeEdgeCTCD( q0start, p0start, q1start, p1start, q0end, p0end, q1end, p1end, eta, t) )
    {
        return true;
    }

    // Edge-edge vertices
    if( vertexEdgeCTCD(q0start, q1start, p1start, q0end, q1end, p1end, eta, t) )
    {
        return true;
    }
    if( vertexEdgeCTCD(p0start, q1start, p1start, p0end, q1end, p1end, eta, t) )
    {
        return true;
    }
    if( vertexEdgeCTCD(q1start, q0start, p0start, q1end, q0end, p0end, eta, t) )
    {
        return true;
    }
    if( vertexEdgeCTCD(p1start, q0start, p0start, p1end, q0end, p0end, eta, t) )
    {
        return true;
    }

    // edge vertex-edge vertex
    if( vertexVertexCTCD(q0start, q1start, q0end, q1end, eta, t) )
    {
        return true;
    }
    if( vertexVertexCTCD(q0start, p1start, q0end, p1end, eta, t) )
    {
        return true;
    }
    if( vertexVertexCTCD(p0start, q1start, p0end, q1end, eta, t) )
    {
        return true;
    }
    if( vertexVertexCTCD(p0start, p1start, p0end, p1end, eta, t) )
    {
        return true;
    }
    return false;
}

template<typename ComparableT>
EIGEN_STRONG_INLINE ComparableT clamp( const ComparableT x, const ComparableT l, const ComparableT u )
{
    return ( x > u ) ? u : ( ( x > l ) ? x : l );
}

// Adapted from Christer Ericson, "Real Time Collision Detection"
// Computes closest points C1 and C2 of S1(s)=P1+s*(Q1-P1) and
// S2(t)=P2+t*(Q2-P2), returning s and t. Function result is squared
// distance between between S1(s) and S2(t).
// TODO: Explore behavior in degenerate case more closely.
double CTCD::ClosestPtSegmentSegment( const Vector3d& p1, const Vector3d& q1, const Vector3d& p2, const Vector3d& q2,
        double& s, double& t, Vector3d& c1, Vector3d& c2 )
{
    double EPSILON = 1.0e-12;

    Vector3d d1 = q1 - p1; // Direction vector of segment S1
    Vector3d d2 = q2 - p2; // Direction vector of segment S2
    Vector3d r = p1 - p2;
    double a = d1.dot( d1 ); // Squared length of segment S1, always nonnegative
    double e = d2.dot( d2 ); // Squared length of segment S2, always nonnegative
    double f = d2.dot( r );

    // Check if either or both segments degenerate into points
    if ( a <= EPSILON && e <= EPSILON )
    {
        // Both segments degenerate into points
        s = t = 0.0;
        c1 = p1;
        c2 = p2;
        return ( c1 - c2 ).dot( c1 - c2 );
    }
    if ( a <= EPSILON )
    {
        // First segment degenerates into a point
        s = 0.0;
        t = f / e; // s = 0 => t = (b*s + f) / e = f / e
        t = clamp( t, 0.0, 1.0 );
    }
    else
    {
        double c = d1.dot( r );
        if ( e <= EPSILON )
        {
            // Second segment degenerates into a point
            t = 0.0;
            s = clamp( -c / a, 0.0, 1.0 ); // t = 0 => s = (b*t - c) / a = -c / a
        }
        else
        {
            // The general nondegenerate case starts here
            double b = d1.dot( d2 );
            double denom = a * e - b * b; // Always nonnegative

            // If segments not parallel, compute closest point on L1 to L2, and
            // clamp to segment S1. Else pick arbitrary s (here 0)
            if ( denom != 0.0 )
            {
                s = clamp( ( b * f - c * e ) / denom, 0.0, 1.0 );
            }
            else
                s = 0.0;

            // Compute point on L2 closest to S1(s) using
            // t = Dot((P1+D1*s)-P2,D2) / Dot(D2,D2) = (b*s + f) / e
            t = ( b * s + f ) / e;

            // If t in [0,1] done. Else clamp t, recompute s for the new value
            // of t using s = Dot((P2+D2*t)-P1,D1) / Dot(D1,D1)= (t*b - c) / a
            // and clamp s to [0, 1]
            if ( t < 0.0 )
            {
                t = 0.0;
                s = clamp( -c / a, 0.0, 1.0 );
            }
            else if ( t > 1.0 )
            {
                t = 1.0;
                s = clamp( ( b - c ) / a, 0.0, 1.0 );
            }
        }
    }

    c1 = p1 + d1 * s;
    c2 = p2 + d2 * t;
    return ( c1 - c2 ).dot( c1 - c2 );
}


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

// clang-format off

//[ getting_started_listing_04

#include <algorithm>           // std::max_element
#include <boost/format.hpp>    // only needed for printing
#include <boost/histogram.hpp> // make_histogram, integer, indexed
#include <iostream>            // std::cout, std::endl
#include <sstream>             // std::ostringstream

int main() {
  using namespace boost::histogram;
  using namespace boost::histogram::literals;

  /*
    We make a 3d histogram for color values (r, g, b) in an image. We assume the values
    are of type char. The value range then is [0, 256). The integer axis is perfect for
    color values.
  */
  auto h = make_histogram(
    axis::integer<>(0, 256, "r"),
    axis::integer<>(0, 256, "g"),
    axis::integer<>(0, 256, "b")
  );

  /*
    We don't have real image data, so fill some fake data.
  */
  h(1, 2, 3);
  h(1, 2, 3);
  h(0, 1, 0);

  /*
    Now let's say we want to know which color is most common. We can use std::max_element
    on an indexed range for that.
  */
  auto ind = indexed(h);
  auto max_it = std::max_element(ind.begin(), ind.end());

  /*
    max_it is a special iterator to the histogram cell with the highest count.
    This iterator allows one to access the cell value, bin indices, and bin values.
    You need to twice dereference it to get to the cell value.
  */
  std::ostringstream os;
  os << boost::format("count=%i rgb=(%i, %i, %i)")
        % **max_it % max_it->bin(0_c) % max_it->bin(1) % max_it->bin(2);

  std::cout << os.str() << std::endl;

  assert(os.str() == "count=2 rgb=(1, 2, 3)");
}

//]


#include <sparse.h>

#include <sparse_fill.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(SPARSE);

BOOST_AUTO_TEST_CASE(transpose_crm_test)
{

  // scalar case
  typedef sparse::CompressedRowMatrix< sparse::Block<1,1,float> > scalar_matrix_type;
  scalar_matrix_type sA(1,1,1);
  sparse::fill(sA(0,0));
  scalar_matrix_type sB;
  sparse::transpose(sA,sB);
  BOOST_CHECK(sA.size() == sB.size());
  BOOST_CHECK(sA.ncols() == sB.nrows());
  BOOST_CHECK(sA.nrows() == sB.ncols());
  BOOST_CHECK(sA[0] == sB[0]);
  sA.clear();
  sB.clear();

  sA.resize(5,5,8);
  sparse::fill(sA(0,0));
  sparse::fill(sA(0,4));
  sparse::fill(sA(1,1));
  sparse::fill(sA(1,2));
  sparse::fill(sA(2,3));
  sparse::fill(sA(2,4));
  sparse::fill(sA(4,2));
  sparse::fill(sA(4,4));
  sparse::transpose(sA,sB);

  BOOST_CHECK( sA.size()  == sB.size() );
  BOOST_CHECK( sA.ncols() == sB.nrows());
  BOOST_CHECK( sA.nrows() == sB.ncols());
  BOOST_CHECK( sA(0,0) == sB(0,0));
  BOOST_CHECK( sA(0,4) == sB(4,0));
  BOOST_CHECK( sA(1,1) == sB(1,1));
  BOOST_CHECK( sA(1,2) == sB(2,1));
  BOOST_CHECK( sA(2,3) == sB(3,2));
  BOOST_CHECK( sA(2,4) == sB(4,2));
  BOOST_CHECK( sA(4,2) == sB(2,4));
  BOOST_CHECK( sA(4,4) == sB(4,4));

  // block case
  typedef sparse::Block<3,4,float> block_type;
  typedef sparse::Block<4,3,float> transposed_block_type;

  typedef sparse::CompressedRowMatrix<block_type> matrix_type;
  matrix_type A(1,1,1);
  sparse::fill(A(0,0));

  typedef sparse::CompressedRowMatrix<transposed_block_type> transposed_matrix_type;
  transposed_matrix_type B;

  sparse::transpose(A,B);
  BOOST_CHECK(A.size() == B.size());
  BOOST_CHECK(A.ncols() == B.nrows());
  BOOST_CHECK(A.nrows() == B.ncols());

  transposed_block_type b;
  sparse::transpose(A[0],b);

  BOOST_CHECK(b == B[0]);
  A.clear();
  B.clear();

  A.resize(5,5,8);
  sparse::fill(A(0,0));
  sparse::fill(A(0,4));
  sparse::fill(A(1,1));
  sparse::fill(A(1,2));
  sparse::fill(A(2,3));
  sparse::fill(A(2,4));
  sparse::fill(A(4,2));
  sparse::fill(A(4,4));
  sparse::transpose(A,B);
  BOOST_CHECK(A.size() == B.size());
  BOOST_CHECK(A.ncols() == B.nrows());
  BOOST_CHECK(A.nrows() == B.ncols());

  sparse::transpose(A(0,0), b);
  BOOST_CHECK(b == B(0,0));

  sparse::transpose(A(0,4), b);
  BOOST_CHECK(b == B(4,0));

  sparse::transpose(A(1,1), b);
  BOOST_CHECK(b == B(1,1));

  sparse::transpose(A(1,2), b);
  BOOST_CHECK(b == B(2,1));

  sparse::transpose(A(2,3), b);
  BOOST_CHECK(b == B(3,2));

  sparse::transpose(A(2,4), b);
  BOOST_CHECK(b == B(4,2));

  sparse::transpose(A(4,2), b);
  BOOST_CHECK(b == B(2,4));

  sparse::transpose(A(4,4), b);
  BOOST_CHECK(b == B(4,4));

}

BOOST_AUTO_TEST_SUITE_END();


/**
 * @file VecImage.hpp
 * @author Jorge Agüero Zamora
 * @brief Contains the `VecImage` class used to manipulate images
 * @version 0.1
 * @date 2021-06-13
 *
 */
#ifndef VECIMAGE_H
#define VECIMAGE_H

#define cimg_use_jpeg
#define cimg_use_png
#define cimg_display 0 // !! Important: Disables X11 libs so project can build with QT !!

#include <string>
#include <vector>
#include <armadillo>
#include <math.h>

#include "CImg.h"

namespace AppLogic
{
    using arma::mat;
    using arma::vec;
    using cimg_library::CImg;
    using std::string;
    using std::vector;

    /**
     * @brief enum with the different type of noises that can be applied to a `VecImage`
     *
     */
    typedef enum
    {
        GAUSSIAN,
        SALT_PEPPER,
        UNIFORM,
        POISSON,
        RICIAN
    } noise_type_t;

    /**
     * @brief Class used for image manipulation
     *
     * Allows users to create images, load images from folder, and get the underlying double
     * values of the image in order to calculate filters and apply filters to the images.
     *
     */
    class VecImage
    {
    private:
        string source_file_location; //!< path of the image in the filesystem when it's loaded from file
        size_t cols; //!< number of columns in the image
        size_t rows; //!< number of rows in the image
        cimg_library::CImg<unsigned char> data; //!< the image data

    public:
        /**
         * @brief empty image constructor
         *
         */
        VecImage();

        /**
         * @brief constructs a new `VecImage` by loading it from file
         *
         * Loads an image from file and creates a new image in grayscale.
         * Images must be in JPG or PNG format.
         *
         * @param filename path of the image to load
         */
        VecImage(string filename);

        /**
         * @brief construct a new image from an std::vector containing the image data
         *
         * Constructs a new grayscale image from the `img_data` vector and  `width` and
         * `height` specified.
         *
         * @param img_data vector containig normalized values [0,1] of the pixel data of the image
         * @param width  horizontal size of the image
         * @param height vertical size of the image
         */
        VecImage(vector<double> &img_data, size_t width, size_t height);

        /**
         * @brief construct a new image from an armadillo::vector containing the image data
         *
         * Constructs a new grayscale image from the `img_data` vector and  `width` and
         * `height` specified.
         *
         * @param img_data vector containig normalized values [0,1] of the pixel data of the image
         * @param width  horizontal size of the image
         * @param height vertical size of the image
         */
        VecImage(const vec &img_data, size_t width, size_t height);

        /**
         * @brief constructs `VecImage` from a `CImg` object
         *
         * @param cImg CImg to be used as source for new the image
         */
        VecImage(CImg<unsigned char> &cImg);

        /**
         * @brief copy constructor for `VecImage`
         *
         * @param vImg image to copy data from
         */
        VecImage(const VecImage &vImg);

        /**
         * @brief assignment operator, copies values from one image to the other
         *
         * @param image
         * @return VecImage&
         */
        VecImage& operator= (const VecImage &image);

        /**
         * @brief returns the number of rows (height) of the image
         *
         * @return size_t number of rows (height) of the image
         */
        size_t numRows();

        /**
         * @brief returns the number of columns (width) of the image
         *
         * @return size_t number of columns (width) of the image
         */
        size_t numCols();

        /**
         * @brief returns the path from where the image was loaded
         *
         * If the image was created in the application and not loaded from file
         * this value will be "internal"
         *
         * @return string path from where the image was loaded
         */
        string getSourceFileLocation();

        /**
         * @brief return std::vector with normalized image data
         *
         * @return vector<double> std::vector with normalized image data
         */
        vector<double> getDoubleData();

        /**
         * @brief return arma::vector with normalized image data
         *
         * @return vec arma::vector with normalized image data
         */
        vec getVecDoubleData();

        /**
         * @brief returns an arma::matrix with normalized image data
         *
         * @return mat arma::matrix with normalized image data with same size as image
         */
        mat getMatDouble();

        /**
         * @brief applies noise to an image
         *
         * @param noise_value value representing the percentage of the noise to be applied [1,99]
         * @param noise type of noise to be applied
         */
        void applyNoise(double noise_value, noise_type_t noise = noise_type_t::GAUSSIAN);

        /**
         * @brief set the data values of the image from an arma::matrix
         *
         * @param new_data arma::matrix containing image data
         */
        void setImgDataMatDouble(const mat &new_data);

        /**
         * @brief returns the raw image data
         *
         * @return const unsigned char* image data
         */
        const unsigned char *getRawImageData();

        /**
         * @brief returns a `string` name for each of the different noise types
         *
         * @param noise_type type of noise to get the name
         * @return string name of the noise
         */
        static string getNoiseName(noise_type_t noise_type);

        /**
         * @brief saves an image to file as PNG
         *
         * @param filename full path of the file where the images will be saved
         * @return true if image is saved succesfully
         * @return false if there is a problem saving the image
         */
        bool save(const string filename);
    };
}
#endif


#include <stan/math/prim/arr.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>
#include <vector>

TEST(MathFunctions, sub) {
  std::vector<double> x(3), y(3), result(3);

  x[0] = 1.0;
  x[1] = 2.0;
  x[2] = 3.0;

  y[0] = 0.5;
  y[1] = 1.0;
  y[2] = 4.0;

  EXPECT_NO_THROW(stan::math::sub(x, y, result));
  EXPECT_FLOAT_EQ(0.5, result[0]);
  EXPECT_FLOAT_EQ(1.0, result[1]);
  EXPECT_FLOAT_EQ(-1.0, result[2]);
}

TEST(MathFunctions, sub_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();
  std::vector<double> x(3), y(3), result(3);

  x[0] = 1.0;
  x[1] = 2.0;
  x[2] = nan;

  y[0] = 0.5;
  y[1] = 1.0;
  y[2] = 4.0;

  EXPECT_NO_THROW(stan::math::sub(x, y, result));
  EXPECT_FLOAT_EQ(0.5, result[0]);
  EXPECT_FLOAT_EQ(1.0, result[1]);
  EXPECT_PRED1(boost::math::isnan<double>, result[2]);

  EXPECT_NO_THROW(stan::math::sub(y, x, result));
  EXPECT_FLOAT_EQ(-0.5, result[0]);
  EXPECT_FLOAT_EQ(-1.0, result[1]);
  EXPECT_PRED1(boost::math::isnan<double>, result[2]);

  y[2] = nan;
  EXPECT_NO_THROW(stan::math::sub(x, y, result));
  EXPECT_FLOAT_EQ(0.5, result[0]);
  EXPECT_FLOAT_EQ(1.0, result[1]);
  EXPECT_PRED1(boost::math::isnan<double>, result[2]);
}


//  (C) Copyright Gennadiy Rozental 2001-2006.
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org/libs/test for the library home page.

// Boost.Test
#ifdef BOOST_MSVC
# pragma warning(disable: C4345)
#endif

#include <boost/test/included/unit_test.hpp>
using boost::unit_test::test_suite;

// Boost.MPL
#include <boost/mpl/range_c.hpp>

BOOST_TEST_CASE_TEMPLATE_FUNCTION( free_test_function, Number )
{
    BOOST_CHECK_EQUAL( 2, (int const)Number::value );
}

test_suite*
init_unit_test_suite( int, char* [] ) {
    test_suite* test= BOOST_TEST_SUITE( "Test case template example" );

    typedef boost::mpl::range_c<int,0,10> numbers;

    test->add( BOOST_TEST_CASE_TEMPLATE( free_test_function, numbers ) );

    return test;
}

// EOF


//
// Copyright (c) 2009-2016 The Bitcoin Core developers
// Copyright (c) 2017-2020 Project PAI Foundation
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.
//

#include "chainparams.h"
#include "validation.h"
#include "consensus/tx_verify.h"
#include "net.h"

#include "test/test_paicoin.h"

#include <boost/signals2/signal.hpp>
#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(main_tests, TestingSetup)

static void TestBlockSubsidyHalvings(const Consensus::Params& consensusParams)
{
    int maxHalvings = 64;
    // PAICOIN Note: If the initial block subsidy has been changed,
    // update the subsidy with the correct value
    CAmount nInitialSubsidy = consensusParams.nTotalBlockSubsidy * COIN;

    CAmount nPreviousSubsidy = nInitialSubsidy * 2; // for height == 0
    BOOST_CHECK_EQUAL(nPreviousSubsidy, nInitialSubsidy * 2);
    for (int nHalvings = 0; nHalvings < maxHalvings; nHalvings++) {
        int nHeight = nHalvings * consensusParams.nSubsidyHalvingInterval;
        CAmount nSubsidy = GetTotalBlockSubsidy(nHeight, consensusParams);
        BOOST_CHECK(nSubsidy <= nInitialSubsidy);
        BOOST_CHECK_EQUAL(nSubsidy, nPreviousSubsidy / 2);
        nPreviousSubsidy = nSubsidy;
    }
    BOOST_CHECK_EQUAL(GetTotalBlockSubsidy(maxHalvings * consensusParams.nSubsidyHalvingInterval, consensusParams), 0);
}

static void TestBlockSubsidyHalvings(int nSubsidyHalvingInterval)
{
    Consensus::Params consensusParams;
    consensusParams.nSubsidyHalvingInterval = nSubsidyHalvingInterval;
    TestBlockSubsidyHalvings(consensusParams);
}

BOOST_AUTO_TEST_CASE(block_subsidy_test)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);

    TestBlockSubsidyHalvings(chainParams->GetConsensus()); // As in main
    TestBlockSubsidyHalvings(150); // As in regtest
    TestBlockSubsidyHalvings(1000); // Just another interval

    CAmount contributedAmount = 20;
    CAmount stakedAmount = 1000;
    CAmount subsidy = 50;
    CAmount contributionSum = 1000;
    CAmount reward = CalculateGrossRemuneration(contributedAmount, stakedAmount, subsidy, contributionSum);
    BOOST_CHECK(reward == 21);

    auto nStakeValidationHeight = chainParams->GetConsensus().nStakeValidationHeight;
    CAmount minerSubsidy = GetMinerSubsidy(nStakeValidationHeight-1, chainParams->GetConsensus());
    BOOST_CHECK(minerSubsidy == 1500 * COIN);
    minerSubsidy = GetMinerSubsidy(nStakeValidationHeight, chainParams->GetConsensus());
    BOOST_CHECK(minerSubsidy == 1050 * COIN);

    CAmount voterSubsidy = GetVoterSubsidy(nStakeValidationHeight-1, chainParams->GetConsensus());
    BOOST_CHECK(voterSubsidy == 0);
    voterSubsidy = GetVoterSubsidy(nStakeValidationHeight, chainParams->GetConsensus());
    BOOST_CHECK(voterSubsidy == 90 * COIN);
}

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    CAmount nSum = 0;
    for (int nHeight = 0; nHeight < 14000000; nHeight += 1000) {
        CAmount nSubsidy = GetTotalBlockSubsidy(nHeight, chainParams->GetConsensus());
        // PAICOIN Note: If the initial block subsidy has been changed,
        // update the subsidy with the correct value
        BOOST_CHECK(nSubsidy <= chainParams->GetConsensus().nTotalBlockSubsidy * COIN);
        nSum += nSubsidy * 1000;
        BOOST_CHECK(MoneyRange(nSum));
    }
    // PAICOIN Note: If the initial block subsidy has been changed,
    // update this sum with the correct value
    BOOST_CHECK_EQUAL(nSum, 62999999996850000ULL);
}

bool ReturnFalse() { return false; }
bool ReturnTrue() { return true; }

BOOST_AUTO_TEST_CASE(test_combiner_all)
{
    boost::signals2::signal<bool (), CombinerAll> Test;
    BOOST_CHECK(Test());
    Test.connect(&ReturnFalse);
    BOOST_CHECK(!Test());
    Test.connect(&ReturnTrue);
    BOOST_CHECK(!Test());
    Test.disconnect(&ReturnFalse);
    BOOST_CHECK(Test());
    Test.disconnect(&ReturnTrue);
    BOOST_CHECK(Test());
}
BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright (c) 2020 Johannes Pankert <pankertj@ethz.ch>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name of this work nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#pragma once

#include <Eigen/Core>
#include <cppad/cg.hpp>

#include <perceptive_mpc/kinematics/KinematicsInterface.hpp>
#include "cppad/cg/math.hpp"

namespace perceptive_mpc {

template <typename SCALAR_T>
class asArmKinematics : public KinematicsInterface<SCALAR_T> {
 public:
  using Base = KinematicsInterface<SCALAR_T>;

  asArmKinematics(const KinematicInterfaceConfig& config);
  std::string armMountLinkName() const override { return "xarm_mount"; }
  std::string toolMountLinkName() const override { return "xarmlink6"; }

 protected:
  Eigen::Matrix<SCALAR_T, 4, 4> computeArmMountToToolMountTransform(const Eigen::Matrix<SCALAR_T, 6, 1>& armState) const override;

  Eigen::Matrix<SCALAR_T, 3, -1> computeArmState2MultiplePointsOnRobot(
      const Eigen::Matrix<SCALAR_T, 6, 1>& state, const std::vector<std::vector<double>>& points,
      const Eigen::Matrix4d& transformBase_X_ArmBase = Eigen::Matrix4d::Identity(),
      const Eigen::Matrix4d& transformToolMount_X_Endeffector = Eigen::Matrix4d::Identity(),
      const Eigen::Matrix<SCALAR_T, 4, 4>& transformWorld_X_Base = Eigen::Matrix<SCALAR_T, 4, 4>::Identity()) const override;

  double getArmMass() const override;
  Eigen::Matrix<SCALAR_T, 3, 1> getArmCOM(const Eigen::Matrix<SCALAR_T, 6, 1>& armState) const override;
};

extern template class asArmKinematics<double>;
extern template class asArmKinematics<CppAD::AD<CppAD::cg::CG<double>>>;
}  // namespace perceptive_mpc


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Copyright 2004 The Trustees of Indiana University
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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_GRAPH_SEQUENTIAL_VERTEX_COLORING_HPP
#define BOOST_GRAPH_SEQUENTIAL_VERTEX_COLORING_HPP

#include <vector>
#include <boost/graph/graph_traits.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/limits.hpp>

#ifdef BOOST_NO_TEMPLATED_ITERATOR_CONSTRUCTORS
#  include <iterator>
#endif

/* This algorithm is to find coloring of a graph

   Algorithm:
   Let G = (V,E) be a graph with vertices (somehow) ordered v_1, v_2, ...,
   v_n. For k = 1, 2, ..., n the sequential algorithm assigns v_k to the
   smallest possible color.

   Reference:

   Thomas F. Coleman and Jorge J. More, Estimation of sparse Jacobian
   matrices and graph coloring problems. J. Numer. Anal. V20, P187-209, 1983

   v_k is stored as o[k] here.

   The color of the vertex v will be stored in color[v].
   i.e., vertex v belongs to coloring color[v] */

namespace boost {
  template <class VertexListGraph, class OrderPA, class ColorMap>
  typename property_traits<ColorMap>::value_type
  sequential_vertex_coloring(const VertexListGraph& G, OrderPA order,
                             ColorMap color)
  {
    typedef graph_traits<VertexListGraph> GraphTraits;
    typedef typename GraphTraits::vertex_descriptor Vertex;
    typedef typename property_traits<ColorMap>::value_type size_type;

    size_type max_color = 0;
    const size_type V = num_vertices(G);

    // We need to keep track of which colors are used by
    // adjacent vertices. We do this by marking the colors
    // that are used. The mark array contains the mark
    // for each color. The length of mark is the
    // number of vertices since the maximum possible number of colors
    // is the number of vertices.
    std::vector<size_type> mark(V,
                                std::numeric_limits<size_type>::max BOOST_PREVENT_MACRO_SUBSTITUTION());

    //Initialize colors
    typename GraphTraits::vertex_iterator v, vend;
    for (boost::tie(v, vend) = vertices(G); v != vend; ++v)
      put(color, *v, V-1);

    //Determine the color for every vertex one by one
    for ( size_type i = 0; i < V; i++) {
      Vertex current = get(order,i);
      typename GraphTraits::adjacency_iterator v, vend;

      //Mark the colors of vertices adjacent to current.
      //i can be the value for marking since i increases successively
      for (boost::tie(v,vend) = adjacent_vertices(current, G); v != vend; ++v)
        mark[get(color,*v)] = i;

      //Next step is to assign the smallest un-marked color
      //to the current vertex.
      size_type j = 0;

      //Scan through all useable colors, find the smallest possible
      //color that is not used by neighbors.  Note that if mark[j]
      //is equal to i, color j is used by one of the current vertex's
      //neighbors.
      while ( j < max_color && mark[j] == i )
        ++j;

      if ( j == max_color )  //All colors are used up. Add one more color
        ++max_color;

      //At this point, j is the smallest possible color
      put(color, current, j);  //Save the color of vertex current
    }

    return max_color;
  }

  template<class VertexListGraph, class ColorMap>
  typename property_traits<ColorMap>::value_type
  sequential_vertex_coloring(const VertexListGraph& G, ColorMap color)
  {
    typedef typename graph_traits<VertexListGraph>::vertex_descriptor
      vertex_descriptor;
    typedef typename graph_traits<VertexListGraph>::vertex_iterator
      vertex_iterator;

    std::pair<vertex_iterator, vertex_iterator> v = vertices(G);
#ifndef BOOST_NO_TEMPLATED_ITERATOR_CONSTRUCTORS
    std::vector<vertex_descriptor> order(v.first, v.second);
#else
    std::vector<vertex_descriptor> order;
    order.reserve(std::distance(v.first, v.second));
    while (v.first != v.second) order.push_back(*v.first++);
#endif
    return sequential_vertex_coloring
             (G,
              make_iterator_property_map
              (order.begin(), identity_property_map(),
               graph_traits<VertexListGraph>::null_vertex()),
              color);
  }
}

#endif


//---------------------------------------------------------------------------//
//!
//! \file   tstPowerDistribution.cpp
//! \author Alex Robinson
//! \brief  Power distribution unit tests.
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <iostream>

// Boost Includes
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/cgs.hpp>
#include <boost/units/io.hpp>

// Trilinos Includes
#include <Teuchos_UnitTestHarness.hpp>
#include <Teuchos_RCP.hpp>
#include <Teuchos_Array.hpp>
#include <Teuchos_ParameterList.hpp>
#include <Teuchos_XMLParameterListCoreHelpers.hpp>
#include <Teuchos_VerboseObject.hpp>

// FRENSIE Includes
#include "Utility_UnitTestHarnessExtensions.hpp"
#include "Utility_OneDDistribution.hpp"
#include "Utility_PowerDistribution.hpp"
#include "Utility_RandomNumberGenerator.hpp"
#include "Utility_PhysicalConstants.hpp"
#include "Utility_UnitTraits.hpp"
#include "Utility_QuantityTraits.hpp"

using boost::units::quantity;
namespace si = boost::units::si;
namespace cgs = boost::units::cgs;

//---------------------------------------------------------------------------//
// Testing Variables
//---------------------------------------------------------------------------//

Teuchos::RCP<Teuchos::ParameterList> test_dists_list;

Teuchos::RCP<Utility::OneDDistribution> distribution;

Teuchos::RCP<Utility::UnitAwareOneDDistribution<cgs::length,si::amount> >
  unit_aware_distribution;

//---------------------------------------------------------------------------//
// Instantiation Macros.
//---------------------------------------------------------------------------//
#define UNIT_TEST_INSTANTIATION( type, name )			\
  TEUCHOS_UNIT_TEST_TEMPLATE_1_INSTANT( type, name, 1u )	\
  TEUCHOS_UNIT_TEST_TEMPLATE_1_INSTANT( type, name, 2u )	\
  TEUCHOS_UNIT_TEST_TEMPLATE_1_INSTANT( type, name, 3u )	\
  TEUCHOS_UNIT_TEST_TEMPLATE_1_INSTANT( type, name, 4u )	\
  TEUCHOS_UNIT_TEST_TEMPLATE_1_INSTANT( type, name, 5u )	\
  TEUCHOS_UNIT_TEST_TEMPLATE_1_INSTANT( type, name, 6u )	\
  TEUCHOS_UNIT_TEST_TEMPLATE_1_INSTANT( type, name, 7u )	\
  TEUCHOS_UNIT_TEST_TEMPLATE_1_INSTANT( type, name, 8u )	\
  TEUCHOS_UNIT_TEST_TEMPLATE_1_INSTANT( type, name, 9u )	\
  TEUCHOS_UNIT_TEST_TEMPLATE_1_INSTANT( type, name, 10u )

//---------------------------------------------------------------------------//
// Testing Functions.
//---------------------------------------------------------------------------//
// Initialize the distribution
template<unsigned N, typename IndepUnit, typename DepUnit>
void initializeDistribution(
	  Teuchos::RCP<Utility::UnitAwareOneDDistribution<IndepUnit,DepUnit> >&
	  distribution )
{
  typedef typename Utility::UnitAwareOneDDistribution<IndepUnit,DepUnit>::IndepQuantity IndepQuantity;
  distribution.reset(
     new Utility::UnitAwarePowerDistribution<N,IndepUnit,DepUnit>(
	   (N+1.0),
	   Utility::QuantityTraits<IndepQuantity>::initializeQuantity( 0.0 ),
	   Utility::QuantityTraits<IndepQuantity>::initializeQuantity( 1.0 )));
}

//---------------------------------------------------------------------------//
// Tests.
//---------------------------------------------------------------------------//
// Check that the distribution can be evaluated
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( PowerDistribution, evaluate, N )
{
  initializeDistribution<N,void,void>( distribution );

  TEST_EQUALITY_CONST( distribution->evaluate( -1.0 ), 0.0 );
  TEST_EQUALITY_CONST( distribution->evaluate( 0.0 ), 0.0 );
  TEST_EQUALITY_CONST( distribution->evaluate( 1.0 ), N + 1.0 );
  TEST_EQUALITY_CONST( distribution->evaluate( 2.0 ), 0.0 );
}

UNIT_TEST_INSTANTIATION( PowerDistribution, evaluate );

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution can be evaluated
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( UnitAwarePowerDistribution,
					    evaluate,
					    N )
{
  initializeDistribution<N,cgs::length,si::amount>( unit_aware_distribution );

  TEST_EQUALITY_CONST( unit_aware_distribution->evaluate(-1.0*cgs::centimeter),
		       0.0*si::mole );
  TEST_EQUALITY_CONST( unit_aware_distribution->evaluate( 0.0*cgs::centimeter),
		       0.0*si::mole );
  TEST_EQUALITY_CONST( unit_aware_distribution->evaluate( 1.0*cgs::centimeter),
		       (N + 1.0)*si::mole );
  TEST_EQUALITY_CONST( unit_aware_distribution->evaluate( 2.0*cgs::centimeter),
		       0.0*si::mole );
}

UNIT_TEST_INSTANTIATION( UnitAwarePowerDistribution, evaluate );

//---------------------------------------------------------------------------//
// Check that the PDF can be evaluated
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( PowerDistribution, evaluatePDF, N )
{
  initializeDistribution<N,void,void>( distribution );

  TEST_EQUALITY_CONST( distribution->evaluatePDF( -1.0 ), 0.0 );
  TEST_EQUALITY_CONST( distribution->evaluatePDF( 0.0 ), 0.0 );
  TEST_EQUALITY_CONST( distribution->evaluatePDF( 1.0 ), N+1.0 );
  TEST_EQUALITY_CONST( distribution->evaluatePDF( 2.0 ), 0.0 );
}

UNIT_TEST_INSTANTIATION( PowerDistribution, evaluatePDF );

//---------------------------------------------------------------------------//
// Check that the unit-aware PDF can be evaluated
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( UnitAwarePowerDistribution,
					    evaluatePDF,
					    N )
{
  initializeDistribution<N,cgs::length,si::amount>( unit_aware_distribution );

  TEST_EQUALITY_CONST(
	          unit_aware_distribution->evaluatePDF( -1.0*cgs::centimeter ),
		  0.0/cgs::centimeter );
  TEST_EQUALITY_CONST(
		   unit_aware_distribution->evaluatePDF( 0.0*cgs::centimeter ),
		   0.0/cgs::centimeter );
  TEST_EQUALITY_CONST(
		   unit_aware_distribution->evaluatePDF( 1.0*cgs::centimeter ),
		   (N+1.0)/cgs::centimeter );
  TEST_EQUALITY_CONST(
		   unit_aware_distribution->evaluatePDF( 2.0*cgs::centimeter ),
		   0.0/cgs::centimeter );
}

UNIT_TEST_INSTANTIATION( UnitAwarePowerDistribution, evaluatePDF );

//---------------------------------------------------------------------------//
// Check that the distribution can be sampled
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( PowerDistribution, sample, N )
{
  initializeDistribution<N,void,void>( distribution );

  std::vector<double> fake_stream( 3 );
  fake_stream[0] = 0.0;
  fake_stream[1] = 0.5;
  fake_stream[2] = 1.0 - 1e-15;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  double sample = distribution->sample();
  TEST_EQUALITY_CONST( sample, 0.0 );

  sample = distribution->sample();
  TEST_EQUALITY_CONST( sample, pow(0.5, 1.0/(N+1u)) );

  sample = distribution->sample();
  TEST_FLOATING_EQUALITY( sample, 1.0, 1e-15 );

  Utility::RandomNumberGenerator::unsetFakeStream();
}

UNIT_TEST_INSTANTIATION( PowerDistribution, sample );

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution can be sampled
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( UnitAwarePowerDistribution,
					    sample,
					    N )
{
  initializeDistribution<N,cgs::length,si::amount>( unit_aware_distribution );

  std::vector<double> fake_stream( 3 );
  fake_stream[0] = 0.0;
  fake_stream[1] = 0.5;
  fake_stream[2] = 1.0 - 1e-15;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  quantity<cgs::length> sample = unit_aware_distribution->sample();
  TEST_EQUALITY_CONST( sample, 0.0*cgs::centimeter );

  sample = unit_aware_distribution->sample();
  TEST_EQUALITY_CONST( sample, pow(0.5, 1.0/(N+1u))*cgs::centimeter );

  sample = unit_aware_distribution->sample();
  UTILITY_TEST_FLOATING_EQUALITY( sample, 1.0*cgs::centimeter, 1e-15 );

  Utility::RandomNumberGenerator::unsetFakeStream();
}

UNIT_TEST_INSTANTIATION( UnitAwarePowerDistribution, sample );

//---------------------------------------------------------------------------//
// Check that the distribution can be sampled
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( PowerDistribution,
					    sample_static,
					    N )
{
  std::vector<double> fake_stream( 3 );
  fake_stream[0] = 0.0;
  fake_stream[1] = 0.5;
  fake_stream[2] = 1.0 - 1e-15;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  double sample = Utility::PowerDistribution<N>::sample( 0.0, 1.0 );
  TEST_EQUALITY_CONST( sample, 0.0 );

  sample = Utility::PowerDistribution<N>::sample( 0.0, 1.0 );
  TEST_EQUALITY_CONST( sample, pow(0.5, 1.0/(N+1u)) );

  sample = Utility::PowerDistribution<N>::sample( 0.0, 1.0 );
  TEST_FLOATING_EQUALITY( sample, 1.0, 1e-15 );

  Utility::RandomNumberGenerator::unsetFakeStream();
}

UNIT_TEST_INSTANTIATION( PowerDistribution, sample_static );

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution can be sampled
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( UnitAwarePowerDistribution,
					    sample_static,
					    N )
{
  std::vector<double> fake_stream( 3 );
  fake_stream[0] = 0.0;
  fake_stream[1] = 0.5;
  fake_stream[2] = 1.0 - 1e-15;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  quantity<cgs::length> sample =
    Utility::UnitAwarePowerDistribution<N,cgs::length>::sample(
							 0.0*cgs::centimeter,
						         1.0*cgs::centimeter );
  TEST_EQUALITY_CONST( sample, 0.0*cgs::centimeter );

  sample =
    Utility::UnitAwarePowerDistribution<N,cgs::length>::sample(
							 0.0*cgs::centimeter,
						         1.0*cgs::centimeter );
  TEST_EQUALITY_CONST( sample, pow(0.5, 1.0/(N+1u))*cgs::centimeter );

  sample =
    Utility::UnitAwarePowerDistribution<N,cgs::length>::sample(
							 0.0*cgs::centimeter,
						         1.0*cgs::centimeter );
  UTILITY_TEST_FLOATING_EQUALITY( sample, 1.0*cgs::centimeter, 1e-15 );

  Utility::RandomNumberGenerator::unsetFakeStream();
}

UNIT_TEST_INSTANTIATION( UnitAwarePowerDistribution, sample_static );

//---------------------------------------------------------------------------//
// Check that the distribution can be sampled
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( PowerDistribution,
					    sampleAndRecordTrials,
					    N )
{
  initializeDistribution<N,void,void>( distribution );

  std::vector<double> fake_stream( 3 );
  fake_stream[0] = 0.0;
  fake_stream[1] = 0.5;
  fake_stream[2] = 1.0 - 1e-15;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  unsigned trials = 0;

  double sample = distribution->sampleAndRecordTrials( trials );
  TEST_EQUALITY_CONST( sample, 0.0 );
  TEST_EQUALITY_CONST( 1.0/trials, 1.0 );

  sample = distribution->sampleAndRecordTrials( trials );
  TEST_EQUALITY_CONST( sample, pow(0.5, 1.0/(N+1u)) );
  TEST_EQUALITY_CONST( 2.0/trials, 1.0 );

  sample = distribution->sampleAndRecordTrials( trials );
  TEST_FLOATING_EQUALITY( sample, 1.0, 1e-15 );
  TEST_EQUALITY_CONST( 3.0/trials, 1.0 );

  Utility::RandomNumberGenerator::unsetFakeStream();
}

UNIT_TEST_INSTANTIATION( PowerDistribution, sampleAndRecordTrials );

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution can be sampled
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( UnitAwarePowerDistribution,
					    sampleAndRecordTrials,
					    N )
{
  initializeDistribution<N,cgs::length,si::amount>( unit_aware_distribution );

  std::vector<double> fake_stream( 3 );
  fake_stream[0] = 0.0;
  fake_stream[1] = 0.5;
  fake_stream[2] = 1.0 - 1e-15;

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  unsigned trials = 0;

  quantity<cgs::length> sample =
    unit_aware_distribution->sampleAndRecordTrials( trials );
  TEST_EQUALITY_CONST( sample, 0.0*cgs::centimeter );
  TEST_EQUALITY_CONST( 1.0/trials, 1.0 );

  sample = unit_aware_distribution->sampleAndRecordTrials( trials );
  TEST_EQUALITY_CONST( sample, pow(0.5, 1.0/(N+1u))*cgs::centimeter );
  TEST_EQUALITY_CONST( 2.0/trials, 1.0 );

  sample = unit_aware_distribution->sampleAndRecordTrials( trials );
  UTILITY_TEST_FLOATING_EQUALITY( sample, 1.0*cgs::centimeter, 1e-15 );
  TEST_EQUALITY_CONST( 3.0/trials, 1.0 );

  Utility::RandomNumberGenerator::unsetFakeStream();
}

UNIT_TEST_INSTANTIATION( UnitAwarePowerDistribution, sampleAndRecordTrials );

//---------------------------------------------------------------------------//
// Check that the upper bound of the distribution independent variable can be
// returned
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( PowerDistribution,
					    getUpperBoundOfIndepVar,
					    N )
{
  initializeDistribution<N,void,void>( distribution );

  TEST_EQUALITY_CONST( distribution->getUpperBoundOfIndepVar(), 1.0 );
}

UNIT_TEST_INSTANTIATION( PowerDistribution, getUpperBoundOfIndepVar );

//---------------------------------------------------------------------------//
// Check that the upper bound of the unit-aware distribution independent
// variable can be returned
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( UnitAwarePowerDistribution,
					    getUpperBoundOfIndepVar,
					    N )
{
  initializeDistribution<N,cgs::length,si::amount>( unit_aware_distribution );

  TEST_EQUALITY_CONST( unit_aware_distribution->getUpperBoundOfIndepVar(),
		       1.0*cgs::centimeter );
}

UNIT_TEST_INSTANTIATION( UnitAwarePowerDistribution, getUpperBoundOfIndepVar );

//---------------------------------------------------------------------------//
// Check that the lower bound of the distribution independent variable can be
// returned
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( PowerDistribution,
					    getLowerBoundOfIndepVar,
					    N )
{
  initializeDistribution<N,void,void>( distribution );

  TEST_EQUALITY_CONST( distribution->getLowerBoundOfIndepVar(), 0.0 );
}

UNIT_TEST_INSTANTIATION( PowerDistribution, getLowerBoundOfIndepVar );

//---------------------------------------------------------------------------//
// Check that the lower bound of the unit-aware distribution independent
// variable can be returned
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( UnitAwarePowerDistribution,
					    getLowerBoundOfIndepVar,
					    N )
{
  initializeDistribution<N,cgs::length,si::amount>( unit_aware_distribution );

  TEST_EQUALITY_CONST( unit_aware_distribution->getLowerBoundOfIndepVar(),
		       0.0*cgs::centimeter );
}

UNIT_TEST_INSTANTIATION( UnitAwarePowerDistribution, getLowerBoundOfIndepVar );

//---------------------------------------------------------------------------//
// Check that the distribution type can be returned
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( PowerDistribution,
					    getDistributionType,
					    N )
{
  initializeDistribution<N,void,void>( distribution );

  switch( N )
  {
  case 1u:
    TEST_EQUALITY_CONST( distribution->getDistributionType(),
			 Utility::POWER_1_DISTRIBUTION );
    break;
  case 2u:
    TEST_EQUALITY_CONST( distribution->getDistributionType(),
			 Utility::POWER_2_DISTRIBUTION );
    break;
  default:
    TEST_EQUALITY_CONST( distribution->getDistributionType(),
			 Utility::POWER_N_DISTRIBUTION );
    break;
  }
}

UNIT_TEST_INSTANTIATION( PowerDistribution, getDistributionType );

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution type can be returned
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( UnitAwarePowerDistribution,
					    getDistributionType,
					    N )
{
  initializeDistribution<N,cgs::length,si::amount>( unit_aware_distribution );

  switch( N )
  {
  case 1u:
    TEST_EQUALITY_CONST( unit_aware_distribution->getDistributionType(),
			 Utility::POWER_1_DISTRIBUTION );
    break;
  case 2u:
    TEST_EQUALITY_CONST( unit_aware_distribution->getDistributionType(),
			 Utility::POWER_2_DISTRIBUTION );
    break;
  default:
    TEST_EQUALITY_CONST( unit_aware_distribution->getDistributionType(),
			 Utility::POWER_N_DISTRIBUTION );
    break;
  }
}

UNIT_TEST_INSTANTIATION( UnitAwarePowerDistribution, getDistributionType );

//---------------------------------------------------------------------------//
// Check if the distribution is tabular
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( PowerDistribution,
					    isTabular,
					    N )
{
  initializeDistribution<N,void,void>( distribution );

  TEST_ASSERT( !distribution->isTabular() );
}

UNIT_TEST_INSTANTIATION( PowerDistribution, isTabular );

//---------------------------------------------------------------------------//
// Check if the unit-aware distribution is tabular
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( UnitAwarePowerDistribution,
					    isTabular,
					    N )
{
  initializeDistribution<N,cgs::length,si::amount>( unit_aware_distribution );

  TEST_ASSERT( !unit_aware_distribution->isTabular() );
}

UNIT_TEST_INSTANTIATION( UnitAwarePowerDistribution, isTabular );

//---------------------------------------------------------------------------//
// Check if the distribution is continuous
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( PowerDistribution,
					    isContinuous,
					    N )
{
  initializeDistribution<N,void,void>( distribution );

  TEST_ASSERT( distribution->isContinuous() );
}

UNIT_TEST_INSTANTIATION( PowerDistribution, isContinuous );

//---------------------------------------------------------------------------//
// Check if the unit-aware distribution is continuous
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( UnitAwarePowerDistribution,
					    isContinuous,
					    N )
{
  initializeDistribution<N,cgs::length,si::amount>( unit_aware_distribution );

  TEST_ASSERT( unit_aware_distribution->isContinuous() );
}

UNIT_TEST_INSTANTIATION( UnitAwarePowerDistribution, isContinuous );

//---------------------------------------------------------------------------//
// Check that the distribution can be written to an xml file
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( PowerDistribution,
					    toParameterList,
					    N )
{
  initializeDistribution<N,void,void>( distribution );

  Teuchos::RCP<Utility::PowerDistribution<N> > true_distribution =
    Teuchos::rcp_dynamic_cast<Utility::PowerDistribution<N> >( distribution );

  Teuchos::ParameterList parameter_list;

  parameter_list.set<Utility::PowerDistribution<N> >( "test distribution",
						      *true_distribution );

  std::ostringstream xml_file_name;
  xml_file_name << "power_" << N << "_dist_test_list.xml";

  Teuchos::writeParameterListToXmlFile( parameter_list,
					xml_file_name.str() );

  Teuchos::RCP<Teuchos::ParameterList> read_parameter_list =
    Teuchos::getParametersFromXmlFile( xml_file_name.str() );

  TEST_EQUALITY( parameter_list, *read_parameter_list );

  Teuchos::RCP<Utility::PowerDistribution<N> >
    copy_distribution( new Utility::PowerDistribution<N> );

  *copy_distribution =
    read_parameter_list->get<Utility::PowerDistribution<N> >(
							  "test distribution");

  TEST_EQUALITY( *copy_distribution, *true_distribution );
}

UNIT_TEST_INSTANTIATION( PowerDistribution, toParameterList );

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution can be written to an xml file
UTILITY_UNIT_TEST_UNSIGNED_TEMPLATE_1_DECL( UnitAwarePowerDistribution,
					    toParameterList,
					    N )
{
  initializeDistribution<N,cgs::length,si::amount>( unit_aware_distribution );

  typedef Utility::UnitAwarePowerDistribution<N,cgs::length,si::amount> UnitAwarePowerDistribution;

  Teuchos::RCP<UnitAwarePowerDistribution> true_distribution =
    Teuchos::rcp_dynamic_cast<UnitAwarePowerDistribution>( unit_aware_distribution );

  Teuchos::ParameterList parameter_list;

  parameter_list.set<UnitAwarePowerDistribution>( "test unit-aware distribution",
						  *true_distribution );

  std::ostringstream xml_file_name;
  xml_file_name << "unit_aware_power_" << N << "_dist_test_list.xml";

  Teuchos::writeParameterListToXmlFile( parameter_list,
					xml_file_name.str() );

  Teuchos::RCP<Teuchos::ParameterList> read_parameter_list =
    Teuchos::getParametersFromXmlFile( xml_file_name.str() );

  TEST_EQUALITY( parameter_list, *read_parameter_list );

  Teuchos::RCP<UnitAwarePowerDistribution>
    copy_distribution( new UnitAwarePowerDistribution );

  *copy_distribution =
    read_parameter_list->get<UnitAwarePowerDistribution>(
					      "test unit-aware distribution" );

  TEST_EQUALITY( *copy_distribution, *true_distribution );
}

UNIT_TEST_INSTANTIATION( UnitAwarePowerDistribution, toParameterList );

//---------------------------------------------------------------------------//
// Check that the distribution can be read from an xml file
TEUCHOS_UNIT_TEST( PowerDistribution, fromParameterList )
{
  Utility::PowerDistribution<1u> distribution_1 =
    test_dists_list->get<Utility::PowerDistribution<1u> >( "Power Distribution A" );

  TEST_EQUALITY_CONST( distribution_1.getLowerBoundOfIndepVar(), 0 );
  TEST_EQUALITY_CONST( distribution_1.getUpperBoundOfIndepVar(), 1 );
  TEST_EQUALITY_CONST( distribution_1.evaluate( 1.0 ), 2.0 );

  distribution_1 =
    test_dists_list->get<Utility::PowerDistribution<1u> >( "Power Distribution B" );

  TEST_EQUALITY_CONST( distribution_1.getLowerBoundOfIndepVar(), 0 );
  TEST_EQUALITY_CONST( distribution_1.getUpperBoundOfIndepVar(),
		       Utility::PhysicalConstants::pi/2 );
  TEST_EQUALITY_CONST( distribution_1.evaluate( 1.0 ), 1.0 );

  Utility::PowerDistribution<2u> distribution_2 =
    test_dists_list->get<Utility::PowerDistribution<2u> >( "Power Distribution C" );

  TEST_EQUALITY_CONST( distribution_2.getLowerBoundOfIndepVar(), 0 );
  TEST_EQUALITY_CONST( distribution_2.getUpperBoundOfIndepVar(),
		       2*Utility::PhysicalConstants::pi );
  TEST_EQUALITY_CONST( distribution_2.evaluate( 1.0 ), 3.0 );

  distribution_2 =
    test_dists_list->get<Utility::PowerDistribution<2u> >( "Power Distribution D" );

  TEST_EQUALITY_CONST( distribution_2.getLowerBoundOfIndepVar(), 0 );
  TEST_EQUALITY_CONST( distribution_2.getUpperBoundOfIndepVar(),
		       3*Utility::PhysicalConstants::pi/4 );
  TEST_EQUALITY_CONST( distribution_2.evaluate( 0.5 ), 0.25 );

  Utility::PowerDistribution<3u> distribution_3 =
    test_dists_list->get<Utility::PowerDistribution<3u> >( "Power Distribution E" );

  TEST_EQUALITY_CONST( distribution_3.getLowerBoundOfIndepVar(), 0 );
  TEST_EQUALITY_CONST( distribution_3.getUpperBoundOfIndepVar(),
		       Utility::PhysicalConstants::pi );
  TEST_EQUALITY_CONST( distribution_3.evaluate( 1.0 ), 4.0 );

  distribution_3 =
    test_dists_list->get<Utility::PowerDistribution<3u> >( "Power Distribution F" );

  TEST_EQUALITY_CONST( distribution_3.getLowerBoundOfIndepVar(), 0 );
  TEST_EQUALITY_CONST( distribution_3.getUpperBoundOfIndepVar(),
		       5*Utility::PhysicalConstants::pi/4 );
  TEST_EQUALITY_CONST( distribution_3.evaluate( 1.0 ), 1.0 );
}

template<unsigned N> using UnitAwarePowerDistribution =
    Utility::UnitAwarePowerDistribution<N,cgs::length,si::amount>;

//---------------------------------------------------------------------------//
// Check that the unit-aware distribution can be read from an xml file
TEUCHOS_UNIT_TEST( UnitAwarePowerDistribution, fromParameterList )
{
  UnitAwarePowerDistribution<1u> distribution_1 =
    test_dists_list->get<UnitAwarePowerDistribution<1u> >( "Unit-Aware Power Distribution A" );

  TEST_EQUALITY_CONST( distribution_1.getLowerBoundOfIndepVar(),
		       0.0*cgs::centimeter );
  TEST_EQUALITY_CONST( distribution_1.getUpperBoundOfIndepVar(),
		       1.0*cgs::centimeter );
  TEST_EQUALITY_CONST( distribution_1.evaluate( 1.0*cgs::centimeter ),
		       2.0*si::mole );

  distribution_1 =
    test_dists_list->get<UnitAwarePowerDistribution<1u> >( "Unit-Aware Power Distribution B" );

  TEST_EQUALITY_CONST( distribution_1.getLowerBoundOfIndepVar(),
		       0.0*cgs::centimeter );
  TEST_EQUALITY_CONST( distribution_1.getUpperBoundOfIndepVar(),
  		       Utility::PhysicalConstants::pi/2*cgs::centimeter );
  TEST_EQUALITY_CONST( distribution_1.evaluate( 1.0*cgs::centimeter ),
		       1.0*si::mole );

  UnitAwarePowerDistribution<2u> distribution_2 =
    test_dists_list->get<UnitAwarePowerDistribution<2u> >( "Unit-Aware Power Distribution C" );

  TEST_EQUALITY_CONST( distribution_2.getLowerBoundOfIndepVar(),
		       0.0*cgs::centimeter );
  TEST_EQUALITY_CONST( distribution_2.getUpperBoundOfIndepVar(),
  		       2*Utility::PhysicalConstants::pi*cgs::centimeter );
  TEST_EQUALITY_CONST( distribution_2.evaluate( 1.0*cgs::centimeter ),
		       3.0*si::mole );

  distribution_2 =
    test_dists_list->get<UnitAwarePowerDistribution<2u> >( "Unit-Aware Power Distribution D" );

  TEST_EQUALITY_CONST( distribution_2.getLowerBoundOfIndepVar(),
		       0.0*cgs::centimeter );
  TEST_EQUALITY_CONST( distribution_2.getUpperBoundOfIndepVar(),
  		       3*Utility::PhysicalConstants::pi/4*cgs::centimeter );
  TEST_EQUALITY_CONST( distribution_2.evaluate( 0.5*cgs::centimeter ),
		       0.25*si::mole );

  UnitAwarePowerDistribution<3u> distribution_3 =
    test_dists_list->get<UnitAwarePowerDistribution<3u> >( "Unit-Aware Power Distribution E" );

  TEST_EQUALITY_CONST( distribution_3.getLowerBoundOfIndepVar(),
		       0.0*cgs::centimeter );
  TEST_EQUALITY_CONST( distribution_3.getUpperBoundOfIndepVar(),
  		       Utility::PhysicalConstants::pi*cgs::centimeter );
  TEST_EQUALITY_CONST( distribution_3.evaluate( 1.0*cgs::centimeter ),
		       4.0*si::mole );

  distribution_3 =
    test_dists_list->get<UnitAwarePowerDistribution<3u> >( "Unit-Aware Power Distribution F" );

  TEST_EQUALITY_CONST( distribution_3.getLowerBoundOfIndepVar(),
		       0.0*cgs::centimeter );
  TEST_EQUALITY_CONST( distribution_3.getUpperBoundOfIndepVar(),
  		       5*Utility::PhysicalConstants::pi/4*cgs::centimeter );
  TEST_EQUALITY_CONST( distribution_3.evaluate( 1.0*cgs::centimeter ),
		       1.0*si::mole );
}

//---------------------------------------------------------------------------//
// Check that distributions can be scaled
TEUCHOS_UNIT_TEST_TEMPLATE_4_DECL( UnitAwarePowerDistribution,
				   explicit_conversion_1,
				   IndepUnitA,
				   DepUnitA,
				   IndepUnitB,
				   DepUnitB )
{
  typedef typename Utility::UnitTraits<IndepUnitA>::template GetQuantityType<double>::type IndepQuantityA;
  typedef typename Utility::UnitTraits<typename Utility::UnitTraits<IndepUnitA>::InverseUnit>::template GetQuantityType<double>::type InverseIndepQuantityA;

  typedef typename Utility::UnitTraits<IndepUnitB>::template GetQuantityType<double>::type IndepQuantityB;
  typedef typename Utility::UnitTraits<typename Utility::UnitTraits<IndepUnitB>::InverseUnit>::template GetQuantityType<double>::type InverseIndepQuantityB;

  typedef typename Utility::UnitTraits<DepUnitA>::template GetQuantityType<double>::type DepQuantityA;
  typedef typename Utility::UnitTraits<DepUnitB>::template GetQuantityType<double>::type DepQuantityB;

  initializeDistribution<1,void,void>( distribution );

  // Copy from unitless distribution to distribution type A
  Utility::UnitAwarePowerDistribution<1,IndepUnitA,DepUnitA>
    unit_aware_dist_a_copy = Utility::UnitAwarePowerDistribution<1,IndepUnitA,DepUnitA>::fromUnitlessDistribution( *Teuchos::rcp_dynamic_cast<Utility::PowerDistribution<1> >( distribution ) );

  // Copy from distribution type A to distribution type B
  Utility::UnitAwarePowerDistribution<1,IndepUnitB,DepUnitB>
    unit_aware_dist_b_copy( unit_aware_dist_a_copy );

  IndepQuantityA indep_quantity_a =
    Utility::QuantityTraits<IndepQuantityA>::initializeQuantity( 0.0 );
  InverseIndepQuantityA inv_indep_quantity_a =
    Utility::QuantityTraits<InverseIndepQuantityA>::initializeQuantity( 0.0 );
  DepQuantityA dep_quantity_a =
    Utility::QuantityTraits<DepQuantityA>::initializeQuantity( 0.0 );

  IndepQuantityB indep_quantity_b( indep_quantity_a );
  InverseIndepQuantityB inv_indep_quantity_b( inv_indep_quantity_a );
  DepQuantityB dep_quantity_b( dep_quantity_a );

  UTILITY_TEST_FLOATING_EQUALITY(
			   unit_aware_dist_a_copy.evaluate( indep_quantity_a ),
			   dep_quantity_a,
			   1e-15 );
  UTILITY_TEST_FLOATING_EQUALITY(
			unit_aware_dist_a_copy.evaluatePDF( indep_quantity_a ),
			inv_indep_quantity_a,
			1e-15 );
  UTILITY_TEST_FLOATING_EQUALITY(
			   unit_aware_dist_b_copy.evaluate( indep_quantity_b ),
			   dep_quantity_b,
			   1e-15 );
  UTILITY_TEST_FLOATING_EQUALITY(
			unit_aware_dist_b_copy.evaluatePDF( indep_quantity_b ),
			inv_indep_quantity_b,
			1e-15 );

  Utility::setQuantity( indep_quantity_a, 1.0 );
  Utility::setQuantity( inv_indep_quantity_a, 2.0 );
  Utility::setQuantity( dep_quantity_a, 2.0 );

  indep_quantity_b = IndepQuantityB( indep_quantity_a );
  inv_indep_quantity_b = InverseIndepQuantityB( inv_indep_quantity_a );
  dep_quantity_b = DepQuantityB( dep_quantity_a );

  UTILITY_TEST_FLOATING_EQUALITY(
			   unit_aware_dist_a_copy.evaluate( indep_quantity_a ),
			   dep_quantity_a,
			   1e-15 );
  UTILITY_TEST_FLOATING_EQUALITY(
			unit_aware_dist_a_copy.evaluatePDF( indep_quantity_a ),
			inv_indep_quantity_a,
			1e-15 );
  UTILITY_TEST_FLOATING_EQUALITY(
			   unit_aware_dist_b_copy.evaluate( indep_quantity_b ),
			   dep_quantity_b,
			   1e-15 );
  UTILITY_TEST_FLOATING_EQUALITY(
			unit_aware_dist_b_copy.evaluatePDF( indep_quantity_b ),
			inv_indep_quantity_b,
			1e-15 );
}

//---------------------------------------------------------------------------//
// Check that distributions can be scaled
TEUCHOS_UNIT_TEST_TEMPLATE_4_DECL( UnitAwarePowerDistribution,
				   explicit_conversion_2,
				   IndepUnitA,
				   DepUnitA,
				   IndepUnitB,
				   DepUnitB )
{
  typedef typename Utility::UnitTraits<IndepUnitA>::template GetQuantityType<double>::type IndepQuantityA;
  typedef typename Utility::UnitTraits<typename Utility::UnitTraits<IndepUnitA>::InverseUnit>::template GetQuantityType<double>::type InverseIndepQuantityA;

  typedef typename Utility::UnitTraits<IndepUnitB>::template GetQuantityType<double>::type IndepQuantityB;
  typedef typename Utility::UnitTraits<typename Utility::UnitTraits<IndepUnitB>::InverseUnit>::template GetQuantityType<double>::type InverseIndepQuantityB;

  typedef typename Utility::UnitTraits<DepUnitA>::template GetQuantityType<double>::type DepQuantityA;
  typedef typename Utility::UnitTraits<DepUnitB>::template GetQuantityType<double>::type DepQuantityB;

  initializeDistribution<2,void,void>( distribution );

  // Copy from unitless distribution to distribution type A
  Utility::UnitAwarePowerDistribution<2,IndepUnitA,DepUnitA>
    unit_aware_dist_a_copy = Utility::UnitAwarePowerDistribution<2,IndepUnitA,DepUnitA>::fromUnitlessDistribution( *Teuchos::rcp_dynamic_cast<Utility::PowerDistribution<2> >( distribution ) );

  // Copy from distribution type A to distribution type B
  Utility::UnitAwarePowerDistribution<2,IndepUnitB,DepUnitB>
    unit_aware_dist_b_copy( unit_aware_dist_a_copy );

  IndepQuantityA indep_quantity_a =
    Utility::QuantityTraits<IndepQuantityA>::initializeQuantity( 0.0 );
  InverseIndepQuantityA inv_indep_quantity_a =
    Utility::QuantityTraits<InverseIndepQuantityA>::initializeQuantity( 0.0 );
  DepQuantityA dep_quantity_a =
    Utility::QuantityTraits<DepQuantityA>::initializeQuantity( 0.0 );

  IndepQuantityB indep_quantity_b( indep_quantity_a );
  InverseIndepQuantityB inv_indep_quantity_b( inv_indep_quantity_a );
  DepQuantityB dep_quantity_b( dep_quantity_a );

  UTILITY_TEST_FLOATING_EQUALITY(
			   unit_aware_dist_a_copy.evaluate( indep_quantity_a ),
			   dep_quantity_a,
			   1e-15 );
  UTILITY_TEST_FLOATING_EQUALITY(
			unit_aware_dist_a_copy.evaluatePDF( indep_quantity_a ),
			inv_indep_quantity_a,
			1e-15 );
  UTILITY_TEST_FLOATING_EQUALITY(
			   unit_aware_dist_b_copy.evaluate( indep_quantity_b ),
			   dep_quantity_b,
			   1e-15 );
  UTILITY_TEST_FLOATING_EQUALITY(
			unit_aware_dist_b_copy.evaluatePDF( indep_quantity_b ),
			inv_indep_quantity_b,
			1e-15 );

  Utility::setQuantity( indep_quantity_a, 1.0 );
  Utility::setQuantity( inv_indep_quantity_a, 3.0 );
  Utility::setQuantity( dep_quantity_a, 3.0 );

  indep_quantity_b = IndepQuantityB( indep_quantity_a );
  inv_indep_quantity_b = InverseIndepQuantityB( inv_indep_quantity_a );
  dep_quantity_b = DepQuantityB( dep_quantity_a );

  UTILITY_TEST_FLOATING_EQUALITY(
			   unit_aware_dist_a_copy.evaluate( indep_quantity_a ),
			   dep_quantity_a,
			   1e-15 );
  UTILITY_TEST_FLOATING_EQUALITY(
			unit_aware_dist_a_copy.evaluatePDF( indep_quantity_a ),
			inv_indep_quantity_a,
			1e-15 );
  UTILITY_TEST_FLOATING_EQUALITY(
			   unit_aware_dist_b_copy.evaluate( indep_quantity_b ),
			   dep_quantity_b,
			   1e-15 );
  UTILITY_TEST_FLOATING_EQUALITY(
			unit_aware_dist_b_copy.evaluatePDF( indep_quantity_b ),
			inv_indep_quantity_b,
			1e-15 );
}

typedef cgs::length cgs_length;
typedef si::length si_length;
typedef cgs::mass cgs_mass;
typedef si::mass si_mass;
typedef cgs::dimensionless cgs_dimensionless;
typedef si::dimensionless si_dimensionless;
typedef si::amount si_amount;

TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwarePowerDistribution,
				      explicit_conversion_1,
				      si_length,
				      si_amount,
				      cgs_length,
				      si_amount );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwarePowerDistribution,
				      explicit_conversion_1,
				      cgs_length,
				      si_amount,
				      si_length,
				      si_amount );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwarePowerDistribution,
				      explicit_conversion_1,
				      cgs_length,
				      cgs_mass,
				      si_length,
				      si_mass );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwarePowerDistribution,
				      explicit_conversion_1,
				      si_length,
				      si_mass,
				      cgs_length,
				      cgs_mass );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwarePowerDistribution,
				      explicit_conversion_1,
				      cgs_length,
				      cgs_dimensionless,
				      si_length,
				      si_dimensionless );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwarePowerDistribution,
				      explicit_conversion_1,
				      si_length,
				      si_dimensionless,
				      cgs_length,
				      cgs_dimensionless );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwarePowerDistribution,
				      explicit_conversion_1,
				      cgs_length,
				      void,
				      si_length,
				      void );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwarePowerDistribution,
				      explicit_conversion_1,
				      si_length,
				      void,
				      cgs_length,
				      void );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwarePowerDistribution,
				      explicit_conversion_1,
				      void,
				      cgs_dimensionless,
				      void,
				      si_dimensionless );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwarePowerDistribution,
				      explicit_conversion_1,
				      void,
				      si_dimensionless,
				      void,
				      cgs_dimensionless );

TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwarePowerDistribution,
				      explicit_conversion_2,
				      si_length,
				      si_amount,
				      cgs_length,
				      si_amount );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwarePowerDistribution,
				      explicit_conversion_2,
				      cgs_length,
				      si_amount,
				      si_length,
				      si_amount );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwarePowerDistribution,
				      explicit_conversion_2,
				      cgs_length,
				      cgs_mass,
				      si_length,
				      si_mass );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwarePowerDistribution,
				      explicit_conversion_2,
				      si_length,
				      si_mass,
				      cgs_length,
				      cgs_mass );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwarePowerDistribution,
				      explicit_conversion_2,
				      cgs_length,
				      cgs_dimensionless,
				      si_length,
				      si_dimensionless );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwarePowerDistribution,
				      explicit_conversion_2,
				      si_length,
				      si_dimensionless,
				      cgs_length,
				      cgs_dimensionless );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwarePowerDistribution,
				      explicit_conversion_2,
				      cgs_length,
				      void,
				      si_length,
				      void );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwarePowerDistribution,
				      explicit_conversion_2,
				      si_length,
				      void,
				      cgs_length,
				      void );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwarePowerDistribution,
				      explicit_conversion_2,
				      void,
				      cgs_dimensionless,
				      void,
				      si_dimensionless );
TEUCHOS_UNIT_TEST_TEMPLATE_4_INSTANT( UnitAwarePowerDistribution,
				      explicit_conversion_2,
				      void,
				      si_dimensionless,
				      void,
				      cgs_dimensionless );

//---------------------------------------------------------------------------//
// Custom main function
//---------------------------------------------------------------------------//
int main( int argc, char** argv )
{
  std::string test_dists_xml_file;

  Teuchos::CommandLineProcessor& clp = Teuchos::UnitTestRepository::getCLP();

  clp.setOption( "test_dists_xml_file",
		 &test_dists_xml_file,
		 "Test distributions xml file name" );

  const Teuchos::RCP<Teuchos::FancyOStream> out =
    Teuchos::VerboseObjectBase::getDefaultOStream();

  Teuchos::CommandLineProcessor::EParseCommandLineReturn parse_return =
    clp.parse(argc,argv);

  if ( parse_return != Teuchos::CommandLineProcessor::PARSE_SUCCESSFUL ) {
    *out << "\nEnd Result: TEST FAILED" << std::endl;
    return parse_return;
  }

  TEUCHOS_ADD_TYPE_CONVERTER( Utility::PowerDistribution<1u> );
  TEUCHOS_ADD_TYPE_CONVERTER( Utility::PowerDistribution<2u> );
  TEUCHOS_ADD_TYPE_CONVERTER( Utility::PowerDistribution<3u> );
  typedef Utility::UnitAwarePowerDistribution<1u,cgs::length,si::amount> UnitAwarePowerDistribution1;
  typedef Utility::UnitAwarePowerDistribution<2u,cgs::length,si::amount> UnitAwarePowerDistribution2;
  typedef Utility::UnitAwarePowerDistribution<3u,cgs::length,si::amount> UnitAwarePowerDistribution3;
  TEUCHOS_ADD_TYPE_CONVERTER( UnitAwarePowerDistribution1 );
  TEUCHOS_ADD_TYPE_CONVERTER( UnitAwarePowerDistribution2 );
  TEUCHOS_ADD_TYPE_CONVERTER( UnitAwarePowerDistribution3 );

  test_dists_list = Teuchos::getParametersFromXmlFile( test_dists_xml_file );

  // Initialize the random number generator
  Utility::RandomNumberGenerator::createStreams();

  // Run the unit tests
  Teuchos::GlobalMPISession mpiSession( &argc, &argv );

  const bool success = Teuchos::UnitTestRepository::runUnitTests(*out);

  if (success)
    *out << "\nEnd Result: TEST PASSED" << std::endl;
  else
    *out << "\nEnd Result: TEST FAILED" << std::endl;

  clp.printFinalTimerSummary(out.ptr());

  return (success ? 0 : 1);
}

//---------------------------------------------------------------------------//
// end tstPowerDistribution.cpp
//---------------------------------------------------------------------------//


/*!
 * @file
 *
 *
 * command to run:
 * mpirun -n 2 ./bin/pawn
 *
 * */

/*
#include <random>
#include <stdexcept>

#include <boost/mpi.hpp>
#include <boost/algorithm/string/split.hpp>

#include <ezl.hpp>
#include <ezl/algorithms/predicates.hpp>

#include "pawn/exprParser.hpp"
#include "pawn/fromFilePawn.hpp"
*/

/**
 * fromFile junk | $x = $1 + ($2 * $3) | where ($2 == 5.0 and $1 > $2) or ($x >= 0.1) | reduce($5, $6) $y=sum $x
 *
 *
file junk|%x =+$3 $4|%y =*+$3 $4 $3|reduce($1, $2) %y=sum $3|show
file junk|%x =+*$3 $4 $3|show
file junk|%x =++$4 $3 $4|show
 * */
// using dataT = std::tuple<const std::vector<std::string>&, const std::vector<double>&>;
// using sourceT = std::shared_ptr<ezl::Source<dataT>>;

/*
struct ColIndices {
  std::vector<int> str;
  std::vector<int> num;
};

using Ops = std::vector<std::string>;

auto isShow(Ops ops, size_t i) {
  if (i == ops.size() - 2) {
    return true;
  }
  return false;
}

auto getSource(Ops ops, ColIndices indices, std::vector<int> workers) {
  using std::string; using std::vector;
  using ezl::rise; using ezl::fromFilePawn;
  auto &op = ops[0];
  vector<string> args;
  boost::split(args, op, boost::is_any_of(" \t"));
  // auto &cmd = args[0];
  // std::transform(begin(cmd), end(cmd), begin(cmd), tolower);
  // assert(cmd == "file"); only possibility rightnow
  auto &inFile = args[1];
  auto x = rise(fromFilePawn(inFile, indices.str, indices.num)).prll(workers);
  if (isShow(ops, 0)) x.dump();
  return x.build();
}

auto mapCols(std::string op) {
  auto res = std::vector<int>{};
  auto it = std::find(begin(op), end(op), '$');
  while (it != std::end(op)) {
    auto x = extractInt(it + 1, std::end(op));
    res.push_back(x);
    it = std::find(it + 1, end(op), '$');
  }
  return res;
}

auto reduceCols(std::string op, ColIndices& result) {
  auto sep = std::find(begin(op), end(op), '=');
  auto it = std::find(begin(op), sep, '$');
  while (it != sep) {
    auto x = extractInt(it + 1, sep);
    result.str.push_back(x);
    it = std::find(it + 1, sep, '$');
  }
  it = std::find(sep, end(op), '$');
  while (it != std::end(op)) {
    auto x = extractInt(it + 1, std::end(op));
    result.num.push_back(x);
    it = std::find(it + 1, end(op), '$');
  }
}

auto getColumnNumbers(std::vector<std::string> ops) {
  ColIndices cols;
  for (size_t i = 1; i < ops.size() - 1; ++i) {
    auto cmd = ops[i];
    if (cmd[0] == '%') {  // true for all rightnow
      auto x = mapCols(ops[i]);
      cols.num.insert(end(cols.num), begin(x), end(x));
    } else {
      reduceCols(ops[i], cols);
    }
  }
  std::set<int> temp {begin(cols.num), end(cols.num)};
  cols.num = std::vector<int>{begin(temp), end(temp)};
  std::set<int> temp2 {begin(cols.str), end(cols.str)};
  cols.str = std::vector<int>{begin(temp2), end(temp2)};
  return cols;
}

auto getOps(std::string line) {
  std::vector<std::string> ops;
  boost::split(ops, line, boost::is_any_of("|"));
  return ops;
}

sourceT addMap(sourceT src, Ops ops, int i, ColIndices cols) {
  auto op = ops[i];
  op.erase(begin(op), std::find(begin(op), end(op), '=') + 1);
 // auto fn = parseMathExpr(op.begin(), op.end(), cols.num);
  auto x = ezl::flow(src).map<2>([op, cols](std::vector<double> v) {
    auto fn = parseMathExpr(op.begin(), op.end(), cols.num);
    v.push_back(fn(v));
    return std::make_tuple(v);
  }).colsTransform();
  if (isShow(ops, i)) x.dump();
  return x.build();
 }

sourceT addReduce(sourceT src, Ops ops, int i, ColIndices cols) {
  using std::string; using std::vector; using std::tuple;
  auto op = ops[i];
  op.erase(begin(op), find(begin(op), end(op), '=') + 1);
  auto initial = tuple<vector<double>>{};
  std::get<0>(initial).push_back(0);
  // if (cols.str.empty()) {
  //   auto x = ezl::flow(src).reduce<ezl::key<>, ezl::val<2>>([op, cols](std::tuple<std::vector<double>>& res, std::vector<double> cur) -> std::tuple<vector<double>>& {
  //     //auto fn = parseRedExpr(polish.begin(), polish.end(), cols.num);
  //     std::get<0>(res)[0] += cur[0];
  //     return res;
  //   }, initial);
  //   if (isShow(ops, i)) x.dump();
  //   return x.build();
  // }
  auto x = ezl::flow(src).reduce<1>([op, cols](tuple<vector<double>>& res,
  const vector<string>& key, const vector<double> &cur) -> auto& {
    //auto fn = parseRedExpr(polish.begin(), polish.end(), cols.num);
    std::get<0>(res)[0] += cur[0];
    return res;
  }, std::move(initial)).inprocess();
  std::get<0>(initial).push_back(0);
  auto y = x.reduce<1>([op, cols](tuple<vector<double>>& res, const vector<string>& key, const vector<double> &cur) -> auto& {
    //auto fn = parseRedExpr(polish.begin(), polish.end(), cols.num);
    std::get<0>(res)[0] += cur[0];
    return res;
  }, std::move(initial)).prll({0}, ezl::llmode::task);
  if (isShow(ops, i)) y.dump();
  return y.build();
}

auto addUnit(sourceT src, std::vector<std::string> ops, int i, ColIndices cols) {
  using std::string; using std::vector; using ezl::flow;
  auto &op = ops[i];
  vector<string> args;
  boost::split(args, op, boost::is_any_of(" \t"));
  auto &cmd = args[0];
  std::transform(begin(cmd), end(cmd), begin(cmd), tolower);
  if(cmd[0] == '%') {
    return addMap(src, ops, i, cols);
  } else {
    return addReduce(src, ops, i, cols);
  }
}

auto runFlow(sourceT src, std::vector<std::string> ops, ColIndices cols, std::vector<int> workers) {
  std::vector<int> all = workers;
  all.push_back(0);
  ezl::flow(src).run(all);
}

auto readQuery(std::string line, std::vector<int> workers) {
  Ops ops = getOps(line);
  if (ops.empty()) return false;
  ColIndices cols = getColumnNumbers(ops);
  sourceT src = getSource(ops, cols, workers);
  sourceT cur = src;
  for (size_t i = 1; i < ops.size() - 1; ++i) {
    cur = addUnit(cur, ops, i, cols);
  }
  runFlow(cur, ops, cols, workers);
  return true;
}

auto checkQuery(std::string line) {
  return std::string{};
}

auto pawn(int argc, char *argv[]) {
  int master = 0;
  auto nProc = ezl::Karta::inst().nProc();
  std::vector<int> workers(ezl::Karta::inst().nProc() - 1);
  if (nProc == 1) {
    if (workers.empty()) workers.push_back(0); // clang
    else workers[0] = 0; // gcc
  }
  else std::iota(begin(workers), end(workers), 1);
  auto scheduler = [](std::vector<int> workers) {
    return workers;
  };
  //workers.pop_back();
  //std::cout << workers << std::endl;
  auto queryCin = [&scheduler, &workers]{
    std::cout << "> ";
    std::string line;
    std::getline(std::cin, line);
    if (line == "exit") return std::make_pair(line, false);
    auto err = checkQuery(line);
    if (err.length() != 0) {
      std::cout << err;
      line = "";
    }
    auto curWorkers = scheduler(workers);
    return std::make_pair(line, true);
  };
  ezl::rise(queryCin).prll({master})
  .filter([&workers](std::string line) {
    if (line == "") return false;
    return readQuery(line, workers);
  }).prll(1.0, ezl::llmode::task | ezl::llmode::dupe)
  .run();
}
*/
/*
auto pawkSerial(int argc, char *argv[]) {
  int master = 0;
  auto nProc = ezl::Karta::inst().nProc();
  std::vector<int> workers(ezl::Karta::inst().nProc() - 1);
  if (nProc == 1) workers[0] = 0;
  else std::iota(begin(workers), end(workers), 1);
  auto scheduler = [](std::vector<int> workers) {
    return workers;
  };
  auto queryCin = [&scheduler, &workers]{
    std::cout << "> ";
    std::string line;
    std::getline(std::cin, line);
    if (line == "exit") return std::make_pair(line, false);
    auto err = checkQuery(line);
    if(err.length() != 0) {
      std::cout << err;
      line = "";
    }
    auto curWorkers = scheduler(workers);
    return std::make_pair(line, true);
  };
  while (true) {
    auto x = queryCin();
    if (x.second == false) break;
    readQuery(x.first, workers);
  }
}
*/
/*
int main(int argc, char *argv[]) {
  boost::mpi::environment env(argc, argv, false);
  try {
    pawn(argc, argv);
  } catch (const std::exception& ex) {
    std::cerr<<"error: "<<ex.what()<<'\n';
    env.abort(1);
  } catch (...) {
    std::cerr<<"unknown exception\n";
    env.abort(2);
  }
  return 0;
}
*/