//
//  IglUtils.hpp
//  IPC
//
//  Created by Minchen Li on 8/30/17.
//

#ifndef IglUtils_hpp
#define IglUtils_hpp

#include "Mesh.hpp"

#ifdef USE_CLOSEDFORMSVD2D
#include "ClosedFormSVD2d.hpp"
#else
#include "AutoFlipSVD.hpp"
#endif

#include "LinSysSolver.hpp"

#ifdef USE_PREDICATES
#include <igl/predicates/predicates.h>
// #include <ECCD.hpp>
#endif

#include <Eigen/Eigen>

#include <iostream>
#include <fstream>

namespace IPC {

// a static class implementing basic geometry processing operations that are not provided in libIgl
class IglUtils {

public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
    template <int dim>
    static void addBlockToMatrix(const Eigen::Matrix<double, dim, dim*(dim + 1)>& block,
        const Eigen::Matrix<int, 1, dim + 1>& index, int rowIndI,
        LinSysSolver<Eigen::VectorXi, Eigen::VectorXd>* linSysSolver)
    {
        int rowStart = index[rowIndI] * dim;
        if (rowStart < 0) {
            rowStart = -rowStart - dim;
            linSysSolver->setCoeff(rowStart, rowStart, 1.0);
            linSysSolver->setCoeff(rowStart + 1, rowStart + 1, 1.0);
            if constexpr (dim == 3) {
                linSysSolver->setCoeff(rowStart + 2, rowStart + 2, 1.0);
            }
            return;
        }

        if (index[0] >= 0) {
            int _dimIndex0 = index[0] * dim;
            linSysSolver->addCoeff(rowStart, _dimIndex0, block(0, 0));
            linSysSolver->addCoeff(rowStart, _dimIndex0 + 1, block(0, 1));
            linSysSolver->addCoeff(rowStart + 1, _dimIndex0, block(1, 0));
            linSysSolver->addCoeff(rowStart + 1, _dimIndex0 + 1, block(1, 1));
            if constexpr (dim == 3) {
                linSysSolver->addCoeff(rowStart, _dimIndex0 + 2, block(0, 2));
                linSysSolver->addCoeff(rowStart + 1, _dimIndex0 + 2, block(1, 2));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex0, block(2, 0));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex0 + 1, block(2, 1));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex0 + 2, block(2, 2));
            }
        }

        if (index[1] >= 0) {
            int _dimIndex1 = index[1] * dim;
            linSysSolver->addCoeff(rowStart, _dimIndex1, block(0, dim));
            linSysSolver->addCoeff(rowStart, _dimIndex1 + 1, block(0, dim + 1));
            linSysSolver->addCoeff(rowStart + 1, _dimIndex1, block(1, dim));
            linSysSolver->addCoeff(rowStart + 1, _dimIndex1 + 1, block(1, dim + 1));
            if constexpr (dim == 3) {
                linSysSolver->addCoeff(rowStart, _dimIndex1 + 2, block(0, dim + 2));
                linSysSolver->addCoeff(rowStart + 1, _dimIndex1 + 2, block(1, dim + 2));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex1, block(2, dim));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex1 + 1, block(2, dim + 1));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex1 + 2, block(2, dim + 2));
            }
        }

        if (index[2] >= 0) {
            int _2dim = 2 * dim;
            int _dimIndex2 = index[2] * dim;
            linSysSolver->addCoeff(rowStart, _dimIndex2, block(0, _2dim));
            linSysSolver->addCoeff(rowStart, _dimIndex2 + 1, block(0, _2dim + 1));
            linSysSolver->addCoeff(rowStart + 1, _dimIndex2, block(1, _2dim));
            linSysSolver->addCoeff(rowStart + 1, _dimIndex2 + 1, block(1, _2dim + 1));
            if constexpr (dim == 3) {
                linSysSolver->addCoeff(rowStart, _dimIndex2 + 2, block(0, _2dim + 2));
                linSysSolver->addCoeff(rowStart + 1, _dimIndex2 + 2, block(1, _2dim + 2));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex2, block(2, _2dim));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex2 + 1, block(2, _2dim + 1));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex2 + 2, block(2, _2dim + 2));
            }
        }

        if constexpr (dim == 3) {
            if (index[3] >= 0) {
                int _3dim = 3 * dim;
                int _dimIndex3 = index[3] * dim;
                linSysSolver->addCoeff(rowStart, _dimIndex3, block(0, _3dim));
                linSysSolver->addCoeff(rowStart, _dimIndex3 + 1, block(0, _3dim + 1));
                linSysSolver->addCoeff(rowStart, _dimIndex3 + 2, block(0, _3dim + 2));
                linSysSolver->addCoeff(rowStart + 1, _dimIndex3, block(1, _3dim));
                linSysSolver->addCoeff(rowStart + 1, _dimIndex3 + 1, block(1, _3dim + 1));
                linSysSolver->addCoeff(rowStart + 1, _dimIndex3 + 2, block(1, _3dim + 2));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex3, block(2, _3dim));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex3 + 1, block(2, _3dim + 1));
                linSysSolver->addCoeff(rowStart + 2, _dimIndex3 + 2, block(2, _3dim + 2));
            }
        }
    }

    // project a symmetric real matrix to the nearest SPD matrix
    template <typename Scalar, int size>
    static void makePD(Eigen::Matrix<Scalar, size, size>& symMtr)
    {
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, size, size>> eigenSolver(symMtr);
        if (eigenSolver.eigenvalues()[0] >= 0.0) {
            return;
        }
        Eigen::DiagonalMatrix<Scalar, size> D(eigenSolver.eigenvalues());
        int rows = ((size == Eigen::Dynamic) ? symMtr.rows() : size);
        for (int i = 0; i < rows; i++) {
            if (D.diagonal()[i] < 0.0) {
                D.diagonal()[i] = 0.0;
            }
            else {
                break;
            }
        }
        symMtr = eigenSolver.eigenvectors() * D * eigenSolver.eigenvectors().transpose();
    }
    template <typename Scalar, int size>
    static void makePD2d(Eigen::Matrix<Scalar, size, size>& symMtr)
    {
        // based on http://www.math.harvard.edu/archive/21b_fall_04/exhibits/2dmatrices/

        if (size == Eigen::Dynamic) {
            assert(symMtr.rows() == 2);
        }
        else {
            assert(size == 2);
        }

        const double a = symMtr(0, 0);
        const double b = (symMtr(0, 1) + symMtr(1, 0)) / 2.0;
        const double d = symMtr(1, 1);

        double b2 = b * b;
        const double D = a * d - b2;
        const double T_div_2 = (a + d) / 2.0;
        const double sqrtTT4D = std::sqrt(T_div_2 * T_div_2 - D);
        const double L2 = T_div_2 - sqrtTT4D;
        if (L2 < 0.0) {
            const double L1 = T_div_2 + sqrtTT4D;
            if (L1 <= 0.0) {
                symMtr.setZero();
            }
            else {
                if (b2 == 0.0) {
                    symMtr << L1, 0.0, 0.0, 0.0;
                }
                else {
                    const double L1md = L1 - d;
                    const double L1md_div_L1 = L1md / L1;
                    symMtr(0, 0) = L1md_div_L1 * L1md;
                    symMtr(0, 1) = symMtr(1, 0) = b * L1md_div_L1;
                    symMtr(1, 1) = b2 / L1;
                }
            }
        }
    }
    template <typename Scalar, int size>
    static void flipDet_SVD(Eigen::Matrix<Scalar, size, size>& mtr)
    {
        Eigen::JacobiSVD<Eigen::Matrix<Scalar, size, size>> svd(mtr, Eigen::ComputeFullU | Eigen::ComputeFullV);

        Eigen::Matrix<Scalar, size, size> U = svd.matrixU(), V = svd.matrixV();
        if (U.determinant() < 0) {
            U.col(U.cols() - 1) *= -1.0;
        }
        if (V.determinant() < 0) {
            V.col(V.cols() - 1) *= -1.0;
        }
        mtr = U * Eigen::DiagonalMatrix<Scalar, size>(svd.singularValues()) * V.transpose();
    }

    static void writeSparseMatrixToFile(const std::string& filePath,
        const Eigen::SparseMatrix<double>& mtr,
        bool MATLAB = false);
    static void writeSparseMatrixToFile(const std::string& filePath,
        const std::map<std::pair<int, int>, double>& mtr,
        bool MATLAB = false);
    static void writeSparseMatrixToFile(const std::string& filePath,
        LinSysSolver<Eigen::VectorXi, Eigen::VectorXd>* linSysSolver,
        bool MATLAB = false);
    static void loadSparseMatrixFromFile(const std::string& filePath,
        Eigen::SparseMatrix<double>& mtr);

    static void writeVectorToFile(const std::string& filePath,
        const Eigen::VectorXd& vec);
    static void readVectorFromFile(const std::string& filePath,
        Eigen::VectorXd& vec);

    // read segment mesh file
    static bool readSEG(const std::string& filePath, Eigen::MatrixXd& V, Eigen::MatrixXi& E);
    static void writeSEG(const std::string& filePath, const Eigen::MatrixXd& V, const Eigen::MatrixXi& E);

    static bool segTriIntersect(const Eigen::RowVector3d& ve0, const Eigen::RowVector3d& ve1,
        const Eigen::RowVector3d& vt0, const Eigen::RowVector3d& vt1, const Eigen::RowVector3d& vt2)
    {
        Eigen::Matrix3d coefMtr;
        coefMtr.col(0) = vt1 - vt0;
        coefMtr.col(1) = vt2 - vt0;
        coefMtr.col(2) = ve0 - ve1;

#ifdef USE_PREDICATES
        igl::predicates::exactinit();
        const auto ori1 = igl::predicates::orient3d(vt0, vt1, vt2, ve0);
        const auto ori2 = igl::predicates::orient3d(vt0, vt1, vt2, ve1);
        if (ori1 == igl::predicates::Orientation::COPLANAR || ori2 == igl::predicates::Orientation::COPLANAR) {
            // coplanar, we can detect it by d(EE)=0 or d(PT)=0
            return false;
        }

        if (ori1 == ori2) {
            // edge is on one side of the plane that triangle is in
            return false;
        }
#else
        Eigen::RowVector3d n = coefMtr.col(0).cross(coefMtr.col(1));
        if (n.dot(ve0 - vt0) * n.dot(ve1 - vt0) > 0.0) {
            return false; // edge is on one side of the plane that triangle is in
        }

        if (coefMtr.determinant() == 0.0) {
            return false; // coplanar, we can detect it by d(EE)=0 or d(PT)=0
        }
#endif

#ifdef USE_PREDICATES
        // int res = eccd::segment_triangle_inter(ve0, ve1, vt0, vt1, vt2);
        // if(res == 1){
        // std::cout << ve0 << std::endl;
        // std::cout << ve1 << std::endl;
        // std::cout << vt0 << std::endl;
        // std::cout << vt1 << std::endl;
        // std::cout << vt2 << std::endl;
        // exit(0);
        // }
        // return res == 1 ? true : false;
#endif
        Eigen::Vector3d uvt = coefMtr.fullPivLu().solve((ve0 - vt0).transpose());
        if (uvt[0] >= 0.0 && uvt[1] >= 0.0 && uvt[0] + uvt[1] <= 1.0 && uvt[2] >= 0.0 && uvt[2] <= 1.0) {
            return true;
        }
        else {
            return false;
        }
    }

    static bool
    pointBehindTri(
        const Eigen::RowVector3d& vt0, const Eigen::RowVector3d& vt1,
        const Eigen::RowVector3d& vt2, const Eigen::RowVector3d& v0)
    {
        return (vt1 - vt0).cross(vt2 - vt0).dot(v0 - vt0) <= 0.0;
    }

    static bool
    pointInsideTetrahedron(const Eigen::RowVector3d& v0,
        const Eigen::RowVector3d& vt0, const Eigen::RowVector3d& vt1,
        const Eigen::RowVector3d& vt2, const Eigen::RowVector3d& vt3)
    {
#ifdef USE_PREDICATES
        igl::predicates::exactinit();
        const auto ori1 = igl::predicates::orient3d(vt0, vt2, vt1, v0);
        if (ori1 != igl::predicates::Orientation::NEGATIVE) {
            const auto ori2 = igl::predicates::orient3d(vt0, vt3, vt2, v0);
            if (ori2 != igl::predicates::Orientation::NEGATIVE) {
                const auto ori3 = igl::predicates::orient3d(vt0, vt1, vt3, v0);
                if (ori3 != igl::predicates::Orientation::NEGATIVE) {
                    const auto ori4 = igl::predicates::orient3d(vt1, vt2, vt3, v0);
                    if (ori4 != igl::predicates::Orientation::NEGATIVE) {
                        return true;
                    }
                }
            }
        }
#else
        const bool ori1 = pointBehindTri(vt0, vt2, vt1, v0);
        if (ori1) {
            const bool ori2 = pointBehindTri(vt0, vt3, vt2, v0);
            if (ori2) {
                const bool ori3 = pointBehindTri(vt0, vt1, vt3, v0);
                if (ori3) {
                    const bool ori4 = pointBehindTri(vt1, vt2, vt3, v0);
                    if (ori4) {
                        return true;
                    }
                }
            }
        }
#endif
        return false;
    }

    static void findSurfaceTris(const Eigen::MatrixXi& TT, Eigen::MatrixXi& F);
    static void buildSTri2Tet(const Eigen::MatrixXi& F, const Eigen::MatrixXi& SF,
        std::vector<int>& sTri2Tet);

    static void saveTetMesh(const std::string& filePath,
        const Eigen::MatrixXd& TV, const Eigen::MatrixXi& TT,
        const Eigen::MatrixXi& F = Eigen::MatrixXi(),
        bool findSurface = true);
    static void saveTetMesh_msh4(const std::string& filePath,
        const Eigen::MatrixXd& TV, const Eigen::MatrixXi& TT,
        const Eigen::MatrixXi& F = Eigen::MatrixXi(),
        bool findSurface = true);
    static void saveTetMesh_vtk(const std::string& filePath,
        const Eigen::MatrixXd& TV, const Eigen::MatrixXi& TT);
    static bool readTetMesh(const std::string& filePath,
        Eigen::MatrixXd& TV, Eigen::MatrixXi& TT,
        Eigen::MatrixXi& F, bool findSurface = true);
    static bool readTetMesh_msh4(const std::string& filePath,
        Eigen::MatrixXd& TV, Eigen::MatrixXi& TT,
        Eigen::MatrixXi& F, bool findSurface = true);
    static void readNodeEle(const std::string& filePath,
        Eigen::MatrixXd& TV, Eigen::MatrixXi& TT,
        Eigen::MatrixXi& F);

    template <int colSize>
    static void dF_div_dx_mult(const Eigen::Matrix<double, DIM * DIM, colSize>& right,
        const Eigen::Matrix<double, DIM, DIM>& A,
        Eigen::Matrix<double, DIM*(DIM + 1), colSize>& result,
        bool symmetric)
    {
        if (colSize == Eigen::Dynamic) {
            assert(right.cols() > 0);
        }
        else {
            assert(colSize > 0);
        }
#if (DIM == 2)
        if (symmetric) {
            if (colSize == Eigen::Dynamic) {
                assert(right.cols() == 6);
            }
            else {
                assert(colSize == 6);
            }
            // int colI = 0;
            const double _0000 = right(0, 0) * A(0, 0);
            const double _0010 = right(0, 0) * A(1, 0);
            const double _1001 = right(1, 0) * A(0, 1);
            const double _1011 = right(1, 0) * A(1, 1);
            const double _2000 = right(2, 0) * A(0, 0);
            const double _2010 = right(2, 0) * A(1, 0);
            const double _3001 = right(3, 0) * A(0, 1);
            const double _3011 = right(3, 0) * A(1, 1);
            result(2, 0) = result(0, 2) = _0000 + _1001;
            result(3, 0) = result(0, 3) = _2000 + _3001;
            result(4, 0) = result(0, 4) = _0010 + _1011;
            result(5, 0) = result(0, 5) = _2010 + _3011;
            result(0, 0) = -result(2, 0) - result(4, 0);
            result(1, 0) = result(0, 1) = -result(3, 0) - result(5, 0);
            // colI = 1;
            const double _2100 = right(2, 1) * A(0, 0);
            const double _2110 = right(2, 1) * A(1, 0);
            const double _3101 = right(3, 1) * A(0, 1);
            const double _3111 = right(3, 1) * A(1, 1);
            result(2, 1) = result(1, 2) = right(0, 1) * A(0, 0) + right(1, 1) * A(0, 1);
            result(3, 1) = result(1, 3) = _2100 + _3101;
            result(4, 1) = result(1, 4) = right(0, 1) * A(1, 0) + right(1, 1) * A(1, 1);
            result(5, 1) = result(1, 5) = _2110 + _3111;
            result(1, 1) = -result(3, 1) - result(5, 1);
            // colI = 2;
            result(2, 2) = right(0, 2) * A(0, 0) + right(1, 2) * A(0, 1);
            result(3, 2) = result(2, 3) = right(2, 2) * A(0, 0) + right(3, 2) * A(0, 1);
            result(4, 2) = result(2, 4) = right(0, 2) * A(1, 0) + right(1, 2) * A(1, 1);
            result(5, 2) = result(2, 5) = right(2, 2) * A(1, 0) + right(3, 2) * A(1, 1);
            // colI = 3;
            result(3, 3) = right(2, 3) * A(0, 0) + right(3, 3) * A(0, 1);
            result(4, 3) = result(3, 4) = right(0, 3) * A(1, 0) + right(1, 3) * A(1, 1);
            result(5, 3) = result(3, 5) = right(2, 3) * A(1, 0) + right(3, 3) * A(1, 1);
            // colI = 4;
            result(4, 4) = right(0, 4) * A(1, 0) + right(1, 4) * A(1, 1),
                      result(5, 4) = result(4, 5) = right(2, 4) * A(1, 0) + right(3, 4) * A(1, 1);
            // colI = 5;
            result(5, 5) = right(2, 5) * A(1, 0) + right(3, 5) * A(1, 1);
        }
        else {
            for (int colI = 0; colI < right.cols(); colI++) {
                const double _000 = right(0, colI) * A(0, 0);
                const double _010 = right(0, colI) * A(1, 0);
                const double _101 = right(1, colI) * A(0, 1);
                const double _111 = right(1, colI) * A(1, 1);
                const double _200 = right(2, colI) * A(0, 0);
                const double _210 = right(2, colI) * A(1, 0);
                const double _301 = right(3, colI) * A(0, 1);
                const double _311 = right(3, colI) * A(1, 1);

                result(2, colI) = _000 + _101;
                result(3, colI) = _200 + _301;
                result(4, colI) = _010 + _111;
                result(5, colI) = _210 + _311;
                result(0, colI) = -result(2, colI) - result(4, colI);
                result(1, colI) = -result(3, colI) - result(5, colI);
            }
        }
#else
        for (int colI = 0; colI < right.cols(); colI++) {
            result(3, colI) = (A.row(0) * right.block(0, colI, DIM, 1))[0];
            result(4, colI) = (A.row(0) * right.block(DIM, colI, DIM, 1))[0];
            result(5, colI) = (A.row(0) * right.block(DIM * 2, colI, DIM, 1))[0];
            result(6, colI) = (A.row(1) * right.block(0, colI, DIM, 1))[0];
            result(7, colI) = (A.row(1) * right.block(DIM, colI, DIM, 1))[0];
            result(8, colI) = (A.row(1) * right.block(DIM * 2, colI, DIM, 1))[0];
            result(9, colI) = (A.row(2) * right.block(0, colI, DIM, 1))[0];
            result(10, colI) = (A.row(2) * right.block(DIM, colI, DIM, 1))[0];
            result(11, colI) = (A.row(2) * right.block(DIM * 2, colI, DIM, 1))[0];
            result(0, colI) = -result(3, colI) - result(6, colI) - result(9, colI);
            result(1, colI) = -result(4, colI) - result(7, colI) - result(10, colI);
            result(2, colI) = -result(5, colI) - result(8, colI) - result(11, colI);
        }
#endif
    }
    static void dF_div_dx_mult(const Eigen::Matrix<double, DIM, DIM>& right,
        const Eigen::Matrix<double, DIM, DIM>& A,
        Eigen::Matrix<double, DIM*(DIM + 1), 1>& result);
    template <int dim>
    static void computeCofactorMtr(const Eigen::Matrix<double, dim, dim>& F,
        Eigen::Matrix<double, dim, dim>& A)
    {
        switch (dim) {
        case 2:
            A(0, 0) = F(1, 1);
            A(0, 1) = -F(1, 0);
            A(1, 0) = -F(0, 1);
            A(1, 1) = F(0, 0);
            break;

        case 3:
            A(0, 0) = F(1, 1) * F(2, 2) - F(1, 2) * F(2, 1);
            A(0, 1) = F(1, 2) * F(2, 0) - F(1, 0) * F(2, 2);
            A(0, 2) = F(1, 0) * F(2, 1) - F(1, 1) * F(2, 0);
            A(1, 0) = F(0, 2) * F(2, 1) - F(0, 1) * F(2, 2);
            A(1, 1) = F(0, 0) * F(2, 2) - F(0, 2) * F(2, 0);
            A(1, 2) = F(0, 1) * F(2, 0) - F(0, 0) * F(2, 1);
            A(2, 0) = F(0, 1) * F(1, 2) - F(0, 2) * F(1, 1);
            A(2, 1) = F(0, 2) * F(1, 0) - F(0, 0) * F(1, 2);
            A(2, 2) = F(0, 0) * F(1, 1) - F(0, 1) * F(1, 0);
            break;

        default:
            assert(0 && "dim not 2 or 3");
            break;
        }
    }

    static void findBorderVerts(const Eigen::MatrixXd& V,
        std::vector<std::vector<int>>& borderVerts,
        double ratio);

    static void Init_Dirichlet(
        const Eigen::MatrixXd& X,
        const Eigen::Vector3d& relBoxMin,
        const Eigen::Vector3d& relBoxMax,
        const std::vector<bool>& isNodeOnBoundary,
        std::vector<int>& selectedVerts);
}; // namespace IPC

} // namespace IPC

#endif /* IglUtils_hpp */


#include <chainparams.h>
#include <coins.h>
#include <consensus/consensus.h>
#include <consensus/merkle.h>
#include <consensus/tx_verify.h>
#include <consensus/validation.h>
#include "crypto/equihash.h"
#include <validation.h>
#include <miner.h>
#include <policy/policy.h>
#include <pubkey.h>
#include <script/standard.h>
#include <txmempool.h>
#include <uint256.h>
#include <util.h>
#include <utilstrencodings.h>
#include <amount.h>

#include <test/test_bitcoin.h>

#include <memory>

#include <boost/test/unit_test.hpp>

struct GenesisRegTestingSetup : public BasicTestingSetup {
    GenesisRegTestingSetup() : BasicTestingSetup(CBaseChainParams::REGTEST) {}
};

BOOST_FIXTURE_TEST_SUITE(genesis_reg_tests, GenesisRegTestingSetup)

BOOST_AUTO_TEST_CASE(GenesisReg)
{
    CBlock genesis = CChainParams::CreateGenesisBlock(
            1296688602,
            uint256S("0x0"),
            ParseHex(""),
            0x207fffff, 1, GENESIS_MONEY);
    CBlock *pblock = &genesis;
    const CChainParams params = Params();

    unsigned int n = params.EquihashN();
    unsigned int k = params.EquihashK();

printf("%d %d\n", n, k);
    // Hash state
    blake2b_state state;
    EhInitialiseState(n, k, state);

    // I = the block header minus nonce and solution.
    CEquihashInput I{*pblock};
    // I||V
    CDataStream ss(SER_NETWORK, PROTOCOL_VERSION);
    ss << I;
    // ss << pblock->nNonce;

    // H(I||V||...
    blake2b_update(&state, (unsigned char*)&ss[0], ss.size());

    while (true) {
            // Yes, there is a chance every nonce could fail to satisfy the -regtest
            // target -- 1 in 2^(2^256). That ain't gonna happen
            pblock->nNonce = ArithToUint256(UintToArith256(pblock->nNonce) + 1);

            // H(I||V||...
            blake2b_state curr_state;
            curr_state = state;
            blake2b_update(&curr_state,
                                              pblock->nNonce.begin(),
                                              pblock->nNonce.size());

            // (x_1, x_2, ...) = A(I, V, n, k)
            std::function<bool(std::vector<unsigned char>)> validBlock =
                    [&pblock](std::vector<unsigned char> soln) {
                pblock->nSolution = soln;
                return CheckProofOfWork(pblock->GetHash(), pblock->nBits, Params().GetConsensus());
            };
            bool found = EhBasicSolveUncancellable(n, k, curr_state, validBlock);
            if (found) {
                goto endloop;
            }
        }
endloop:
    printf("%s\n", pblock->nNonce.GetHex().c_str());

    blake2b_update(&state,
                                              pblock->nNonce.begin(),
                                              pblock->nNonce.size());

    // pblock->nSolution = ParseHex("07c0fa1e314ca6a95c1d604a3a54858922c52e52984d05f496217068f35d90c8e5469b63");
    bool isValid;
    printf("pblock soln = %s\n", HexStr(pblock->nSolution).c_str());
    EhIsValidSolution(n, k, state, pblock->nSolution, isValid);
    printf("isValid = %s\n", isValid ? "true" : "false");
    printf("blockHash = %s\n", pblock->GetHash().ToString().c_str());
}

BOOST_AUTO_TEST_SUITE_END()


//#define _USE_MATH_DEFINES

//#include <cmath>
//#include <iostream>

//#include <catch2/catch.hpp>

// #include <igl/PI.h>
//#include <finitediff.hpp>

//#include <state.hpp>

//#include <Eigen/Geometry>

//#define NUM_ANGLES (5)

// using namespace ipc::rigid;
// using namespace opt;

// TEST_CASE("test the setup", "[opt][displacements][barrier]")
//{
//    State state;

//    // Load the test scene
//    state.load_scene(std::string(FIXTURES_DIR)
//        + "/tests/setup-falling-horizontal-edge.json");
//    REQUIRE(state.vertices.rows() == 4);
//    REQUIRE(state.displacements.rows() == 4);
//    REQUIRE(state.edges.rows() == 2);

//    state.constraint_function = GENERATE(ipc::rigid::ConstraintType::BARRIER);
//    state.getCollisionConstraint().update_collision_set = GENERATE(false,
//    true);

//    state.opt_method = ipc::rigid::OptimizationMethod::BARRIER_SOLVER;

//    state.reset_optimization_problem();

//    CHECK(state.particles_problem.num_vars == state.displacements.size());

//    CHECK(
//        state.particles_problem.x0.size() ==
//        state.particles_problem.num_vars);
//    CHECK(state.particles_problem.x_lower.size()
//        == state.particles_problem.num_vars);
//    CHECK(state.particles_problem.x_upper.size()
//        == state.particles_problem.num_vars);
//    CHECK(state.particles_problem.g_lower.size()
//        == state.particles_problem.num_constraints);
//    CHECK(state.particles_problem.g_upper.size()
//        == state.particles_problem.num_constraints);

//    state.particles_problem.x0.setRandom();

//    // Test ∇f
//    Eigen::VectorXd finite_grad(state.particles_problem.num_vars);
//    finite_gradient(state.particles_problem.x0,
//        state.particles_problem.func_f(), finite_grad);
//    Eigen::VectorXd analytic_grad
//        = state.particles_problem.eval_grad_f(state.particles_problem.x0);
//    CHECK(fd::compare_gradient(finite_grad, analytic_grad));

//    // Test ∇²f
//    Eigen::MatrixXd finite_hessian
//        = state.particles_problem.eval_hessian_f_approx(
//            state.particles_problem.x0);
//    Eigen::MatrixXd analytic_hessian
//        = state.particles_problem
//              .eval_hessian_f(state.particles_problem.x0)
//              .toDense();
//    CHECK(fd::compare_jacobian(finite_hessian, analytic_hessian));

//    // Test ∇g
//    Eigen::MatrixXd finite_jac
//        =
//        state.particles_problem.eval_jac_g_approx(state.particles_problem.x0);
//    Eigen::MatrixXd analytic_jac
//        = state.particles_problem.eval_jac_g(state.particles_problem.x0);
//    CHECK(fd::compare_jacobian(finite_jac, analytic_jac));

//    // Test ∇²g
//    Eigen::MatrixXd finite_hessian_gi(
//        state.particles_problem.num_vars, state.particles_problem.num_vars);
//    finite_hessian_gi.setOnes();
//    long i;

//    auto diff_i = [&](const Eigen::VectorXd& x) -> Eigen::VectorXd {
//        auto jac = state.particles_problem.eval_jac_g(x);
//        return jac.row(i);
//    };

//    std::vector<Eigen::SparseMatrix<double>> analytic_hessian_g
//        = state.particles_problem.eval_hessian_g(state.particles_problem.x0);
//    for (i = 0; i < long(analytic_hessian_g.size()); i++) {
//        finite_jacobian(state.particles_problem.x0, diff_i,
//        finite_hessian_gi); CHECK(fd::compare_jacobian(
//            finite_hessian_gi, analytic_hessian_g[unsigned(i)]));
//    }
//}

// TEST_CASE("two rotating edges", "[opt][displacements][barrier]")
//{
//    State state;
//    state.load_scene(std::string(FIXTURES_DIR)
//        + "/two-edges/falling-edge-00°-same-length.json");

//    REQUIRE(state.vertices.rows() == 4);
//    REQUIRE(state.displacements.rows() == 4);
//    REQUIRE(state.edges.rows() == 2);

//    state.constraint_function = ipc::rigid::ConstraintType::BARRIER;

//    state.getCollisionConstraint().update_collision_set = GENERATE(false,
//    true); state.opt_method = ipc::rigid::OptimizationMethod::BARRIER_SOLVER;

//    double theta1 = 2 * igl::PI / NUM_ANGLES * GENERATE(range(0, NUM_ANGLES));
//    double theta2 = 2 * igl::PI / NUM_ANGLES * GENERATE(range(0, NUM_ANGLES));

//    for (int i = 0; i < state.edges.rows(); i++) {
//        Eigen::Rotation2D<double> R(i == 0 ? theta1 : theta2);
//        Eigen::RowVector2d center = (state.vertices.row(state.edges(i, 1))
//                                        + state.vertices.row(state.edges(i,
//                                        0)))
//            / 2;
//        state.vertices.row(state.edges(i, 0))
//            = (R * (state.vertices.row(state.edges(i, 0)) -
//            center).transpose())
//                  .transpose()
//            + center;
//        state.vertices.row(state.edges(i, 1))
//            = (R * (state.vertices.row(state.edges(i, 1)) -
//            center).transpose())
//                  .transpose()
//            + center;
//    }

//    state.barrier_solver.min_barrier_epsilon = 1e-3;
//    state.barrier_solver.inner_solver_type = BarrierInnerSolver::NEWTON;
//    dynamic_cast<NewtonSolver&>(state.barrier_solver.get_inner_solver())
//        .min_step_length
//        = 1e-4;
//    dynamic_cast<NewtonSolver&>(state.barrier_solver.get_inner_solver())
//        .absolute_tolerance
//        = 1e-3;

//    state.optimize_displacements("");
//    CHECK(state.opt_results.success);
//}

// TEST_CASE("corner case", "[opt][displacements][barrier]")
//{
//    State state;
//    state.load_scene(std::string(FIXTURES_DIR) + "/corner-case.json");

//    REQUIRE(state.vertices.rows() == 5);
//    REQUIRE(state.displacements.rows() == 5);
//    REQUIRE(state.edges.rows() == 3);

//    state.constraint_function = ipc::rigid::ConstraintType::BARRIER;

//    state.getCollisionConstraint().update_collision_set = GENERATE(false,
//    true); state.opt_method = ipc::rigid::OptimizationMethod::BARRIER_SOLVER;

//    state.barrier_solver.min_barrier_epsilon = 1e-3;
//    state.barrier_solver.inner_solver_type = BarrierInnerSolver::NEWTON;
//    dynamic_cast<NewtonSolver&>(state.barrier_solver.get_inner_solver())
//        .min_step_length
//        = 1e-4;
//    dynamic_cast<NewtonSolver&>(state.barrier_solver.get_inner_solver())
//        .absolute_tolerance
//        = 1e-3;

//    state.optimize_displacements("");
//    CHECK(state.opt_results.success);
//}


#include <boost/hana.hpp>
#include <string_view>
#include <tuple>

namespace hana = boost::hana;
using namespace hana::literals;  // contains the _c suffix

struct M {};
struct L {};
struct R {};
struct IF {};
struct P2 {};

struct counter {
  int countM = 2;
  int countL = 1;
  int countR = 0;
  int countIF = 1;
  int countP2 = 1;
};

template <typename ContextT, typename TrueTreeT, typename FalseTreeT>
void Sink(ContextT& contex, hana::tuple<IF, TrueTreeT, FalseTreeT> tup) {
  if (contex.cond())
    Sink(contex, tup[1_c]);
  else
    Sink(contex, tup[2_c]);
}

template <typename ContextT, typename Child0T, typename Child1T>
void Sink(ContextT& contex, hana::tuple<P2, Child0T, Child1T> tup) {
  Sink(contex, tup[1_c]);
  Sink(contex, tup[2_c]);
}

template <typename ContextT>
void Sink(ContextT& contex, hana::tuple<M>) {
  contex.move();
}

template <typename ContextT>
void Sink(ContextT& contex, hana::tuple<L>) {
  contex.left();
}

template <typename ContextT>
void Sink(ContextT& contex, hana::tuple<R>) {
  contex.right();
}

template <typename ContextT, typename... T>
void Sink(ContextT&, hana::tuple<T...>) {}

template <typename T>
counter foobar(T tup, counter c) {
  boost::hana::for_each(
      tup, hana::overload([&](auto x) { c = foobar(x, c); },
                          [&](P2) { --(c.countP2); },
                          [&](IF) { --(c.countIF); }, [&](M) { --(c.countM); },
                          [&](L) { --(c.countL); }, [&](R) { --(c.countR); }));
  return c;
}

struct AntSim {
  mutable int condC = 0;
  mutable int moveC = 0;
  mutable int leftC = 0;
  mutable int rightC = 0;
  bool cond() const {
    condC++;
    return true;
  }
  void move() { moveC += 10; }
  void left() { leftC += 100; }
  void right() { rightC += 1000; }
  int c() { return condC + moveC + leftC + rightC; }
};

int main() {
  auto b =
      hana::tuple<IF,
                  hana::tuple<P2, hana::tuple<M>,
                              hana::tuple<P2, hana::tuple<M>, hana::tuple<R>>>,
                  hana::tuple<L>>{};
  counter co;
  auto b2 = hana::tuple<
      IF, hana::tuple<hana::tuple<P2, hana::tuple<M, M>>, hana::tuple<L>>>{};

  co = foobar(b2, co);
  // ff::f1(b2[0_c], b2[1_c]);
  //    boost::hana::for_each(b, hana::overload(
  //        [&](auto x) {},
  //       [&](P2, auto n1, auto n2){--countIF;},
  //        [&](IF, auto trueNodes, auto falseNodes){--countIF;},
  //        [&](M){--countM;},
  //        [&](L){--countL;},
  //        [&](R){--countR;}
  //    ));

  AntSim as;
  Sink(as, b);
  return as.c();
}


#include "ns_ter.h"

#include <cstdio>
#include <cassert>
#include <iostream>
#include <limits>
#ifndef HAVE_OLD_CPP
# include <unordered_map>
#else
# include <tr1/unordered_map>
namespace std { using std::tr1::unordered_map; }
#endif
#include <set>
#include <boost/functional/hash.hpp>
#include "ns_ter_impl.h"
#include "tdict.h"

static const bool ter_use_average_ref_len = true;

static const unsigned kINSERTIONS = 0;
static const unsigned kDELETIONS = 1;
static const unsigned kSUBSTITUTIONS = 2;
static const unsigned kSHIFTS = 3;
static const unsigned kREF_WORDCOUNT = 4;
static const unsigned kDUMMY_LAST_ENTRY = 5;

using namespace std;

bool TERMetric::IsErrorMetric() const {
  return true;
}

void TERMetric::ComputeSufficientStatistics(const vector<WordID>& hyp,
                                            const vector<vector<WordID> >& refs,
                                            SufficientStats* out) const {
  out->fields.resize(kDUMMY_LAST_ENTRY);
  float best_score = numeric_limits<float>::max();
  unsigned avg_len = 0;
  for (int i = 0; i < refs.size(); ++i)
    avg_len += refs[i].size();
  avg_len /= refs.size();

  for (int i = 0; i < refs.size(); ++i) {
    int subs, ins, dels, shifts;
    NewScorer::TERScorerImpl ter(refs[i]);
    float score = ter.Calculate(hyp, &subs, &ins, &dels, &shifts);
    // cerr << "Component TER cost: " << score << endl;
    if (score < best_score) {
      out->fields[kINSERTIONS] = ins;
      out->fields[kDELETIONS] = dels;
      out->fields[kSUBSTITUTIONS] = subs;
      out->fields[kSHIFTS] = shifts;
      if (ter_use_average_ref_len) {
        out->fields[kREF_WORDCOUNT] = avg_len;
      } else {
        out->fields[kREF_WORDCOUNT] = refs[i].size();
      }

      best_score = score;
    }
  }
}

unsigned TERMetric::SufficientStatisticsVectorSize() const {
  return kDUMMY_LAST_ENTRY;
}

float TERMetric::ComputeScore(const SufficientStats& stats) const {
  float edits = static_cast<float>(stats[kINSERTIONS] + stats[kDELETIONS] + stats[kSUBSTITUTIONS] + stats[kSHIFTS]);
  return edits / static_cast<float>(stats[kREF_WORDCOUNT]);
}

string TERMetric::DetailedScore(const SufficientStats& stats) const {
  char buf[200];
  sprintf(buf, "TER = %.2f, %3.f|%3.f|%3.f|%3.f (len=%3.f)",
     ComputeScore(stats) * 100.0f,
     stats[kINSERTIONS],
     stats[kDELETIONS],
     stats[kSUBSTITUTIONS],
     stats[kSHIFTS],
     stats[kREF_WORDCOUNT]);
  return buf;
}


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2015 The University of Sydney
// 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 the University of Sydney 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 OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
// BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
// OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
// IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

//#include <iostream>
//#include <iomanip>
#include <boost/static_assert.hpp>
#include <comma/math/cyclic.h>
#include "wayline.h"

namespace snark { namespace control {

wayline::wayline( const position_t& from, const position_t& to )
    : v_( ( to - from ).normalized() )
    , heading_( atan2( v_.y(), v_.x() ) )
    , line_( Eigen::ParametrizedLine< double, dimensions >::Through( from, to ) )
    , perpendicular_line_at_end_( v_, to )
    {
        BOOST_STATIC_ASSERT( dimensions == 2 );
    }

double wayline::endpoint_overshoot( const position_t& position ) const
{
    return perpendicular_line_at_end_.signedDistance( position );
}

bool wayline::is_past_endpoint( const position_t& position ) const
{
    return endpoint_overshoot( position ) > 0;
}

double wayline::cross_track_error( const position_t& position ) const
{
    return -line_.signedDistance( position );
}

double wayline::heading_error( double current_heading, double target_heading ) const
{
    return comma::math::cyclic< double >( comma::math::interval< double >( -M_PI, M_PI ), heading_ + target_heading - current_heading )();
}

} } // namespace snark { namespace control {


/*
 * math_util.hpp
 *
 *  Created on: Apr 3, 2013
 *      Author: mdantone
 */

#ifndef MATH_UTIL_HPP_
#define MATH_UTIL_HPP_

#include <glog/logging.h>
#include "opencv2/core/core.hpp"
#include <vector>
#include <limits>
#include <boost/limits.hpp>
#include <boost/random.hpp>
#include <boost/numeric/conversion/bounds.hpp>

namespace learning
{
namespace ps
{
namespace utils
{

cv::Rect inline intersect(const cv::Rect r1, const cv::Rect r2) {
  cv::Rect intersection;

  // find overlapping region
  intersection.x = (r1.x < r2.x) ? r2.x : r1.x;
  intersection.y = (r1.y < r2.y) ? r2.y : r1.y;
  intersection.width = (r1.x + r1.width < r2.x + r2.width) ? r1.x + r1.width
      : r2.x + r2.width;
  intersection.width -= intersection.x;
  intersection.height = (r1.y + r1.height < r2.y + r2.height) ? r1.y
      + r1.height : r2.y + r2.height;
  intersection.height -= intersection.y;

  // check for non-overlapping regions
  if ((intersection.width <= 0) || (intersection.height <= 0)) {
    intersection = cvRect(0, 0, 0, 0);
  }
  return intersection;
}

float inline dist(const cv::Point_<float>& a, const cv::Point_<float>&b) {
  return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y));
}

// calculate the mean and the mean norm.
void inline calculate_mean(const std::vector<cv::Point_<float> >& offsets,
    cv::Point_<float>* mean, float* mean_norm  ) {
  CHECK_GT(offsets.size(), 0);

  // calculate the mean and the mean norm.
  cv::Point_<float> mean_offset(0,0);
  float m_norm = 0;
  for(int i=0; i < offsets.size(); i++) {
    const cv::Point_<float>& offset = offsets[i];
    m_norm += cv::norm(offset);
    mean_offset += offset;
  }
  mean_offset.x /= offsets.size();
  mean_offset.y /= offsets.size();
  m_norm /= offsets.size();

  *mean = mean_offset;
  *mean_norm = m_norm;
}

int inline get_min_index(const std::vector<cv::Mat_<float> >& scores,
    const cv::Point p, const std::vector<float>& weights ) {
  float min_v = boost::numeric::bounds<double>::highest();
  int min_index = 0;
  for(int i=0; i < scores.size(); i++) {

    float s = scores[i].at<float>(p) + weights[i];
    if(s < min_v ) {
      min_v = s;
      min_index = i;
    }
  }
  return min_index;
}

float inline get_min( const std::vector<cv::Mat_<float> >& scores,
    const std::vector<float>& weights, cv::Point p) {
  float min_v = boost::numeric::bounds<double>::highest();
  for(int i=0; i < scores.size(); i++) {
    if(weights[i] >= 0)
      min_v = MIN(min_v, scores[i].at<float>(p)+ weights[i] );
  }
  return min_v;
}

void inline reduce_min( const std::vector<cv::Mat_<float> >& scores,
    const std::vector<float>& weights,
    cv::Mat_<float>& score) {
  CHECK_EQ(scores.size(), weights.size());
  score = cv::Mat::zeros(scores[0].size(), cv::DataType<float>::type);
  for(int x = 0; x < score.cols; ++x) {
    for(int y = 0; y < score.rows; ++y) {
      const cv::Point p(x,y);
      score.at<float>(p) = get_min( scores, weights,  p);
    }
  }
}

float inline get_min( const std::vector<cv::Mat_<float> >& scores, cv::Point p) {
  float min_v = boost::numeric::bounds<double>::highest();
  for(int i=0; i < scores.size(); i++) {
    min_v = MIN(min_v, scores[i].at<float>(p) );
  }
  return min_v;
}

void inline reduce_min( const std::vector<cv::Mat_<float> >& scores,
    cv::Mat_<float>& score) {
  score = cv::Mat::zeros(scores[0].size(), cv::DataType<float>::type);
  for(int x = 0; x < score.cols; ++x) {
    for(int y = 0; y < score.rows; ++y) {
      const cv::Point p(x,y);
      score.at<float>(p) = get_min( scores, p);
    }
  }
}

void inline reduce_min( const std::vector<cv::Mat_<float> >& scores,
    const std::vector<cv::Mat_<int> > d_xs,
    const std::vector<cv::Mat_<int> > d_ys,
    const std::vector<float>& weights,
    cv::Mat& score,  cv::Mat_<int>& d_x, cv::Mat_<int>& d_y) {
  score = cv::Mat::zeros(scores[0].size(), cv::DataType<float>::type);
  d_x   = cv::Mat::zeros(scores[0].size(), cv::DataType<int>::type);
  d_y   = cv::Mat::zeros(scores[0].size(), cv::DataType<int>::type);
  for(int x = 0; x < score.cols; ++x) {
    for(int y = 0; y < score.rows; ++y) {
      const cv::Point p(x,y);
      int min_index = get_min_index(scores, p, weights);
      score.at<float>(p) = scores[min_index].at<float>(p) + weights[min_index];
      d_x.at<int>(p) = d_xs[min_index].at<int>(p);
      d_y.at<int>(p) = d_ys[min_index].at<int>(p);
    }
  }
}

} // namespace util

} //namespace ps

} //namespace learning

#endif /* MATH_UTIL_HPP_ */


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

#include <quaternion_operation/quaternion_operation.h>

#include <traffic_simulator/math/bounding_box.hpp>

// headers in Eigen
#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <vector>

namespace traffic_simulator
{
namespace math
{
/**
 * @brief Get the Polygon Distance object
 *
 * @param pose0 pose of the first bounding box
 * @param bbox0 size of the first bounding box
 * @param pose1 pose of the second bounding box
 * @param bbox1 size of the second bounding box
 * @retval boost::none bounding box intersects
 * @retval 0 <= distance between two bounding boxes
 */
boost::optional<double> getPolygonDistance(
  const geometry_msgs::msg::Pose & pose0, const traffic_simulator_msgs::msg::BoundingBox & bbox0,
  const geometry_msgs::msg::Pose & pose1, const traffic_simulator_msgs::msg::BoundingBox & bbox1)
{
  const auto poly0 = get2DPolygon(pose0, bbox0);
  const auto poly1 = get2DPolygon(pose1, bbox1);
  if (boost::geometry::intersects(poly0, poly1)) {
    return boost::none;
  }
  if (boost::geometry::intersects(poly1, poly0)) {
    return boost::none;
  }
  if (boost::geometry::disjoint(poly0, poly1)) {
    return boost::geometry::distance(poly0, poly1);
  }
  return boost::none;
}

const boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double>> get2DPolygon(
  const geometry_msgs::msg::Pose & pose, const traffic_simulator_msgs::msg::BoundingBox & bbox)
{
  auto points = transformPoints(pose, getPointsFromBbox(bbox));
  typedef boost::geometry::model::d2::point_xy<double> bg_point;
  boost::geometry::model::polygon<bg_point> poly;
  poly.outer().push_back(bg_point(points[0].x, points[0].y));
  poly.outer().push_back(bg_point(points[1].x, points[1].y));
  poly.outer().push_back(bg_point(points[2].x, points[2].y));
  poly.outer().push_back(bg_point(points[3].x, points[3].y));
  poly.outer().push_back(bg_point(points[0].x, points[0].y));
  return poly;
}

std::vector<geometry_msgs::msg::Point> getPointsFromBbox(
  traffic_simulator_msgs::msg::BoundingBox bbox)
{
  std::vector<geometry_msgs::msg::Point> points;
  geometry_msgs::msg::Point p0;
  p0.x = bbox.center.x + bbox.dimensions.x * 0.5;
  p0.y = bbox.center.y + bbox.dimensions.y * 0.5;
  p0.z = bbox.center.z + bbox.dimensions.z * 0.5;
  points.emplace_back(p0);
  geometry_msgs::msg::Point p1;
  p1.x = bbox.center.x - bbox.dimensions.x * 0.5;
  p1.y = bbox.center.y + bbox.dimensions.y * 0.5;
  p1.z = bbox.center.z + bbox.dimensions.z * 0.5;
  points.emplace_back(p1);
  geometry_msgs::msg::Point p2;
  p2.x = bbox.center.x - bbox.dimensions.x * 0.5;
  p2.y = bbox.center.y - bbox.dimensions.y * 0.5;
  p2.z = bbox.center.z + bbox.dimensions.z * 0.5;
  points.emplace_back(p2);
  geometry_msgs::msg::Point p3;
  p3.x = bbox.center.x + bbox.dimensions.x * 0.5;
  p3.y = bbox.center.y - bbox.dimensions.y * 0.5;
  p3.z = bbox.center.z + bbox.dimensions.z * 0.5;
  points.emplace_back(p3);
  return points;
}
}  // namespace math
}  // namespace traffic_simulator


//  Copyright 2020 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#include <iostream>
#include <benchmark/benchmark.h>
#include <nil/crypto3/multiprecision/cpp_int.hpp>
#include <nil/crypto3/multiprecision/gmp.hpp>
#include <boost/random.hpp>
#include <cmath>

#include <immintrin.h>

using namespace nil::crypto3::multiprecision;
using namespace boost::random;

namespace boost {
    namespace multiprecision {
        namespace backends {

            template<unsigned MinBits1,
                     unsigned MaxBits1,
                     cpp_integer_type SignType1,
                     cpp_int_check_type Checked1,
                     class Allocator1>
            inline BOOST_MP_CXX14_CONSTEXPR typename boost::enable_if_c<
                !is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>>::value>::type
                eval_gcd_old(cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& result,
                             const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& a,
                             const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& b) {
                using default_ops::eval_get_sign;
                using default_ops::eval_is_zero;
                using default_ops::eval_lsb;

                if (a.size() == 1) {
                    eval_gcd(result, b, *a.limbs());
                    return;
                }
                if (b.size() == 1) {
                    eval_gcd(result, a, *b.limbs());
                    return;
                }

                cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> u(a), v(b);

                int s = eval_get_sign(u);

                /* GCD(0,x) := x */
                if (s < 0) {
                    u.negate();
                } else if (s == 0) {
                    result = v;
                    return;
                }
                s = eval_get_sign(v);
                if (s < 0) {
                    v.negate();
                } else if (s == 0) {
                    result = u;
                    return;
                }

                /* Let shift := lg K, where K is the greatest power of 2
          dividing both u and v. */

                unsigned us = eval_lsb(u);
                unsigned vs = eval_lsb(v);
                int shift = (std::min)(us, vs);
                eval_right_shift(u, us);
                eval_right_shift(v, vs);

                do {
                    /* Now u and v are both odd, so diff(u, v) is even.
              Let u = min(u, v), v = diff(u, v)/2. */
                    s = u.compare(v);
                    if (s > 0)
                        u.swap(v);
                    if (s == 0)
                        break;

                    while (((u.size() + 2 < v.size()) && (v.size() * 100 / u.size() > 105)) ||
                           ((u.size() <= 2) && (v.size() > 4))) {
                        //
                        // Speical case: if u and v differ considerably in size, then a Euclid step
                        // is more efficient as we reduce v by several limbs in one go.
                        // Unfortunately it requires an expensive long division:
                        //
                        eval_modulus(v, v, u);
                        u.swap(v);
                    }
                    if (v.size() <= 2) {
                        //
                        // Special case: if v has no more than 2 limbs
                        // then we can reduce u and v to a pair of integers and perform
                        // direct integer gcd:
                        //
                        if (v.size() == 1)
                            u = eval_gcd(*v.limbs(), *u.limbs());
                        else {
                            double_limb_type i = v.limbs()[0] | (static_cast<double_limb_type>(v.limbs()[1])
                                                                 << sizeof(limb_type) * CHAR_BIT);
                            double_limb_type j = (u.size() == 1) ?
                                                     *u.limbs() :
                                                     u.limbs()[0] | (static_cast<double_limb_type>(u.limbs()[1])
                                                                     << sizeof(limb_type) * CHAR_BIT);
                            u = eval_gcd(i, j);
                        }
                        break;
                    }
                    //
                    // Regular binary gcd case:
                    //
                    eval_subtract(v, u);
                    vs = eval_lsb(v);
                    eval_right_shift(v, vs);
                } while (true);

                result = u;
                eval_left_shift(result, shift);
            }

        }    // namespace backends
    }        // namespace multiprecision
}    // namespace boost

template<class T>
std::tuple<std::vector<T>, std::vector<T>, std::vector<T>>& get_test_vector(unsigned bits) {
    static std::map<unsigned, std::tuple<std::vector<T>, std::vector<T>, std::vector<T>>> data;

    std::tuple<std::vector<T>, std::vector<T>, std::vector<T>>& result = data[bits];

    if (std::get<0>(result).size() == 0) {
        mt19937 mt;
        uniform_int_distribution<T> ui(T(1) << (bits - 1), T(1) << bits);

        std::vector<T>& a = std::get<0>(result);
        std::vector<T>& b = std::get<1>(result);
        std::vector<T>& c = std::get<2>(result);

        for (unsigned i = 0; i < 1000; ++i) {
            a.push_back(ui(mt));
            b.push_back(ui(mt));
            if (b.back() > a.back())
                b.back().swap(a.back());
            c.push_back(0);
        }
    }
    return result;
}

template<class T>
std::vector<T>& get_test_vector_a(unsigned bits) {
    return std::get<0>(get_test_vector<T>(bits));
}
template<class T>
std::vector<T>& get_test_vector_b(unsigned bits) {
    return std::get<1>(get_test_vector<T>(bits));
}
template<class T>
std::vector<T>& get_test_vector_c(unsigned bits) {
    return std::get<2>(get_test_vector<T>(bits));
}

template<typename T>
static void BM_gcd_old(benchmark::State& state) {
    int bits = state.range(0);

    std::vector<T>& a = get_test_vector_a<T>(bits);
    std::vector<T>& b = get_test_vector_b<T>(bits);
    std::vector<T>& c = get_test_vector_c<T>(bits);

    for (auto _ : state) {
        for (unsigned i = 0; i < a.size(); ++i)
            eval_gcd_old(c[i].backend(), a[i].backend(), b[i].backend());
    }
    state.SetComplexityN(bits);
}

template<typename T>
static void BM_gcd_current(benchmark::State& state) {
    int bits = state.range(0);

    std::vector<T>& a = get_test_vector_a<T>(bits);
    std::vector<T>& b = get_test_vector_b<T>(bits);
    std::vector<T>& c = get_test_vector_c<T>(bits);

    for (auto _ : state) {
        for (unsigned i = 0; i < a.size(); ++i)
            eval_gcd(c[i].backend(), a[i].backend(), b[i].backend());
    }
    state.SetComplexityN(bits);
}

constexpr unsigned lower_range = 512;
constexpr unsigned upper_range = 1 << 15;

BENCHMARK_TEMPLATE(BM_gcd_old, cpp_int)
    ->RangeMultiplier(2)
    ->Range(lower_range, upper_range)
    ->Unit(benchmark::kMillisecond)
    ->Complexity();
BENCHMARK_TEMPLATE(BM_gcd_current, cpp_int)
    ->RangeMultiplier(2)
    ->Range(lower_range, upper_range)
    ->Unit(benchmark::kMillisecond)
    ->Complexity();
BENCHMARK_TEMPLATE(BM_gcd_old, cpp_int)
    ->RangeMultiplier(2)
    ->Range(lower_range, upper_range)
    ->Unit(benchmark::kMillisecond)
    ->Complexity();
BENCHMARK_TEMPLATE(BM_gcd_current, mpz_int)
    ->RangeMultiplier(2)
    ->Range(lower_range, upper_range)
    ->Unit(benchmark::kMillisecond)
    ->Complexity();
BENCHMARK_TEMPLATE(BM_gcd_current, mpz_int)
    ->RangeMultiplier(2)
    ->Range(lower_range, upper_range)
    ->Unit(benchmark::kMillisecond)
    ->Complexity();

BENCHMARK_MAIN();


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

#include "platooning_tactical_plugin/platooning_tactical_plugin.h"

#include <ros/ros.h>
#include <string>
#include <algorithm>
#include <memory>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <lanelet2_core/geometry/Point.h>
#include <trajectory_utils/trajectory_utils.h>
#include <trajectory_utils/conversions/conversions.h>
#include <sstream>
#include <carma_utils/containers/containers.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <platooning_tactical_plugin/smoothing/SplineI.h>
#include <platooning_tactical_plugin/smoothing/BSpline.h>
#include <platooning_tactical_plugin/log/log.h>
#include <platooning_tactical_plugin/smoothing/filters.h>
#include <unordered_set>

using oss = std::ostringstream;

namespace platooning_tactical_plugin
{
PlatooningTacticalPlugin::PlatooningTacticalPlugin(carma_wm::WorldModelConstPtr wm, PlatooningTacticalPluginConfig config,
                                           PublishPluginDiscoveryCB plugin_discovery_publisher)
  : wm_(wm), config_(config), plugin_discovery_publisher_(plugin_discovery_publisher)
{
  plugin_discovery_msg_.name = "PlatooningTacticalPlugin";
  plugin_discovery_msg_.versionId = "v1.0";
  plugin_discovery_msg_.available = true;
  plugin_discovery_msg_.activated = false;
  plugin_discovery_msg_.type = cav_msgs::Plugin::TACTICAL;
  plugin_discovery_msg_.capability = "tactical_plan/plan_trajectory";
}

bool PlatooningTacticalPlugin::onSpin()
{
  plugin_discovery_publisher_(plugin_discovery_msg_);
  return true;
}

bool PlatooningTacticalPlugin::plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest& req,
                                              cav_srvs::PlanTrajectoryResponse& resp)
{
  ros::WallTime start_time = ros::WallTime::now(); // Start timeing the execution time for planning so it can be logged

  lanelet::BasicPoint2d veh_pos(req.vehicle_state.X_pos_global, req.vehicle_state.Y_pos_global);
  double current_downtrack = wm_->routeTrackPos(veh_pos).downtrack;

  auto points_and_target_speeds = maneuvers_to_points(req.maneuver_plan.maneuvers, current_downtrack, wm_); // Convert maneuvers to points

  ROS_DEBUG_STREAM("points_and_target_speeds: " << points_and_target_speeds.size());

  auto downsampled_points =
      carma_utils::containers::downsample_vector(points_and_target_speeds, config_.downsample_ratio);

  ROS_DEBUG_STREAM("downsample_points: " << downsampled_points.size());

  ROS_DEBUG_STREAM("PlanTrajectory");

  cav_msgs::TrajectoryPlan trajectory;
  trajectory.header.frame_id = "map";
  trajectory.header.stamp = ros::Time::now();
  trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()());

  trajectory.trajectory_points = compose_trajectory_from_centerline(downsampled_points, req.vehicle_state); // Compute the trajectory
  trajectory.initial_longitudinal_velocity = req.vehicle_state.longitudinal_vel;

  resp.trajectory_plan = trajectory;
  for (int i=0; i<req.maneuver_plan.maneuvers.size(); i++){
    if (req.maneuver_plan.maneuvers[i].type == cav_msgs::Maneuver::LANE_FOLLOWING){
      resp.related_maneuvers.push_back(i);
      break;
    }
  }

  resp.maneuver_status.push_back(cav_srvs::PlanTrajectory::Response::MANEUVER_IN_PROGRESS);

  ros::WallTime end_time = ros::WallTime::now(); // Planning complete

  ros::WallDuration duration = end_time - start_time;
  ROS_DEBUG_STREAM("ExecutionTime: " << duration.toSec());

  return true;
}

std::vector<double> PlatooningTacticalPlugin::apply_speed_limits(const std::vector<double> speeds,
                                                             const std::vector<double> speed_limits)
{

  if (speeds.size() != speed_limits.size())
  {
    throw std::invalid_argument("Speeds and speed limit lists not same size");
  }
  std::vector<double> out;
  for (size_t i = 0; i < speeds.size(); i++)
  {
    out.push_back(std::min(speeds[i], speed_limits[i]));
  }

  return out;
}

Eigen::Isometry2d PlatooningTacticalPlugin::compute_heading_frame(const lanelet::BasicPoint2d& p1,
                                                              const lanelet::BasicPoint2d& p2)
{
  Eigen::Rotation2Dd yaw(atan2(p2.y() - p1.y(), p2.x() - p1.x()));

  return carma_wm::geometry::build2dEigenTransform(p1, yaw);
}

std::vector<PointSpeedPair> PlatooningTacticalPlugin::constrain_to_time_boundary(const std::vector<PointSpeedPair>& points,
                                                                             double time_span)
{
  std::vector<lanelet::BasicPoint2d> basic_points;
  std::vector<double> speeds;
  splitPointSpeedPairs(points, &basic_points, &speeds);

  std::vector<double> downtracks = carma_wm::geometry::compute_arc_lengths(basic_points);

  size_t time_boundary_exclusive_index =
      trajectory_utils::time_boundary_index(downtracks, speeds, config_.trajectory_time_length);

  if (time_boundary_exclusive_index == 0)
  {
    throw std::invalid_argument("No points to fit in timespan");
  }

  std::vector<PointSpeedPair> time_bound_points;
  time_bound_points.reserve(time_boundary_exclusive_index);

  if (time_boundary_exclusive_index == points.size())
  {
    time_bound_points.insert(time_bound_points.end(), points.begin(),
                             points.end());  // All points fit within time boundary
  }
  else
  {
    time_bound_points.insert(time_bound_points.end(), points.begin(),
                             points.begin() + time_boundary_exclusive_index - 1);  // Limit points by time boundary
  }

  return time_bound_points;
}

double PlatooningTacticalPlugin::get_adaptive_lookahead(double velocity){

  // lookahead:
  // v<10kph:  5m
  // 10kph<v<50kph:  0.5*v
  // v>50kph:  25m

  double lookahead = config_.minimum_lookahead_distance;

  if (velocity < config_.minimum_lookahead_speed)
  {
    lookahead = config_.minimum_lookahead_distance;
  }
  else if (velocity >= config_.minimum_lookahead_speed && velocity < config_.maximum_lookahead_speed)
  {
    lookahead = config_.lookahead_ratio * velocity;
  }
  else lookahead = config_.maximum_lookahead_distance;

  return lookahead;

}

std::vector<double> PlatooningTacticalPlugin::get_lookahead_speed(const std::vector<lanelet::BasicPoint2d>& points, const std::vector<double>& speeds, const double& lookahead){

  if (lookahead < config_.minimum_lookahead_distance)
  {
    throw std::invalid_argument("Invalid lookahead value");
  }

  if (speeds.size() < 1)
  {
    throw std::invalid_argument("Invalid speeds vector");
  }

  if (speeds.size() != points.size())
  {
    throw std::invalid_argument("Speeds and Points lists not same size");
  }

  std::vector<double> out;
  out.reserve(speeds.size());

  for (int i = 0; i < points.size(); i++)
  {
    int idx = i;
    double min_dist = std::numeric_limits<double>::max();
    for (int j=i+1; j < points.size(); j++){
      double dist = lanelet::geometry::distance2d(points[i],points[j]);
      if (abs(lookahead - dist) <= min_dist){
        idx = j;
        min_dist = abs(lookahead - dist);
      }
    }
    out.push_back(speeds[idx]);
  }

  return out;
}

std::vector<cav_msgs::TrajectoryPlanPoint> PlatooningTacticalPlugin::trajectory_from_points_times_orientations(
    const std::vector<lanelet::BasicPoint2d>& points, const std::vector<double>& times, const std::vector<double>& yaws,
    ros::Time startTime)
{
  if (points.size() != times.size() || points.size() != yaws.size())
  {
    throw std::invalid_argument("All input vectors must have the same size");
  }

  std::vector<cav_msgs::TrajectoryPlanPoint> traj;
  traj.reserve(points.size());

  for (int i = 0; i < points.size(); i++)
  {
    cav_msgs::TrajectoryPlanPoint tpp;
    ros::Duration relative_time(times[i]);
    tpp.target_time = startTime + relative_time;
    tpp.x = points[i].x();
    tpp.y = points[i].y();
    tpp.yaw = yaws[i];

    tpp.controller_plugin_name = "default";
    tpp.planner_plugin_name = plugin_discovery_msg_.name;

    traj.push_back(tpp);
  }

  return traj;
}

std::vector<PointSpeedPair> PlatooningTacticalPlugin::maneuvers_to_points(const std::vector<cav_msgs::Maneuver>& maneuvers,
                                                                      double max_starting_downtrack,
                                                                      const carma_wm::WorldModelConstPtr& wm)
{
  std::vector<PointSpeedPair> points_and_target_speeds;
  std::unordered_set<lanelet::Id> visited_lanelets;

  bool first = true;
  ROS_DEBUG_STREAM("VehDowntrack: " << max_starting_downtrack);
  for (const auto& manuever : maneuvers)
  {
    if (manuever.type != cav_msgs::Maneuver::LANE_FOLLOWING)
    {
      throw std::invalid_argument("Platooning tactical plugin does not support this maneuver type");
    }

    if (manuever.lane_following_maneuver.parameters.neogition_type == cav_msgs::ManeuverParameters::NO_NEGOTIATION)
    {
      throw std::invalid_argument("NO_NEGOTIATION maneuver is not supported by this plugin");
    }

    if (manuever.lane_following_maneuver.parameters.neogition_type == cav_msgs::ManeuverParameters::PLATOONING)
    {
      if ( manuever.lane_following_maneuver.start_speed != manuever.lane_following_maneuver.end_speed)
      {
        throw std::invalid_argument("Invalid Platooning Maneuver");
      }
    }

    cav_msgs::LaneFollowingManeuver lane_following_maneuver = manuever.lane_following_maneuver;

    double starting_downtrack = lane_following_maneuver.start_dist;
    if (first)
    {
      if (starting_downtrack > max_starting_downtrack)
      {
        starting_downtrack = max_starting_downtrack;
      }
      first = false;
    }

    ROS_DEBUG_STREAM("Used downtrack: " << starting_downtrack);

    auto lanelets = wm->getLaneletsBetween(starting_downtrack, lane_following_maneuver.end_dist, true);

    ROS_DEBUG_STREAM("Maneuver");
    std::vector<lanelet::ConstLanelet> lanelets_to_add;
    for (auto l : lanelets)
    {
      ROS_DEBUG_STREAM("Lanelet ID: " << l.id());
      if (visited_lanelets.find(l.id()) == visited_lanelets.end())
      {
        lanelets_to_add.push_back(l);
        visited_lanelets.insert(l.id());
      }
    }

    lanelet::BasicLineString2d route_geometry = carma_wm::geometry::concatenate_lanelets(lanelets_to_add);

    first = true;
    for (auto p : route_geometry)
    {
      if (first && points_and_target_speeds.size() != 0)
      {
        first = false;
        continue;  // Skip the first point if we have already added points from a previous maneuver to avoid duplicates
      }
      PointSpeedPair pair;
      pair.point = p;
      pair.speed = lane_following_maneuver.end_speed;
      points_and_target_speeds.push_back(pair);
    }
  }

  return points_and_target_speeds;
}

int PlatooningTacticalPlugin::getNearestPointIndex(const std::vector<PointSpeedPair>& points,
                                               const cav_msgs::VehicleState& state)
{
  lanelet::BasicPoint2d veh_point(state.X_pos_global, state.Y_pos_global);
  ROS_DEBUG_STREAM("veh_point: " << veh_point.x() << ", " << veh_point.y());
  double min_distance = std::numeric_limits<double>::max();
  int i = 0;
  int best_index = 0;
  for (const auto& p : points)
  {
    double distance = lanelet::geometry::distance2d(p.point, veh_point);
    ROS_DEBUG_STREAM("distance: " << distance);
    ROS_DEBUG_STREAM("p: " << p.point.x() << ", " << p.point.y());
    if (distance < min_distance)
    {
      best_index = i;
      min_distance = distance;
    }
    i++;
  }

  return best_index;
}

void PlatooningTacticalPlugin::splitPointSpeedPairs(const std::vector<PointSpeedPair>& points,
                                                std::vector<lanelet::BasicPoint2d>* basic_points,
                                                std::vector<double>* speeds)
{
  basic_points->reserve(points.size());
  speeds->reserve(points.size());

  for (const auto& p : points)
  {
    basic_points->push_back(p.point);
    speeds->push_back(p.speed);
  }
}

std::unique_ptr<smoothing::SplineI>
PlatooningTacticalPlugin::compute_fit(const std::vector<lanelet::BasicPoint2d>& basic_points)
{
  if (basic_points.size() < 3)
  {
    ROS_WARN_STREAM("Insufficient Spline Points");
    return nullptr;
  }

  std::unique_ptr<smoothing::SplineI> spl = std::make_unique<smoothing::BSpline>();
  spl->setPoints(basic_points);

  return spl;
}

std::vector<cav_msgs::TrajectoryPlanPoint> PlatooningTacticalPlugin::compose_trajectory_from_centerline(
    const std::vector<PointSpeedPair>& points, const cav_msgs::VehicleState& state)
{
  ROS_DEBUG_STREAM("VehicleState: "
                   << " x: " << state.X_pos_global << " y: " << state.Y_pos_global << " yaw: " << state.orientation
                   << " speed: " << state.longitudinal_vel);

  ROS_DEBUG_STREAM("points size: " << points.size());

  log::printDebugPerLine(points, &log::pointSpeedPairToStream);

  int nearest_pt_index = getNearestPointIndex(points, state);

  ROS_DEBUG_STREAM("NearestPtIndex: " << nearest_pt_index);

  std::vector<PointSpeedPair> future_points(points.begin() + nearest_pt_index + 1, points.end()); // Points in front of current vehicle position

  auto time_bound_points = constrain_to_time_boundary(future_points, 30);

  ROS_DEBUG_STREAM("time_bound_points: " << time_bound_points.size());

  log::printDebugPerLine(time_bound_points, &log::pointSpeedPairToStream);

  ROS_DEBUG("Got basic points ");

  std::vector<double> final_yaw_values;
  std::vector<double> final_actual_speeds;
  std::vector<lanelet::BasicPoint2d> all_sampling_points;

  std::vector<double> speed_limits;
  std::vector<lanelet::BasicPoint2d> curve_points;
  splitPointSpeedPairs(time_bound_points, &curve_points, &speed_limits);

  std::unique_ptr<smoothing::SplineI> fit_curve = compute_fit(curve_points); // Compute splines based on curve points

    if (!fit_curve)
    {
      throw std::invalid_argument("Could not fit a spline curve along the given trajectory!");
    }

    ROS_DEBUG("Got fit");

    ROS_DEBUG_STREAM("speed_limits.size() " << speed_limits.size());

    std::vector<lanelet::BasicPoint2d> sampling_points;
    sampling_points.reserve(1 + curve_points.size() * 2);

    std::vector<double> distributed_speed_limits;
    distributed_speed_limits.reserve(1 + curve_points.size() * 2);

    // compute total length of the trajectory to get correct number of points
    // we expect using curve_resample_step_size
    std::vector<double> downtracks_raw = carma_wm::geometry::compute_arc_lengths(curve_points);

    int total_step_along_curve = static_cast<int>(downtracks_raw.back() / config_.curve_resample_step_size);

    int current_speed_index = 0;
    size_t total_point_size = curve_points.size();

    double step_threshold_for_next_speed = (double)total_step_along_curve / (double)total_point_size;
    double scaled_steps_along_curve = 0.0; // from 0 (start) to 1 (end) for the whole trajectory

    for (size_t steps_along_curve = 0; steps_along_curve < total_step_along_curve; steps_along_curve++) // Resample curve at tighter resolution
    {
      lanelet::BasicPoint2d p = (*fit_curve)(scaled_steps_along_curve);

      sampling_points.push_back(p);
      if ((double)steps_along_curve > step_threshold_for_next_speed)
      {
        step_threshold_for_next_speed += (double)total_step_along_curve / (double) total_point_size;
        current_speed_index ++;
      }
      distributed_speed_limits.push_back(speed_limits[current_speed_index]); // Identify speed limits for resampled points
      scaled_steps_along_curve += 1.0/total_step_along_curve; //adding steps_along_curve_step_size
    }

  log::printDebugPerLine(sampling_points, &log::basicPointToStream);

  std::vector<double> yaw_values = carma_wm::geometry::compute_tangent_orientations(sampling_points);

  std::vector<double> curvatures = carma_wm::geometry::local_circular_arc_curvatures(
      sampling_points, config_.curvature_calc_lookahead_count);

  curvatures = smoothing::moving_average_filter(curvatures, config_.moving_average_window_size);

  log::printDoublesPerLineWithPrefix("curvatures[i]: ", curvatures);

  std::vector<double> ideal_speeds =
      trajectory_utils::constrained_speeds_for_curvatures(curvatures, config_.lateral_accel_limit);

  log::printDoublesPerLineWithPrefix("ideal_speeds: ", ideal_speeds);

  std::vector<double> actual_speeds = apply_speed_limits(ideal_speeds, distributed_speed_limits);

  log::printDoublesPerLineWithPrefix("actual_speeds: ", actual_speeds);

  log::printDoublesPerLineWithPrefix("yaw_values[i]: ", yaw_values);

  // Drop last point
  final_yaw_values.insert(final_yaw_values.end(), yaw_values.begin(), yaw_values.end());
  all_sampling_points.insert(all_sampling_points.end(), sampling_points.begin(), sampling_points.end() );
  final_actual_speeds.insert(final_actual_speeds.end(), actual_speeds.begin(), actual_speeds.end());

  ROS_DEBUG("Appended to final");

  ROS_DEBUG("Processed all points in computed fit");

  if (all_sampling_points.size() == 0)
  {
    ROS_WARN_STREAM("No trajectory points could be generated");
    return {};
  }

  log::printDoublesPerLineWithPrefix("final_actual_speeds[i]: ", final_actual_speeds);

  log::printDoublesPerLineWithPrefix("final_yaw_values[i]: ", final_yaw_values);

  // Find Lookahead Distance based on Velocity
  double lookahead_distance = get_adaptive_lookahead(state.longitudinal_vel);

  ROS_DEBUG_STREAM("Lookahead distance at current speed: " << lookahead_distance);

  // Apply lookahead speeds
  final_actual_speeds = get_lookahead_speed(all_sampling_points, final_actual_speeds, lookahead_distance);

  // Add current vehicle point to front of the trajectory
  lanelet::BasicPoint2d cur_veh_point(state.X_pos_global, state.Y_pos_global);

  all_sampling_points.insert(all_sampling_points.begin(),
                             cur_veh_point);  // Add current vehicle position to front of sample points

  final_actual_speeds.insert(final_actual_speeds.begin(), std::max(state.longitudinal_vel, config_.minimum_speed));

  final_yaw_values.insert(final_yaw_values.begin(), state.orientation);

  log::printDoublesPerLineWithPrefix("pre_smoot[i]: ", final_actual_speeds);

  // Compute points to local downtracks
  std::vector<double> downtracks = carma_wm::geometry::compute_arc_lengths(all_sampling_points);

  log::printDoublesPerLineWithPrefix("post_shift[i]: ", final_actual_speeds);

  // Apply accel limits
  final_actual_speeds = trajectory_utils::apply_accel_limits_by_distance(downtracks, final_actual_speeds,
                                                                         config_.max_accel, config_.max_accel);
  log::printDoublesPerLineWithPrefix("postAccel[i]: ", final_actual_speeds);

  final_actual_speeds = smoothing::moving_average_filter(final_actual_speeds, config_.moving_average_window_size);
  log::printDoublesPerLineWithPrefix("post_average[i]: ", final_actual_speeds);

  for (auto& s : final_actual_speeds)  // Limit minimum speed. TODO how to handle stopping?
  {
    s = std::max(s, config_.minimum_speed);
  }

  log::printDoublesPerLineWithPrefix("post_min_speed[i]: ", final_actual_speeds);
  // Convert speeds to times
  std::vector<double> times;
  trajectory_utils::conversions::speed_to_time(downtracks, final_actual_speeds, &times);

  log::printDoublesPerLineWithPrefix("times[i]: ", times);

  // Build trajectory points
  // TODO When more plugins are implemented that might share trajectory planning the start time will need to be based
  // off the last point in the plan if an earlier plan was provided
  std::vector<cav_msgs::TrajectoryPlanPoint> traj_points =
      trajectory_from_points_times_orientations(all_sampling_points, times, final_yaw_values, ros::Time::now());

  return traj_points;
}

}  // namespace platooning_tactical_plugin


#include "camera_array/FiducialRewardFunction.h"
#include "fiducials/FiducialCommon.h"
#include <boost/foreach.hpp>

using namespace argus;

namespace argus
{

FiducialRewardFunction::FiducialRewardFunction( const FiducialDetectionModel::Ptr& model,
                                                const RobotArrayTransitionFunction::Ptr& trans )
: detectionModel( model ), transitionFunction( trans )
{}

double FiducialRewardFunction::CalculateReward( const RobotTargetState& state,
                                                const CameraArrayAction& action )
{
	typedef argus_msgs::FiducialDetection Detection;
	typedef std::vector<Detection> Detections;

	RobotTargetState next = transitionFunction->Transition( state, action );

	double reward = 0;

	// Calculate observations for each active camera
	BOOST_FOREACH( const std::string cameraName, next.array.activeCameras )
	{
		double cameraSum = 0;
		BOOST_FOREACH( const RobotTargetState::TargetMap::value_type& item, next.targets )
		{
			const std::string& targetName = item.first;
			const PoseSE3& targetRelPose = item.second.pose;
			Detections detections =
				detectionModel->GenerateDetections( cameraName,
				                                    targetName,
				                                    targetRelPose );
			BOOST_FOREACH( Detection& detection, detections )
			{
				double r = FindMinDistance( detection.points );
				reward += r;
				cameraSum += r;
			}
		}

	}
	return reward;
}

}


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_ElasticBasicBivariateDistribution.hpp
//! \author Luke Kersting
//! \brief  The elastic basic bivariate dist. class decl.
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_ELASTIC_BASIC_BIVARIATE_DISTRIBUTION_HPP
#define MONTE_CARLO_ELASTIC_BASIC_BIVARIATE_DISTRIBUTION_HPP

// Boost Includes
#include <boost/units/physical_dimensions/energy.hpp>

// FRENSIE Includes
#include "Utility_InterpolatedFullyTabularBasicBivariateDistribution.hpp"

namespace MonteCarlo{

/*! The unit-aware interpolated fully tabular two-dimensional distribution
 * \ingroup two_d_distribution
 */
template<typename TwoDGridPolicy,
         typename PrimaryIndependentUnit,
         typename SecondaryIndependentUnit,
         typename DependentUnit>
class UnitAwareElasticBasicBivariateDistribution : public Utility::UnitAwareInterpolatedFullyTabularBasicBivariateDistribution<TwoDGridPolicy,PrimaryIndependentUnit,SecondaryIndependentUnit,DependentUnit>
{
  // Only allow construction when the primary independent unit corresponds to energy
  RESTRICT_UNIT_TO_BOOST_DIMENSION( PrimaryIndependentUnit, energy_dimension );

  // The typedef for this type
  typedef UnitAwareElasticBasicBivariateDistribution<TwoDGridPolicy,PrimaryIndependentUnit,SecondaryIndependentUnit,DependentUnit> ThisType;

  // The parent distribution type
  typedef Utility::UnitAwareInterpolatedFullyTabularBasicBivariateDistribution<TwoDGridPolicy,PrimaryIndependentUnit,SecondaryIndependentUnit,DependentUnit> BaseType;

  // The base one-dimensional distribution type (UnitAwareTabularUnivariateDist)
  typedef typename BaseType::BaseUnivariateDistributionType BaseUnivariateDistributionType;

  // Typedef for QuantityTraits<double>
  typedef typename BaseType::QT QT;

  // Typedef for QuantityTraits<PrimaryIndepQuantity>
  typedef typename BaseType::PIQT PIQT;

  // Typedef for QuantityTraits<SecondaryIndepQuantity>
  typedef typename BaseType::SIQT SIQT;

  // Typedef for QuantityTriats<InverseSecondaryIndepQuantity>
  typedef typename BaseType::ISIQT ISIQT;

  // Typedef for QuantityTraits<DepQuantity>
  typedef typename BaseType::DQT DQT;

  // The primary independent variable processing tag
  typedef typename TwoDGridPolicy::TwoDInterpPolicy::FirstIndepVarProcessingTag FirstIndepVarProcessingTag;

  // The secondary independent quantity type
  typedef typename TwoDGridPolicy::TwoDInterpPolicy::SecondIndepVarProcessingTag SecondIndepVarProcessingTag;

  // The distribution data const iterator
  typedef typename BaseType::DistributionDataConstIterator DistributionDataConstIterator;

public:

  //! The primary independent quantity type
  typedef typename BaseType::PrimaryIndepQuantity PrimaryIndepQuantity;

  //! The secondary independent quantity type
  typedef typename BaseType::SecondaryIndepQuantity SecondaryIndepQuantity;

  //! The inverse secondary independent quantity type
  typedef typename BaseType::InverseSecondaryIndepQuantity InverseSecondaryIndepQuantity;

  //! The dependent quantity type
  typedef typename BaseType::DepQuantity DepQuantity;

  //! Constructor
  UnitAwareElasticBasicBivariateDistribution(
        const std::vector<PrimaryIndepQuantity>& primary_indep_grid,
        const std::vector<std::shared_ptr<const Utility::UnitAwareTabularUnivariateDistribution<SecondaryIndependentUnit,DependentUnit> > >& secondary_distributions,
        const SecondaryIndepQuantity upper_bound_conditional_indep_var = SIQT::one(),
        const double fuzzy_boundary_tol = 1e-7,
        const double evaluate_relative_error_tol = 1e-7,
        const double evaluate_error_tol = 1e-12 );

  //! Destructor
  ~UnitAwareElasticBasicBivariateDistribution()
  { /* ... */ }

  //! Evaluate the distribution
  DepQuantity evaluate(
       const PrimaryIndepQuantity primary_indep_var_value,
       const SecondaryIndepQuantity secondary_indep_var_value ) const override;

  //! Evaluate the secondary conditional PDF
  InverseSecondaryIndepQuantity evaluateSecondaryConditionalPDF(
       const PrimaryIndepQuantity primary_indep_var_value,
       const SecondaryIndepQuantity secondary_indep_var_value ) const override;

  //! Evaluate the secondary conditional PDF
  InverseSecondaryIndepQuantity evaluateSecondaryConditionalPDF(
            const PrimaryIndepQuantity primary_indep_var_value,
            const SecondaryIndepQuantity secondary_indep_var_value,
            const std::function<SecondaryIndepQuantity(PrimaryIndepQuantity)>&
            min_secondary_indep_var_functor,
            const std::function<SecondaryIndepQuantity(PrimaryIndepQuantity)>&
            max_secondary_indep_var_functor ) const override;

  //! Evaluate the secondary conditional CDF
  double evaluateSecondaryConditionalCDF(
       const PrimaryIndepQuantity primary_indep_var_value,
       const SecondaryIndepQuantity secondary_indep_var_value ) const override;

  //! Return a random sample from the secondary conditional PDF
  SecondaryIndepQuantity sampleSecondaryConditional(
           const PrimaryIndepQuantity primary_indep_var_value ) const override;

  //! Return a random sample from the secondary conditional PDF
  SecondaryIndepQuantity sampleSecondaryConditional(
            const PrimaryIndepQuantity primary_indep_var_value,
            const std::function<SecondaryIndepQuantity(PrimaryIndepQuantity)>&
            min_secondary_indep_var_functor,
            const std::function<SecondaryIndepQuantity(PrimaryIndepQuantity)>&
            max_secondary_indep_var_functor ) const override;

  //! Return a random sample and record the number of trials
  SecondaryIndepQuantity sampleSecondaryConditionalAndRecordTrials(
            const PrimaryIndepQuantity primary_indep_var_value,
            Utility::DistributionTraits::Counter& trials ) const override;

// Return a random sample from the secondary conditional PDF and the index
  SecondaryIndepQuantity sampleSecondaryConditionalAndRecordBinIndices(
            const PrimaryIndepQuantity primary_indep_var_value,
            size_t& primary_bin_index,
            size_t& secondary_bin_index ) const override;

  //! Return a random sample from the secondary conditional PDF and the index
  SecondaryIndepQuantity sampleSecondaryConditionalAndRecordBinIndices(
            const PrimaryIndepQuantity primary_indep_var_value,
            SecondaryIndepQuantity& raw_sample,
            size_t& primary_bin_index,
            size_t& secondary_bin_index ) const override;

  //! Return a random sample from the secondary conditional PDF at the CDF val
  SecondaryIndepQuantity sampleSecondaryConditionalWithRandomNumber(
                const PrimaryIndepQuantity primary_indep_var_value,
                const double random_number ) const override;

  //! Return a random sample from the secondary conditional PDF in the subrange
  SecondaryIndepQuantity sampleSecondaryConditionalInSubrange(
   const PrimaryIndepQuantity primary_indep_var_value,
   const SecondaryIndepQuantity max_secondary_indep_var_value ) const override;

  //! Return a random sample from the secondary conditional PDF in the subrange
  SecondaryIndepQuantity sampleSecondaryConditionalWithRandomNumberInSubrange(
   const PrimaryIndepQuantity primary_indep_var_value,
   const double random_number,
   const SecondaryIndepQuantity max_secondary_indep_var_value ) const override;

  //! Return the lower bound of the conditional distribution
  SecondaryIndepQuantity getLowerBoundOfSecondaryConditionalIndepVar(
           const PrimaryIndepQuantity primary_indep_var_value ) const override;

  //! Return the upper bound of the conditional distribution
  SecondaryIndepQuantity getUpperBoundOfSecondaryConditionalIndepVar(
           const PrimaryIndepQuantity primary_indep_var_value ) const override;

  //! Method for placing the object in an output stream
  void toStream( std::ostream& os ) const override;

private:

  //! Default constructor
  UnitAwareElasticBasicBivariateDistribution();

  //! Evaluate the distribution using the desired evaluation method
  template<typename ReturnType,
           typename EvaluationMethod>
  ReturnType evaluateImpl(
                    const PrimaryIndepQuantity incoming_energy,
                    const SecondaryIndepQuantity angle_cosine,
                    EvaluationMethod evaluate ) const;

  //! Evaluate the distribution using the desired evaluation method
  template<typename ReturnType,
           typename EvaluationMethod>
  ReturnType evaluateImpl(
    const PrimaryIndepQuantity incoming_energy,
    const SecondaryIndepQuantity angle_cosine,
    const std::function<SecondaryIndepQuantity(PrimaryIndepQuantity)>&
      min_secondary_indep_var_functor,
    const std::function<SecondaryIndepQuantity(PrimaryIndepQuantity)>&
      max_secondary_indep_var_functor,
    EvaluationMethod evaluate,
    unsigned max_number_of_iterations = 500 ) const;

  //! Evaluate the distribution using the desired evaluation method
  template<typename EvaluationMethod>
  double evaluateCDFImpl(
                    const PrimaryIndepQuantity incoming_energy,
                    const SecondaryIndepQuantity angle_cosine,
                    EvaluationMethod evaluateCDF ) const;

  //! Evaluate the distribution using the desired evaluation method
  template<typename EvaluationMethod>
  double evaluateCDFImpl(
    const PrimaryIndepQuantity incoming_energy,
    const SecondaryIndepQuantity angle_cosine,
    const std::function<SecondaryIndepQuantity(PrimaryIndepQuantity)>&
      min_secondary_indep_var_functor,
    const std::function<SecondaryIndepQuantity(PrimaryIndepQuantity)>&
      max_secondary_indep_var_functor,
    EvaluationMethod evaluateCDF,
    unsigned max_number_of_iterations = 500 ) const;

  //! Sample from the distribution using the desired sampling functor
  template<typename SampleFunctor>
  SecondaryIndepQuantity sampleDetailedImpl(
                        const PrimaryIndepQuantity primary_indep_var_value,
                        SampleFunctor sample_functor,
                        SecondaryIndepQuantity& raw_sample,
                        size_t& primary_bin_index ) const;

  //! Sample from the distribution using the desired sampling functor
  template<typename SampleFunctor>
  SecondaryIndepQuantity sampleImpl(
                        const PrimaryIndepQuantity primary_indep_var_value,
                        SampleFunctor sample_functor ) const;

  //! Verify that the second independent variable processing type is compatible with Cosine processing
  static void verifyValidSecondIndepVarProcessingType();

  // Save the distribution to an archive
  template<typename Archive>
  void save( Archive& ar, const unsigned version ) const;

  // Load the distribution from an archive
  template<typename Archive>
  void load( Archive& ar, const unsigned version );

  BOOST_SERIALIZATION_SPLIT_MEMBER();

  // Declare the boost serialization access object as a friend
  friend class boost::serialization::access;

  // The max upper bound of the conditional distribution ( 1.0 )
  SecondaryIndepQuantity d_max_upper_bound_conditional_indep_var;

  // The upper bound of the conditional distribution ( -1.0 < mu_cut <= 1.0 )
  SecondaryIndepQuantity d_upper_bound_conditional_indep_var;

  // The lower bound of the conditional distribution ( -1.0 )
  SecondaryIndepQuantity d_lower_bound_conditional_indep_var;
};

/*! \brief The interpolated fully tabular two-dimensional distribution
 * (unit-agnostic)
 * \ingroup two_d_distributions
 */
template<typename TwoDGridPolicy> using ElasticBasicBivariateDistribution =
  UnitAwareElasticBasicBivariateDistribution<TwoDGridPolicy,void,void,void>;

} // end MonteCarlo namespace

BOOST_SERIALIZATION_CLASS4_VERSION( UnitAwareElasticBasicBivariateDistribution, MonteCarlo, 0 );

#define BOOST_SERIALIZATION_ELASTIC_BASIC_BIVARIATE_DISTRIBUTION_EXPORT_STANDARD_KEY() \
  BOOST_SERIALIZATION_CLASS4_EXPORT_STANDARD_KEY( UnitAwareElasticBasicBivariateDistribution, MonteCarlo ) \
  BOOST_SERIALIZATION_TEMPLATE_CLASS_EXPORT_KEY_IMPL(                   \
    UnitAwareElasticBasicBivariateDistribution, MonteCarlo, \
    __BOOST_SERIALIZATION_FORWARD_AS_SINGLE_ARG__( std::string( "ElasticBasicBivariateDistribution<" ) + Utility::typeName<GridPolicy>() + ">" ), \
    __BOOST_SERIALIZATION_FORWARD_AS_SINGLE_ARG__( typename GridPolicy ), \
    __BOOST_SERIALIZATION_FORWARD_AS_SINGLE_ARG__( GridPolicy, void, void, void ) )

BOOST_SERIALIZATION_ELASTIC_BASIC_BIVARIATE_DISTRIBUTION_EXPORT_STANDARD_KEY();

//---------------------------------------------------------------------------//
// Template Includes
//---------------------------------------------------------------------------//

#include "MonteCarlo_ElasticBasicBivariateDistribution_def.hpp"

//---------------------------------------------------------------------------//

#endif // end MONTE_CARLO_ELASTIC_BASIC_BIVARIATE_DISTRIBUTION_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_ElasticBasicBivariateDistribution.hpp
//---------------------------------------------------------------------------//


/* -----------------------------------------------------------------------------

  Copyright (c) 2006 Simon Brown                          si@sjbrown.co.uk

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

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

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

   -------------------------------------------------------------------------- */

/// @file
/// @brief  This program tests the error for 1 and 2-color DXT compression.
///
/// This tests the effectiveness of the DXT compression algorithm for all
/// possible 1 and 2-color blocks of pixels.

#include <squish.h>
#include <iostream>
#include <cmath>
#include <cfloat>
#include <boost/test/unit_test.hpp>

using namespace squish;

double color_error(std::uint8_t const* a, std::uint8_t const* b)
{
  double error = 0.0;
  for (int i = 0; i < 16; ++i)
  {
    for (int j = 0; j < 3; ++j)
    {
      int index = 4 * i + j;
      int diff = static_cast<int>(a[index]) - static_cast<int>(b[index]);
      error += diff * diff;
    }
  }
  return error / 16.0;
}

BOOST_AUTO_TEST_CASE(squish_one_color_random)
{
  flags_t flags =
    squish_flag::compression_bc1 | squish_flag::compressor_color_range_fit;

  std::uint8_t input[4 * 16];
  std::uint8_t output[4 * 16];
  std::uint8_t block[16];

  double avg = 0.0, min = DBL_MAX, max = -DBL_MAX;
  int counter = 0;

  // test all single-channel colors
  for (int test = 0; test < 1000; ++test)
  {
    // set a constant random color
    for (int channel = 0; channel < 3; ++channel)
    {
      auto value = static_cast<std::uint8_t>(rand() & 0xff);
      for (int i = 0; i < 16; ++i)
        input[4 * i + channel] = value;
    }
    for (int i = 0; i < 16; ++i)
      input[4 * i + 3] = 255;

    // compress and decompress
    compress(input, block, flags);
    decompress(output, block, flags);

    // test the results
    double rm = color_error(input, output);
    double rms = std::sqrt(rm);

    // accumulate stats
    min = std::min(min, rms);
    max = std::max(max, rms);
    avg += rm;
    ++counter;
  }

  // finish stats
  avg = std::sqrt(avg / counter);

  // show stats
  std::cout << "random one color error (min, max, avg): " << min << ", " << max
            << ", " << avg << std::endl;
}

BOOST_AUTO_TEST_CASE(squish_one_color)
{
  flags_t flags = squish_flag::compression_bc1;

  std::uint8_t input[4 * 16];
  std::uint8_t output[4 * 16];
  std::uint8_t block[16];

  double avg = 0.0, min = DBL_MAX, max = -DBL_MAX;
  int counter = 0;

  // test all single-channel colors
  for (int i = 0; i < 16 * 4; ++i)
    input[i] = ((i % 4) == 3) ? 255 : 0;
  for (int channel = 0; channel < 3; ++channel)
  {
    for (int value = 0; value < 255; ++value)
    {
      // set the channnel value
      for (int i = 0; i < 16; ++i)
        input[4 * i + channel] = static_cast<std::uint8_t>(value);

      // compress and decompress
      compress(input, block, flags);
      decompress(output, block, flags);

      // test the results
      double rm = color_error(input, output);
      double rms = std::sqrt(rm);

      // accumulate stats
      min = std::min(min, rms);
      max = std::max(max, rms);
      avg += rm;
      ++counter;
    }

    // reset the channel value
    for (int i = 0; i < 16; ++i)
      input[4 * i + channel] = 0;
  }

  // finish stats
  avg = std::sqrt(avg / counter);

  // show stats
  std::cout << "one color error (min, max, avg): " << min << ", " << max << ", "
            << avg << std::endl;
}

BOOST_AUTO_TEST_CASE(squish_two_colors)
{
  flags_t flags = squish_flag::compression_bc1;

  std::uint8_t input[4 * 16];
  std::uint8_t output[4 * 16];
  std::uint8_t block[16];

  double avg = 0.0, min = DBL_MAX, max = -DBL_MAX;
  int counter = 0;

  // test all single-channel colors
  for (int i = 0; i < 16 * 4; ++i)
    input[i] = ((i % 4) == 3) ? 255 : 0;
  for (int channel = 0; channel < 3; ++channel)
  {
    for (int value1 = 0; value1 < 255; ++value1)
    {
      for (int value2 = value1 + 1; value2 < 255; ++value2)
      {
        // set the channnel value
        for (int i = 0; i < 16; ++i)
        {
          input[4 * i + channel] =
            static_cast<std::uint8_t>((i < 8) ? value1 : value2);
        }

        // compress and decompress
        compress(input, block, flags);
        decompress(output, block, flags);

        // test the results
        double rm = color_error(input, output);
        double rms = std::sqrt(rm);

        // accumulate stats
        min = std::min(min, rms);
        max = std::max(max, rms);
        avg += rm;
        ++counter;
      }
    }

    // reset the channel value
    for (int i = 0; i < 16; ++i)
      input[4 * i + channel] = 0;
  }

  // finish stats
  avg = std::sqrt(avg / counter);

  // show stats
  std::cout << "two color error (min, max, avg): " << min << ", " << max << ", "
            << avg << std::endl;
}