#include <vector>
#include <string>
#include <Eigen/Dense>
#include <opencv2/opencv.hpp>
#include <omp.h>

#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/eigen.h>

namespace py = pybind11;

void calcIndexMap( const Eigen::Array2Xf &coords, const Eigen::Array3Xi &coordIdxs, const cv::Size &texSize,
                   cv::Mat &indexMap )
{
    // IndexMap: 3 channels for each pixel, 1: triId, 2 and 3: baycentric coordinates
    indexMap.create( texSize, CV_32FC3 );
    indexMap.setTo( cv::Vec3f( -1.f, -1.f, -1.f ) );

    // For each triangle, find the coverred pixels
    int triNum = int( coordIdxs.cols() );
    for( int triId=0; triId<triNum; triId++ ){
        const Eigen::Vector3i& coordIdx = coordIdxs.col( triId );

        Eigen::Vector2i bboxMin( texSize.width, texSize.height );
        Eigen::Vector2i bboxMax( 0, 0 );

        for( int i=0; i<3; i++ ){
            const Eigen::Vector2f& coord = coords.col( coordIdx[ i ] );
            int x = int( coord[ 0 ] * texSize.width );
            int y = int( coord[ 1 ] * texSize.height );
            bboxMin[ 0 ] = std::min( bboxMin[ 0 ], x );
            bboxMin[ 1 ] = std::min( bboxMin[ 1 ], y );
            bboxMax[ 0 ] = std::max( bboxMax[ 0 ], x );
            bboxMax[ 1 ] = std::max( bboxMax[ 1 ], y );
        }

        bboxMin[ 0 ] = std::min( texSize.width - 1, std::max( 0, bboxMin[ 0 ] ) );
        bboxMin[ 1 ] = std::min( texSize.height - 1, std::max( 0, bboxMin[ 1 ] ) );
        bboxMax[ 0 ] = std::min( texSize.width - 1, std::max( 0, bboxMax[ 0 ] ) );
        bboxMax[ 1 ] = std::min( texSize.height - 1, std::max( 0, bboxMax[ 1 ] ) );

        Eigen::Vector2f vec01 = ( Eigen::Vector2f )coords.col( coordIdx[ 1 ] ) - ( Eigen::Vector2f )coords.col( coordIdx[ 0 ] );
        Eigen::Vector2f vec02 = ( Eigen::Vector2f )coords.col( coordIdx[ 2 ] ) - ( Eigen::Vector2f )coords.col( coordIdx[ 0 ] );

        for( int v = bboxMin[ 1 ]; v <= bboxMax[ 1 ]; v++ ){
            for( int u = bboxMin[ 0 ]; u <= bboxMax[ 0 ]; u++ ){
                Eigen::Vector2f uvCoord( ( u + 0.5f ) / float( texSize.width ), ( v + 0.5f ) / float( texSize.height ) );
                Eigen::Vector2f pt = uvCoord - ( Eigen::Vector2f )coords.col( coordIdx[ 0 ] );

                float denom = vec01[ 0 ] * vec02[ 1 ] - vec01[ 1 ] * vec02[ 0 ];
                float lambda_2 = (pt[ 0 ] * vec02[ 1 ] - pt[ 1 ] * vec02[ 0 ]) / denom;
                float lambda_3 = (vec01[ 0 ] * pt[ 1 ] - vec01[ 1 ] * pt[ 0 ]) / denom;
                float lambda_1 = 1.f - lambda_2 - lambda_3;

                if( lambda_1 > 0.f && lambda_1 < 1.f &&
                    lambda_2 > 0.f && lambda_2 < 1.f &&
                    lambda_3 > 0.f && lambda_3 < 1.f ){
                    indexMap.at<cv::Vec3f>( v, u ) = cv::Vec3f( triId + 0.5f, lambda_1, lambda_2 );
                }
            }
        }
    }
}

py::array_t<float> calcIndexMap_py( const Eigen::Array2Xf& coords, const Eigen::Array3Xi& coordIdxs, int texWidth, int texHeight )
{
    cv::Mat idxMapMat;
    calcIndexMap( coords, coordIdxs, cv::Size( texWidth, texHeight ), idxMapMat );

    py::array_t<float> idxMap = py::array_t<float>( texHeight * texWidth * 3 );
    auto idxMapBuf = idxMap.request();
    memcpy( (float*)idxMapBuf.ptr, idxMapMat.ptr(), texHeight * texWidth * 3 * sizeof(float) );
    idxMap.resize( { texHeight, texWidth, 3 } );

    return idxMap;
}

void unwarpTex( const cv::Mat& img, const Eigen::Array3Xf& ptUVWs, const Eigen::Array3Xi& posIdxs, const cv::Mat& indexMap, cv::Mat& tex )
{
    cv::Size imgSize = img.size();
    cv::Size texSize = indexMap.size();

    tex.create( texSize, CV_8UC4 );
    tex.setTo( cv::Vec4b( 0, 0, 0, 0 ) );

    for( int v=0; v<texSize.height; v++ ){
        for( int u=0; u<texSize.width; u++ ){
            const cv::Vec3f& idx = indexMap.at<cv::Vec3f>( v, u );
            float lambda_1 = idx[ 1 ], lambda_2 = idx[ 2 ];
            float lambda_3 = 1.f - lambda_1 - lambda_2;

            int triId = int( idx[ 0 ] );
            const Eigen::Vector3i& posIdx = posIdxs.col( triId );

            Eigen::Vector3f triUVWs[ 3 ];
            for( int i=0; i<3; i++ ) triUVWs[ i ] = ptUVWs.col( posIdx[ i ] );

            Eigen::Vector3f pixUVW = triUVWs[ 0 ] * lambda_1 + triUVWs[ 1 ] * lambda_2 + triUVWs[ 2 ] * lambda_3;

            float x = pixUVW[ 0 ], y = pixUVW[ 1 ];
            int x0 = int( x ), y0 = int( y );
            int x1 = x0 + 1, y1 = y0 + 1;
            float xWeight = x - float( x0 ), yWeight = y - float( y0 );

            if( x0 < 0 || x1 >= imgSize.width || y0 < 0 || y1 >= imgSize.height ) continue;

            cv::Vec3f pixes[ 4 ];
            pixes[ 0 ] = (cv::Vec3f)img.at<cv::Vec3b>( y0, x0 );
            pixes[ 1 ] = (cv::Vec3f)img.at<cv::Vec3b>( y0, x1 );
            pixes[ 2 ] = (cv::Vec3f)img.at<cv::Vec3b>( y1, x0 );
            pixes[ 3 ] = (cv::Vec3f)img.at<cv::Vec3b>( y1, x1 );

            cv::Vec3f pixVal =
                    ( pixes[ 0 ] * ( 1.f - xWeight ) + pixes[ 1 ] * xWeight ) * ( 1.f - yWeight )
                    + ( pixes[ 2 ] * ( 1.f - xWeight ) + pixes[ 3 ] * xWeight ) * yWeight;

            for( int i=0; i<3; i++ )
                pixVal[ i ] = std::min( 255.f, std::max( 0.f, pixVal[ i ] ) );

            tex.at<cv::Vec4b>( v, u ) = cv::Vec4b( (uchar)pixVal[0], (uchar)pixVal[1], (uchar)pixVal[2], 255 );
        }
    }
}

std::vector<uchar> unwarpTex_py( const std::vector<uchar>& img_vec, int imgWidth, int imgHeight,
    const Eigen::Array3Xf& ptUVWs, const Eigen::Array3Xi& posIdxs,
    const std::vector<float>& idxMap_vec, int texWidth, int texHeight )
{
    cv::Mat img;
    img.create( imgHeight, imgWidth, CV_8UC3 );
    memcpy( img.ptr(), &img_vec[ 0 ], imgHeight * imgWidth * 3 );

    cv::Mat idxMap;
    idxMap.create( texHeight, texWidth, CV_32FC3 );
    memcpy( idxMap.ptr(), &idxMap_vec[ 0 ], texWidth * texHeight * 3 * sizeof(float) );

    cv::Mat tex;
    unwarpTex( img, ptUVWs, posIdxs, idxMap, tex );

    std::vector<uchar> tex_vec;
    tex_vec.resize( texWidth * texHeight * 4 );
    memcpy( &tex_vec[ 0 ], tex.ptr(), texWidth * texHeight * 4 );

    return tex_vec;
}

void unwarpTexNor( const cv::Mat& img, const Eigen::Array3Xf& ptUVWs, const Eigen::Array3Xi& posIdxs,
                   const cv::Mat& indexMap, const cv::Mat& texMask, cv::Mat& tex )
{
    // Calculate the ptUVWNors
    int ptNum = int( ptUVWs.cols() ), triNum = int( posIdxs.cols() );
    Eigen::Array3Xf ptUVWNors( 3, ptNum );
    ptUVWNors.setZero();
    for( int triId=0; triId<triNum; triId++ ){
        const Eigen::Vector3i& posIdx = posIdxs.col( triId );
        Eigen::Vector3f e0 = ptUVWs.col( posIdx[ 1 ] ) - ptUVWs.col( posIdx[ 0 ] );
        Eigen::Vector3f e1 = ptUVWs.col( posIdx[ 2 ] ) - ptUVWs.col( posIdx[ 0 ] );
        Eigen::Vector3f nor = e0.cross( e1 );

        for( int i=0; i<3; i++ ){
            Eigen::Vector3f ptNor = ptUVWNors.col( posIdx[ i ] );
            ptUVWNors.col( posIdx[ i ] ) = ptNor + nor;
        }
    }
    for( int ptId=0; ptId<ptNum; ptId++ ){
        Eigen::Vector3f ptNor = ptUVWNors.col( ptId );
        ptUVWNors.col( ptId ) = ptNor.normalized();
    }

    // Unwarp texture
    cv::Size imgSize = img.size();
    cv::Size texSize = indexMap.size();

    tex.create( texSize, CV_8UC4 );
    tex.setTo( cv::Vec4b( 0, 0, 0, 0 ) );

    const float cPi = 3.1415926535897932384626433f;

    #pragma omp parallel for
    for( int v=0; v<texSize.height; v++ ){
        for( int u=0; u<texSize.width; u++ ){
            float pixWeight = texMask.at<float>( v, u );
            if( pixWeight == 0.f ) continue;

            const cv::Vec3f& idx = indexMap.at<cv::Vec3f>( v, u );
            float lambda_1 = idx[ 1 ], lambda_2 = idx[ 2 ];
            float lambda_3 = 1.f - lambda_1 - lambda_2;

            int triId = int( idx[ 0 ] );
            const Eigen::Vector3i& posIdx = posIdxs.col( triId );

            Eigen::Vector3f triUVWs[ 3 ];
            for( int i=0; i<3; i++ ) triUVWs[ i ] = ptUVWs.col( posIdx[ i ] );

            Eigen::Vector3f pixUVW = triUVWs[ 0 ] * lambda_1 + triUVWs[ 1 ] * lambda_2 + triUVWs[ 2 ] * lambda_3;

            float x = pixUVW[ 0 ], y = pixUVW[ 1 ];
            int x0 = int( x ), y0 = int( y );
            int x1 = x0 + 1, y1 = y0 + 1;
            float xWeight = x - float( x0 ), yWeight = y - float( y0 );

            if( x0 < 0 || x1 >= imgSize.width || y0 < 0 || y1 >= imgSize.height ) continue;

            cv::Vec3f pixes[ 4 ];
            pixes[ 0 ] = (cv::Vec3f)img.at<cv::Vec3b>( y0, x0 );
            pixes[ 1 ] = (cv::Vec3f)img.at<cv::Vec3b>( y0, x1 );
            pixes[ 2 ] = (cv::Vec3f)img.at<cv::Vec3b>( y1, x0 );
            pixes[ 3 ] = (cv::Vec3f)img.at<cv::Vec3b>( y1, x1 );

            cv::Vec3f pixVal =
                    ( pixes[ 0 ] * ( 1.f - xWeight ) + pixes[ 1 ] * xWeight ) * ( 1.f - yWeight )
                    + ( pixes[ 2 ] * ( 1.f - xWeight ) + pixes[ 3 ] * xWeight ) * yWeight;

            // Get the pixel's normal
            Eigen::Vector3f triNors[ 3 ];
            for( int i=0; i<3; i++ ) triNors[ i ] = ptUVWNors.col( posIdx[ i ] );
            Eigen::Vector3f pixNor = triNors[ 0 ] * lambda_1 + triNors[ 1 ] * lambda_2 + triNors[ 2 ] * lambda_3;

            if( pixNor[ 2 ] < 0.f ) pixWeight *= tanh( pixNor[ 2 ] * 2.f * cPi + cPi );
            pixVal *= pixWeight;

            // Set pixel
            for( int i=0; i<3; i++ )
                pixVal[ i ] = std::min( 255.f, std::max( 0.f, pixVal[ i ] ) );
            float alpha = ( pixNor[ 2 ] + 1.f ) * 0.5f;
            alpha = std::min( 255.f, std::max( 0.f, alpha * 255.f ) );
            tex.at<cv::Vec4b>( v, u ) = cv::Vec4b( (uchar)pixVal[0], (uchar)pixVal[1], (uchar)pixVal[2], (uchar)alpha );
        }
    }
}

py::array_t<uchar> unwarpTexNor_py( py::array_t<uchar>& py_imgs, py::array_t<float>& py_ptUVWs,
                                    py::array_t<int>& py_posIdxs, py::array_t<float>& py_idxMap,
                                    py::array_t<int>& py_viewIds, py::array_t<float>& py_texMasks,
                                    py::array_t<int>& py_texRect, py::array_t<int>& py_texSize )
{
    // Get the buffer, and get the parameters
    auto py_imgsBuf = py_imgs.request();
    auto py_ptUVWsBuf = py_ptUVWs.request();
    auto py_posIdxsBuf = py_posIdxs.request();
    auto py_idxMapBuf = py_idxMap.request();
    auto py_viewIdsBuf = py_viewIds.request();
    auto py_texMasksBuf = py_texMasks.request();
    auto py_texRectBuf = py_texRect.request();
    auto py_texSizeBuf = py_texSize.request();

    int imgNum = int( py_imgsBuf.shape[ 0 ] );
    int imgHeight = int( py_imgsBuf.shape[ 1 ] ), imgWidth = int( py_imgsBuf.shape[ 2 ] );
    int texHeight = int( py_idxMapBuf.shape[ 0 ] ), texWidth = int( py_idxMapBuf.shape[ 1 ] );
    int ptNum = int( py_ptUVWsBuf.shape[ 1 ] );
    int triNum = int( py_posIdxsBuf.shape[ 0 ] );
    int viewNum = int( py_texMasksBuf.shape[ 0 ] );

    // Build the data
    cv::Mat idxMap;
    idxMap.create( texHeight, texWidth, CV_32FC3 );
    memcpy( idxMap.ptr(), (float*)py_idxMapBuf.ptr, texWidth * texHeight * 3 * sizeof(float) );

    Eigen::Array3Xi posIdxs( 3, triNum );
    memcpy( posIdxs.data(), (int*)py_posIdxsBuf.ptr, triNum * 3 * sizeof(int) );

    float* py_texMaksPtr = (float*)py_texMasksBuf.ptr;
    std::vector<cv::Mat> texMasks( viewNum );
    for( int viewId=0; viewId<viewNum; viewId++ ){
        cv::Mat& texMask = texMasks[ viewId ];
        texMask.create( texHeight, texWidth, CV_32F );
        memcpy( texMask.ptr(), &py_texMaksPtr[ texHeight * texWidth * viewId ],
                texHeight * texWidth * sizeof( float ) );
        cv::flip( texMask, texMask, 0 );
    }

    // Unwarp
    cv::Rect cropRect( 0, 0, texHeight, texWidth );
    if( py_texRectBuf.shape[ 0 ] == 4 ){
        int* ptr = (int*)py_texRectBuf.ptr;
        cropRect = cv::Rect( ptr[0], ptr[1], ptr[2], ptr[3] );
    }

    cv::Size cropSize( texHeight, texWidth );
    if( py_texSizeBuf.shape[ 0 ] == 2 ){
        int* ptr = (int*)py_texSizeBuf.ptr;
        cropSize = cv::Size( ptr[ 0 ], ptr[ 1 ] );
    }

    py::array_t<uchar> py_texes = py::array_t<uchar>( imgNum * cropSize.width * cropSize.height * 4 );
    auto py_texesBuf = py_texes.request();
    uchar* py_texesPtr = (uchar*)py_texesBuf.ptr;

    uchar* py_imgsPtr = (uchar*)py_imgsBuf.ptr;
    float* py_ptUVWsPtr = (float*)py_ptUVWsBuf.ptr;
    int* py_viewIdsPtr = (int*)py_viewIdsBuf.ptr;
    for( int imgId=0; imgId<imgNum; imgId++ ){
        cv::Mat img;
        img.create( imgHeight, imgWidth, CV_8UC3 );
        memcpy( img.ptr(), &py_imgsPtr[ imgHeight * imgWidth * 3 * imgId ], imgHeight * imgWidth * 3 );

        Eigen::Array3Xf ptUVWs( 3, ptNum );
        memcpy( ptUVWs.data(), &py_ptUVWsPtr[ ptNum * 3 * imgId ], ptNum * 3 * sizeof( float ) );

        cv::Mat tex;
        const cv::Mat& texMask = texMasks[ py_viewIdsPtr[ imgId ] ];
        unwarpTexNor( img, ptUVWs, posIdxs, idxMap, texMask, tex );
        cv::flip( tex, tex, 0 );
        cv::resize( tex( cropRect ), tex, cropSize );

        memcpy( &py_texesPtr[ cropSize.width * cropSize.height * 4 * imgId ], tex.ptr(),
                cropSize.width * cropSize.height * 4 );

        printf( " Unwarp texture for image %d/%d\r", imgId + 1, imgNum );
    }
    printf( "\n" );

    py_texes.resize( { imgNum, cropSize.height, cropSize.width, 4 } );
    return py_texes;
}

void unwarpPosMap( const Eigen::Array3Xf& ptXYZs, const Eigen::Array3Xi& posIdxs,
                const cv::Mat& indexMap, cv::Mat& posMap  )
{
    cv::Size texSize = indexMap.size();
    posMap.create( texSize, CV_32FC3 );
    posMap.setTo( cv::Vec3f( 0.0, 0.0, 0.0 ) );

    for( int v=0; v<texSize.height; v++ ){
        for( int u=0; u<texSize.width; u++ ){
            const cv::Vec3f& idx = indexMap.at<cv::Vec3f>( v, u );
            int triId = int( idx[ 0 ] );
            float lambda_1 = idx[ 1 ], lambda_2 = idx[ 2 ];
            float lambda_3 = 1.f - lambda_1 - lambda_2;

            const Eigen::Vector3i& posIdx = posIdxs.col( triId );
            Eigen::Vector3f triXYZs[ 3 ];
            for( int i=0; i<3; i++ ) triXYZs[ i ] = ptXYZs.col( posIdx[ i ] );

            Eigen::Vector3f pixXYZ = triXYZs[ 0 ] * lambda_1
                                    + triXYZs[ 1 ] * lambda_2
                                    + triXYZs[ 2 ] * lambda_3;

            posMap.at<cv::Vec3f>( v, u ) = cv::Vec3f( pixXYZ[ 0 ], pixXYZ[ 1 ], pixXYZ[ 2 ] );
        }
    }
}

std::vector<float> unwarpPosMap_py( const Eigen::Array3Xf& ptXYZs, const Eigen::Array3Xi& posIdxs,
                const std::vector<float>& idxMap_vec, int texWidth, int texHeight )
{
    cv::Mat idxMap;
    idxMap.create( texHeight, texWidth, CV_32FC3 );
    memcpy( idxMap.ptr(), &idxMap_vec[ 0 ], texWidth * texHeight * 3 * sizeof(float) );

    cv::Mat posMap;
    unwarpPosMap( ptXYZs, posIdxs, idxMap, posMap );

    std::vector<float> posMap_vec;
    posMap_vec.resize( texWidth * texHeight * 3 );
    memcpy( &posMap_vec[ 0 ], posMap.ptr(), texWidth * texHeight * 3 * sizeof( float ) );

    return posMap_vec;
}

// --------------------------------------------------
// *Pybind
PYBIND11_MODULE( texUnwarp, m ){
    m.doc() = "Unwarp texture from image";

    m.def( "calcIndexMap", &calcIndexMap_py, "Calculate texture index map" );
    m.def( "unwarpTex", &unwarpTex_py, "Unwarp the texture" );
    m.def( "unwarpTexNor", &unwarpTexNor_py, "Unwarp the texture" );
    m.def( "unwarpPos", &unwarpPosMap_py, "Unwarp the position map" );
}


#include "hard_nrosy.h"
#include <Eigen/Geometry>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
#include <Eigen/Eigenvalues>
#include <iostream>

using namespace Eigen;

MatrixXd hard_nrosy
        (
        const MatrixXd& V,          // Vertices of the mesh
        const MatrixXi& F,          // Faces
        const MatrixXi& TT,         // Adjacency triangle-triangle
        const VectorXi& hard_id,         // constraints face ids
        const MatrixXd& hard_value,      // constraints 3d vectors
        const int n                 // Degree of the n-rosy field
        )
{
  // assert(hard_id.size() > 0); // One constraint is necessary to make the solution unique

  // This code works only for n==1, see tutorial_nrosy_complete for a generic implementation that works for n >= 1
  // assert(n==1);

  Matrix<double,Dynamic,3> T1(F.rows(),3), T2(F.rows(),3);

  // Compute the local reference systems for each face
  for (unsigned i=0;i<F.rows();++i)
  {
    Vector3d e1 =  V.row(F(i, 1)) - V.row(F(i, 0));
    Vector3d e2 =  V.row(F(i, 2)) - V.row(F(i, 0));
    T1.row(i) = e1.normalized();
    T2.row(i) = T1.row(i).cross(T1.row(i).cross(e2)).normalized();
  }

  // Build the sparse matrix, with an energy term for each edge
  std::vector< Triplet<std::complex<double> > > t;
  std::vector< Triplet<std::complex<double> > > tb;

  unsigned count = 0;
  for (unsigned f=0;f<F.rows();++f)
  {
    for (unsigned ei=0;ei<F.cols();++ei)
    {
      // Look up the opposite face
      int g = TT(f,ei);
      // If it is a boundary edge, it does not contribute to the energy
      if (g == -1) continue;
      // Avoid to count every edge twice
      if (f > g) continue;
      // Compute the complex representation of the common edge
      Vector3d e  = (V.row(F(f,(ei+1)%3)) - V.row(F(f,ei)));
      Vector2d vef = Vector2d(e.dot(T1.row(f)),e.dot(T2.row(f))).normalized();
      std::complex<double> ef(vef(0),vef(1));
      Vector2d veg = Vector2d(e.dot(T1.row(g)),e.dot(T2.row(g))).normalized();
      std::complex<double> eg(veg(0),veg(1));
      // Add the term conj(f)^n*ui - conj(g)^n*uj to the energy matrix
      t.push_back(Triplet<std::complex<double> >(count,f,    std::conj(ef)));
      t.push_back(Triplet<std::complex<double> >(count,g,-1.*std::conj(eg)));
      ++count;
    }
  }

  for (unsigned r=0; r<hard_id.size(); ++r)
  {
    int f = hard_id(r);
    Vector3d v = hard_value.row(r);
    std::complex<double> c(v.dot(T1.row(f)),v.dot(T2.row(f)));
    std::complex<double> o(1,0);
    t.push_back(Triplet<std::complex<double> >(count,f, o));
    tb.push_back(Triplet<std::complex<double> >(count,0, c));
    ++count;
  }

  // Solve the linear system
  typedef SparseMatrix<std::complex<double>> SparseMatrixXcd;
  SparseMatrixXcd A(count,F.rows());
  A.setFromTriplets(t.begin(), t.end());
  SparseMatrixXcd b(count,1);
  b.setFromTriplets(tb.begin(), tb.end());
  SimplicialLDLT< SparseMatrixXcd > solver;
  solver.compute(A.adjoint()*A);
  assert(solver.info()==Success);
  MatrixXcd u = solver.solve(A.adjoint()*MatrixXcd(b));
  assert(solver.info()==Success);

  // Convert the interpolated polyvector into Euclidean vectors
  MatrixXd R(F.rows(),3);
  for (int f=0; f<F.rows(); ++f)
    R.row(f) = T1.row(f) * u(f).real() + T2.row(f) * u(f).imag();

  return R;
}


#include <iostream>
#include <fstream>
#include <iterator>
#include <mintrace.h>
// Following header files are used in the step-3 in the examples of deal.II;
// Here just using these to get a large scale matrix as the test data.
#include <deal.II/grid/tria.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/compressed_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
// Finally, this is for output to a file and to the console:
#include <deal.II/numerics/data_out.h>
#include <fstream>
#include <iostream>

using namespace dealii;

// Computing matrix A times vector b, and using the A[0] to store the result and return it.
std::vector<double> multiply(std::vector<std::vector<double>> A, std::vector<double> b)
{
  int m=A.size();
  int n=b.size();
  for (int i=0;i<m;i++)
    {
      double temp=0;
      for(int j=0;j<n;j++)
	{
	  temp+=A[i][j]*b[j];
	}
      A[0][i]=temp;
      //std::cout<<temp;
      //std::cout<<"The result of "<<i+1<<"th iteration\n";

    }

  return A[0];
}

// Computing u'*v;
double multiply(std::vector<double> u, std::vector<double> v)
{
  if(u.size()!=v.size())
    {
      std::cout<<"Multiply() ERROR: u'*v error! Please Check the size of u and v!";
    }
  else
    {
      double sum=0;
      for(int i=0;i<u.size();i++)
	{
	  sum+=u[i]*v[i];
	}

      return sum;
    }
}

// Design for return transpose matrix of A;
std::vector<std::vector<double>> transpose(std::vector<std::vector<double>> A)
{
  int n=A.size();
  int m=A[0].size();
    if (n < m)
      {
        for (int i = 0; i < n; i++)
	  {
            for (int j = 0; j < i; j++)
	      {
                double temp = A[j][i];
                A[j][i] = A[i][j];
                A[i][j] = temp;
	      }
	  }
        for (int i = n; i < m; i++)
	  {
            std::vector<double> temp;
            for (int j = 0; j < n; j++)
	      {
                temp.push_back(A[j][i]);
	      }
            A.push_back(temp);
	  }
        for (int i = 0; i < n; i++)
	  {
            for (int j = n; j < m; j++)
	      {
                A[i].pop_back();
	      }
	  }
      }
    else if(m<n)
      {
        for (int i = 0; i < m; i++)
	  {
            for (int j = 0; j < i; j++)
	      {
                double temp = A[j][i];
                A[j][i] = A[i][j];
		A[i][j] = temp;
	      }
	  }
        for (int i = 0; i < m; i++)
	  {
            for (int j = m; j < n; j++)
	      {
                A[i].push_back(A[j][i]);
	      }
	  }

        for (int i = m; i < n; i++)
	  {
            A.pop_back();
	  }

      }
    else
      {
	for(int i=0;i<n;i++)
	  {
	    for(int j=0;j<i;j++)
	      {
		double temp=A[j][i];
		A[j][i]=A[i][j];
		A[i][j]=temp;
	      }
	  }
      }

    return A;

}

//This function computes the matrix A times matrix B, and its result will be stored in A.
// It can be improved if reducing the use of the temp matrix to store the original values which
// is used in computing.
void multiply(std::vector<std::vector<double>> &A, std::vector<std::vector<double>> B)
{
    int n = A.size();
    int m = B.size();
    int col = B[0].size();
    std::vector<double> temp(m);
    std::vector<std::vector<double>> A0(n, temp);

    if (col < n)
      {
        for (int i = 0; i < n; i++)
	  {
            for (int j = 0; j < col; j++)
	      {
                double tempValue = 0;
                for (int t = 0; t < m; t++)
		  {
                    tempValue += A[i][t] * B[t][j];
		  }
                A0[i][j] = tempValue;
	      }
            for (int j = col; j < m; j++)
	      {
                A0[i].pop_back();
	      }
	  }

        A = A0;
      }
    else
      {
        for (int i = 0; i <n; i++)
	  {
            for (int j = 0; j < m; j++)
	      {
                double tempValue = 0;
                for (int t = 0; t < m; t++)
		  {
                    tempValue += A[i][t] * B[t][j];
		  }
                A0[i][j] = tempValue;
	      }
            for (int j = m; j < col; j++)
	      {
                double tempValue = 0;
                for (int t = 0; t < m; t++)
		  {
                    tempValue += A[i][t] * B[t][j];
		  }
                A0[i].push_back(tempValue);
	      }
	  }
        A = A0;
      }
}

// this function will turn the matrix Q into the nxn identity matrix;
void identitymatrix(std::vector<std::vector<double>> &Q, int n)
{
  Q.clear();
  std::vector<double> temp(n,0);
  for (int i=0;i<n;i++)
    {
      Q.push_back(temp);
      Q[i][i]=1;
    }
}

// This function computes the infi-norm of vector x;
double infi_Norm(std::vector<double> x)
{
  double norm=0;
  for (int i=0;i<x.size();i++)
    {
      if (norm<abs(x[i]))
	{
	  norm=abs(x[i]);
	}
    }
  return norm;
}

// This function computes the inner-product corresponding with M of <u,v>
void M_inner(SparseMatrix<double> M, std::vector<double>u, std::vector<double>v){}

class Step3
{
public:
  Step3 ();

  void run ();
  void make_grid ();
  void setup_system ();
  void assemble_system ();

  //void multiply(std::vector<double>& x);
  std::vector<double> multiply(std::vector<double> x0);

  //void solve ();
  //
  void output_results () const;

  Triangulation<2>     triangulation;
  FE_Q<2>              fe;
  DoFHandler<2>        dof_handler;
  SparsityPattern      sparsity_pattern;
  SparseMatrix<double> system_matrix;

  Vector<double>       solution;
  Vector<double>       system_rhs;

  //std::vector<double> x;

};

//
//
//void Step3::multiply(std::vector<double>& x)
std::vector<double> multiply(SparseMatrix<double>A, std::vector<double> x0)
{
  std::vector<double> x(x0.size(),0);

  if (A.n()!=x0.size())
    {
      std::cout<<"Function multiply() ERROR: The sizes of matrix and vector are not same! Please check it!\n";
    }
  else
    {
      // std::cout<<"TEST POINT 2 \n";
      for(int k=0;k<A.m();k++)
	{
	  //std::cout<<"this is the "<<k<<"th iterations \n";
	  SparseMatrix<double>::const_iterator i=A.begin(k);

	  while(i!=A.end(k))
	    {
	      x[k]+=i->value()*x0[i->column()];
	      ++i;
	    }
	}
      return x;
    }
}

Step3::Step3 ()
  :
  fe (1),
  dof_handler (triangulation)
{}

void Step3::make_grid ()
{
  // First create the grid and refine all cells five times. Since the initial
  // grid (which is the square [-1,1]x[-1,1]) consists of only one cell, the
  // final grid has 32 times 32 cells, for a total of 1024.
  GridGenerator::hyper_cube (triangulation, -1, 1);
  triangulation.refine_global (5);
  // Unsure that 1024 is the correct number?  Let's see: n_active_cells
  // returns the number of active cells:
  std::cout << "Number of active cells: "
            << triangulation.n_active_cells()
            << std::endl;

  std::cout << "Total number of cells: "
            << triangulation.n_cells()
            << std::endl;
  // Note the distinction between n_active_cells() and n_cells().
}

void Step3::setup_system ()
{
  dof_handler.distribute_dofs (fe);
  std::cout << "Number of degrees of freedom: "
            << dof_handler.n_dofs()
            << std::endl;

  CompressedSparsityPattern c_sparsity(dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern (dof_handler, c_sparsity);
  sparsity_pattern.copy_from(c_sparsity);

  system_matrix.reinit (sparsity_pattern);

  solution.reinit (dof_handler.n_dofs());
  system_rhs.reinit (dof_handler.n_dofs());
}

void Step3::assemble_system ()
{

  QGauss<2>  quadrature_formula(2);

  FEValues<2> fe_values (fe, quadrature_formula,
                         update_values | update_gradients | update_JxW_values);

  const unsigned int   dofs_per_cell = fe.dofs_per_cell;
  const unsigned int   n_q_points    = quadrature_formula.size();

  FullMatrix<double>   cell_matrix (dofs_per_cell, dofs_per_cell);
  Vector<double>       cell_rhs (dofs_per_cell);

  std::vector<types::global_dof_index> local_dof_indices (dofs_per_cell);

  DoFHandler<2>::active_cell_iterator
  cell = dof_handler.begin_active(),
  endc = dof_handler.end();
  for (; cell!=endc; ++cell)
    {
      fe_values.reinit (cell);

      cell_matrix = 0;
      cell_rhs = 0;

      for (unsigned int i=0; i<dofs_per_cell; ++i)
        for (unsigned int j=0; j<dofs_per_cell; ++j)
          for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
            cell_matrix(i,j) += (fe_values.shape_grad (i, q_point) *
                                 fe_values.shape_grad (j, q_point) *
                                 fe_values.JxW (q_point));

      for (unsigned int i=0; i<dofs_per_cell; ++i)
        for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
          cell_rhs(i) += (fe_values.shape_value (i, q_point) *
                          1 *
                          fe_values.JxW (q_point));

      cell->get_dof_indices (local_dof_indices);
      ///////////////////////
      // Following commands compute the interior of the matrix and store them into the matrix.

      for (unsigned int i=0; i<dofs_per_cell; ++i)
        for (unsigned int j=0; j<dofs_per_cell; ++j)
          system_matrix.add (local_dof_indices[i],
                             local_dof_indices[j],
                             cell_matrix(i,j));

      // And again, we do the same thing for the right hand side vector.
      for (unsigned int i=0; i<dofs_per_cell; ++i)
        system_rhs(local_dof_indices[i]) += cell_rhs(i);
    }

  std::map<types::global_dof_index,double> boundary_values;
  VectorTools::interpolate_boundary_values (dof_handler,
                                            0,
                                            ZeroFunction<2>(),
                                            boundary_values);

  MatrixTools::apply_boundary_values (boundary_values,
                                      system_matrix,
                                      solution,
                                      system_rhs);
}

//void Step3::multiply(std::vector<double>& x)
std::vector<double> Step3::multiply(std::vector<double> x0)
{
  std::vector<double> x(x0.size(),0);

  if (system_matrix.n()!=x0.size())
    {
      std::cout<<"Function multiply() ERROR: The sizes of matrix and vector are not same! Please check it!\n";
    }
  else
    {
      // std::cout<<"TEST POINT 2 \n";
      for(int k=0;k<system_matrix.m();k++)
	{
	  //std::cout<<"this is the "<<k<<"th iterations \n";
	  SparseMatrix<double>::const_iterator i=system_matrix.begin(k);

	  while(i!=system_matrix.end(k))
	    {
	      x[k]+=i->value()*x0[i->column()];
	      ++i;
	    }
	}
      return x;
    }
}

//void Householder(std::vector<std::vector<double>> *A, std::vector<std::vector<double>> *P)
//void Householder(std::vector<std::vector<double>> *A, std::vector<std::vector<double>> *P)

// This function computes the householder process of the symmetric definite matrix, it return
// two matrices. The tridiagonal matrix coordinating with A and the hermitian unitary matrix P.
void Householder(std::vector<std::vector<double>> &A, std::vector<std::vector<double>> &P)
{
  int n=A.size();
  std::vector<std::vector<double>> Pk;

  for(int k=0;k<n-2;k++)
    {
      identitymatrix(Pk, n);
      double q=0;
      double alpha=0;
      // Computing the value of q in the Householder function;
      for(int i=k+1;i<n;i++)
	{
	  q+=A[i][k]*(A[i][k]);
	}

      // Computing the value of alpha
      if(A[k+1][k]==0)
	{
	  alpha=-sqrt(q);
	}
      else
	{
	  alpha=-sqrt(q)*A[k+1][k]/abs(A[k+1][k]);
	  // std::cout<<"the abs A[k+1][k] is "<<A[k+1][k]<<std::endl;
	}

      double RSQ=alpha*alpha-alpha*A[k+1][k]; // RSQ = 2r^2;

      //std::cout<<"Check Point 2, This is RSQ:::"<<RSQ<<"\n";

      std::vector<double> v(n-k,0), u(n-k,0), z(n-k,0);
      v[0]=0;
      v[1]=A[k+1][k]-alpha;

      //std::cout<<"Check Point 3::: "<<v[1]<<" \n";
      // w=(1/sqrt(2*RSQ)*v=1/2r*v);
      for(int j=2;j<v.size();j++)
	{
	  v[j]=A[k+j][k];
	}

      // std::cout<<"CheckPPPPPoint555 \n";

      for(int j=0;j<n-k;j++)
	{
	  for(int i=1;i<n-k;i++)
	    {
	      u[j]+=A[k+j][k+i]*v[i];
	    }
	  u[j]/=RSQ;
	}

      //std::cout<<"Check POOOIONIOHIHO\n";

      double PROD=0;
      for(int i=1;i<n-k;i++)
	{
	  PROD+=v[i]*u[i];
	}

      //std::cout<<"CheckPoint 4!!!!!!!!!\n";

      for(int j=0;j<n-k;j++)
	{
	  z[j]=u[j]-PROD/(2*RSQ)*v[j];
	}

      for(int l=k+1;l<n-1;l++)
	{
	  for(int j=l+1;j<n;j++)
	    {
	      A[j][l]=A[j][l]-v[l-k]*z[j-k]-v[j-k]*z[l-k];
	      A[l][j]=A[j][l];
	    }
	  A[l][l]=A[l][l]-2*v[l-k]*z[l-k];
	}

      A[n-1][n-1]=A[n-1][n-1]-2*v[n-k-1]*z[n-k-1];

      for(int j=k+2;j<n;j++)
	{
	  A[k][j]=0;
	  A[j][k]=0;
	}

      A[k+1][k]=A[k+1][k]-v[1]*z[0];
      A[k][k+1]=A[k+1][k];

      for (int j=k+1;j<n;j++)
	{
	  std::vector<double> temp(j-k,0);
	  for(int i=k+1;i<=j;i++)
	    {
	      for(int t=k+1;t<n;t++)
		{
		  temp[i-k-1]+=v[i-k]*v[t-k]*Pk[t][j]/(2*RSQ);
		}
	    }

	  //temp=multiply(W,Pj);// to get the jth column of the W*P;
	  for (int i=k+1;i<=j;i++)
	    {
	      Pk[i][j]-=2*temp[i-k-1];
	      Pk[j][i]=Pk[i][j];
	    }
	}
      multiply(P,Pk);

    }

}

void Step3::run ()
{
  make_grid ();
  setup_system();
  assemble_system ();
  std::cout<<system_matrix.m()<<"\n";
  std::cout<<system_matrix.n()<<"\n";

  //std::cout<<system_matrix.begin()->value()<<"\n";
  //std::cout<<system_matrix.begin()->column()<<"\n";

  std::vector<double> x(system_matrix.n(),1);
  //
  // Due to the system_matrix is a member of the class step3, but x is not a member of the class,
  // So we can not directly define the multiply function out of the class and using the
  // system_matrix directly.
  // we need to get a variable A equals to the system_matrix but A is not a member of the class;

  std::cout << "CheckPoint 1 \n";

  // multiply(x);
  x=multiply(x);
  //std::cout<<"This is the value of the vector x"<<x[0]<<std::endl;
  std::ofstream out ("sparse_matrix");
  system_matrix.print(out);

  std::ofstream output_file ("solution");
  for (const auto &e : x) output_file << e << "\n";

}

///////
// This function computes the QR factorization of the tridiagonal matrix A.
// Input: tridiagonal matrix A, identity matrix Q
// Output: uptriangle matrix A_, the orthonormal matrix Q; A=Q*A_;
//
// tips: Maybe I can improve the function by replace the matrix A by two vectors an bn-1;
// an contains the diagonal entries of A; bn-1 contains the sub-diagonal entries of A;

void QR(std::vector<std::vector<double>> &A, std::vector<std::vector<double>> &Q)
{
  int n=A.size();

  // obtain the diagonal and subdiagonal entries of matrix A;
  /*
  std::vector<double>an(n),bn(n-1);
  for (int i=0;i<A.size()-1;i++)
    {
      an[i]=A[i][i];
      bn[i]=A[i+1][i];
    }
  an[n-1]=A[n-1][n-1];
  */

  for (int i=0;i<A.size()-1;i++)
    {
      double theta=0;
      double tempa1=A[i][i],tempa2=A[i+1][i+1],tempb=A[i+1][i],tempc=A[i][i+1];
      theta=atan(A[i+1][i]/A[i][i]);
      double c=cos(theta), s=sin(theta);

      A[i][i]=c*tempa1+s*tempb;
      A[i][i+1]=c*tempc+s*tempa2;
      A[i+1][i+1]=-s*tempc+c*tempa2;
      A[i+1][i]=-s*tempa1+c*tempb;
      if (i<A.size()-2)
	{
	  A[i][i+2]=s*A[i+1][i+2];
	  A[i+1][i+2]=c*A[i+1][i+2];
	}

      // update the orthogonal matrix Q by Q_k=Q_k-1*Q;

      for(int k=0;k<n;k++)
	{
	  double tmp1=Q[k][i],tmp2=Q[k][i+1];
	  Q[k][i]=Q[k][i]*c+Q[k][i+1]*s;
	  Q[k][i+1]=tmp1*(-s)+tmp2*c;
	}

    }

}

// This function computes the eigenvalue of the matrix A by QR methods, and it will also return the
// matrix Q during the process;
void QRSolver(std::vector<std::vector<double>> &A, std::vector<std::vector<double>> &Q, double tol=0.001)
{
  int n=A.size();
  // using householder transform matrix A into the tridiagonal matrix
  // store it in A and store the Householder unitary matrix in Q; A=QA_Q;
  //Householder(A,Q);

  // Qk as the initial matrix for every QR factorization, multiplying it together to get the
  // final unitary matrix;
  std::vector<std::vector<double>> Qk;

  // Get the subdiagonal entries of the matrix A and verify if its norm smaller than the tolerance;
  // If it is small enough that means we diagonalize the matrix A and the diagonal entries are
  // eigenvalues of A.
  std::vector<double> b(n-1,1);
  for(int i=0;i<n-1;i++)
    {
      b.push_back(A[i+1][i]);
    }

  int num=0;
  while (infi_Norm(b)>tol&&num<1000)
    {
      num++;

      b.clear();

      Householder(A,Q);

      identitymatrix(Qk,n);

      QR(A,Qk);

      // Compute the num-th iteration, get the Qk in this step;
      multiply(Q,Qk);
      // compute R*Q beacuse I store the R(computed above) into A, So I directly use multiply
      // function to get A*Q into A=RQ.
      multiply(A,Qk);

      //update the entries of b, i.e. the subdiagonal entries of matrix A=RQ;
        for(int i=0;i<n-1;i++)
	  {
	    b.push_back(A[i+1][i]);
	  }
    }
}

// The function using Conjugate Gradient method to get the solution of the problem Ax=b;
// It is modified CG function that is designed for solve PAP delta = PAX which needs A, M, X, b;
// ATTENTION: It can be revised as a class for A and M, It will be more convenient.
std::vector<double> CG(SparseMatrix<double>A, SparseMatrix<double> M, std::vector<std::vector<double>> X, std::vector<double> b, std::vector<double> &x)
{
  return x;
}

// Computing the matrix P during the minimal trace process;
std::vector<std::vector<double>> matrixP(SparseMatrix<double> M, std::vector<std::vector<double>>X )
{
  std::vector<std::vector<double>> P;
  return P;
}

//
// This function computes the matrix M-orthogonal modified Gram-Schmidt procedure;
std::vector<double> M_orth(SparseMatrix<double> M, std::vector<double> x)
{
  return x;
}

// This function computes a random matrix V as the initial matrix of the iteration;
// n implies the column number of V;
void rand_V(SparseMatrix<double>M, std::vector<std::vector<double>> &V, int n)
{
  std::vector<std::vector<double>> A(V);
  V=A;
}

// Compute matrix A - B;
std::vector<std::vector<double>> minus(std::vector<std::vector<double>> A, std::vector<std::vector<double>> B)
{
  return A;
}

// This function computes the smallest p eigenvalues of matrix A corresponding to matrix M;
// Notice: The A and M are square matrices here.
std::vector<double> min_trace(SparseMatrix<double> A, SparseMatrix<double>M, int p, double tol=0.001, int max_iter=1)
{
  int n=A.m();
  std::vector<std::vector<double>>V(p);
  std::vector<std::vector<double>>W(n);
  std::vector<std::vector<double>> MXTheta(p), U, X, Rk, delta;
  //SparseMatrix<double> P;

  // Construct the initial matrix V1 which is a n x p matrix and it is orthogonal by M, i.e.
  // V1'*M*V1=I_p;
  // I store the V(i,j)=V[j][i];
  rand_V(M,V,p);
  for (int k=0;k<max_iter;k++)
    {
      // Computing the matrix W and H;
      // Here I use the property of the symmetric A: H=V'*A*V=V'*W=W'*V=H'=V'*A'*V=H;

      // store the A *V[i] into W[i];
      for(int i=0;i<p;i++)
	{
	  std::vector<double> tempV=V[i];
	  // W[i]=multiply(A,V[i]);
	  W[i]=multiply(A,tempV);
	}

      // copy the W into H;
      std::vector<std::vector<double>> H(W);
      // Computing H'*V; in fact H[i][j]=H(j,i); So above the H=H';
      // Get the actual Hk;
      multiply(H,transpose(V));

      // Compute the spectral decomposition of Hk;
      identitymatrix(U,p);
      QRSolver(H,U); // H = theta_k, U=U_k;

      // transpose V into V[i][j]=V(i,j);
      X=transpose(V);
      // Compute Ritz vectors X_k=V_k*U_k;
      multiply(X,U);

      // Compute MXTheta = M * X_k * Theta_k
      // using XTheta to represent the matrix otherwise X will be changed by multiply;
      std::vector<std::vector<double>> XTheta(X);
      // Compute XTheta = X_k * Theta_k = XTheta*H;
      multiply(XTheta,H);
      for(int i=0;i<p;i++)
	{
	  MXTheta[i]=multiply(M, XTheta[i]);
	}
      // Compute residual Rk in fact Rk[i][j] = Rk(j,i) ;
      // This step might be wrong;
      //Rk=minus(multiply(transpose(U),W), MXTheta);
      /*
      if(infi_Norm(Rk)<tol)
	{
	  break;
	}
      */

      // Compute matrix P;
      //  P=matrixP(M, X);

      // Solve the SPD eigenvalue problem by modified PCG or CG;
      // PAP delta_k = PA X_k;
      // divide it into p eigenvalue problem:
      // PAP d_i = PA x_i; d_i = delta_k*ei, x_i = X_k*e_i;

      for(int i=0;i<p;i++)
	{
	  std::vector<double> x(n,0);
	  SparseMatrix<double> PAP;
	  std::vector<double> PAXi;
	  delta[i]=CG(A, M, X, PAXi, x);
	}

      // update the V = V_k+1;
      std::vector<std::vector<double>> XT;
      XT=transpose(X);
      for(int i=0;i<p;i++)
	{
	  XT=minus(XT, delta);
	  V[i]=M_orth(M,XT[i]);
	}
      //V=transpose(V);
    }

  // Compute the diagonal Matrix V'*A*V whose diagonal entries are eigenvalues of the matrix A
  // corresponding to matrix M;

  std::vector<double> eigenvalue(p);
  for(int i=0;i<p;i++)
    {
      std::vector<double> tempV, tempV2;
      tempV=multiply(A,V[i]);
      tempV2=V[i];
      eigenvalue[i]=multiply(tempV,tempV2);
    }

  return eigenvalue;

}

int main()
{
  Step3 laplace_problem;
  /////laplace_problem.run ();

  //std::stringstream result;
  std::vector<double> b(4,0);
  std::vector<std::vector<double>> A(4, b), P(4,b);
  P[0][0]=1;
  P[1][1]=1;
  P[2][2]=1;
  P[3][3]=1;
  std::vector<std::vector<double>> Q(P);

  A[0][0]=4;
  A[0][1]=1;
  A[0][2]=-2;
  A[0][3]=2;
  A[1][0]=1;
  A[1][1]=2;
  A[1][2]=0;
  A[1][3]=1;
  A[2][0]=-2;
  A[2][1]=0;
  A[2][2]=3;
  A[2][3]=-2;
  A[3][0]=2;
  A[3][1]=1;
  A[3][2]=-2;
  A[3][3]=-1;

  //std::vector<double> c={1,2,3,4};
  //transpose(A);

  std::cout<<"The matrix A is :\n";
  std::cout<<"\n";

  for(int i=0;i<A.size();i++)
    {
      for(int j=0; j<A.size();j++)
	{
	  std::cout<<A[i][j]<<" ";
	}
      std::cout<<"\n";
    }
  std::cout<<"\n";

  // multiply(A,A);

  // c=multiply(A,c);

  /*
  Householder(A,P);

  std::cout<<"The matrix A is :\n";
  std::cout<<"\n";

  for(int i=0;i<A.size();i++)
    {
      for(int j=0; j<A.size();j++)
	{
	  std::cout<<A[i][j]<<" ";
	}
      std::cout<<"\n";
    }
  std::cout<<"\n";

  QR(A,Q);

  */

  ////
  //  ATTENTION:!!!!! the original matrix A is symmetric, but after A= QR to get A_=RQ, A_ might be  // nonsymmetric!!!!!! the function is useless!!!!!!! Please edit the function.
  QRSolver(A,Q);

  std::cout<<"The matrix A is :\n";
  std::cout<<"\n";

  for(int i=0;i<A.size();i++)
    {
      for(int j=0; j<A.size();j++)
	{
	  std::cout<<A[i][j]<<" ";
	}
      std::cout<<"\n";
    }
  std::cout<<"\n";

  std::cout<<"The matrix Q is :::::::\n";
  std::cout<<"\n";

  for(int i=0;i<Q.size();i++)
    {
      for(int j=0; j<Q.size();j++)
	{
	  std::cout<<Q[i][j]<<" ";
	}
      std::cout<<"\n";
    }
  std::cout<<"\n";

  std::cout<<"check if the matrix Q is orthogonal\n";
  multiply(Q,transpose(Q));

  std::cout<<"The matrix Q*Q' is :::::::\n";
  std::cout<<"\n";

  for(int i=0;i<Q.size();i++)
    {
      for(int j=0; j<Q.size();j++)
	{
	  std::cout<<Q[i][j]<<" ";
	}
      std::cout<<"\n";
    }
  std::cout<<"\n";

  std::cout<<"hello world!"<<std::endl;
  //SparseMatrix<double> A;
  return 0;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#include <algorithm>
#include <iostream>
#include <vector>

#include <boost/compute/lambda.hpp>
#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/container/vector.hpp>

#include "perf.hpp"

float rand_float()
{
    return (float(rand()) / float(RAND_MAX)) * 1000.f;
}

// y <- alpha * x + y
void serial_saxpy(size_t n, float alpha, const float *x, float *y)
{
    for(size_t i = 0; i < n; i++){
        y[i] = alpha * x[i] + y[i];
    }
}

int main(int argc, char *argv[])
{
    perf_parse_args(argc, argv);

    using boost::compute::lambda::_1;
    using boost::compute::lambda::_2;

    std::cout << "size: " << PERF_N << std::endl;

    float alpha = 2.5f;

    // setup context and queue for the default device
    boost::compute::device device = boost::compute::system::default_device();
    boost::compute::context context(device);
    boost::compute::command_queue queue(context, device);
    std::cout << "device: " << device.name() << std::endl;

    // create vector of random numbers on the host
    std::vector<float> host_x(PERF_N);
    std::vector<float> host_y(PERF_N);
    std::generate(host_x.begin(), host_x.end(), rand_float);
    std::generate(host_y.begin(), host_y.end(), rand_float);

    // create vector on the device and copy the data
    boost::compute::vector<float> device_x(host_x.begin(), host_x.end(), queue);
    boost::compute::vector<float> device_y(host_y.begin(), host_y.end(), queue);

    perf_timer t;
    for(size_t trial = 0; trial < PERF_TRIALS; trial++){
        boost::compute::copy(host_x.begin(), host_x.end(), device_x.begin(), queue);
        boost::compute::copy(host_y.begin(), host_y.end(), device_y.begin(), queue);

        t.start();
        boost::compute::transform(
            device_x.begin(),
            device_x.end(),
            device_y.begin(),
            device_y.begin(),
            alpha * _1 + _2,
            queue
        );
        queue.finish();
        t.stop();
    }
    std::cout << "time: " << t.min_time() / 1e6 << " ms" << std::endl;

    // perform saxpy on host
    serial_saxpy(PERF_N, alpha, &host_x[0], &host_y[0]);

    // copy device_y to host_x
    boost::compute::copy(device_y.begin(), device_y.end(), host_x.begin(), queue);

    for(size_t i = 0; i < PERF_N; i++){
        float host_value = host_y[i];
        float device_value = host_x[i];

        if(std::abs(device_value - host_value) > 1e-3){
            std::cout << "ERROR: "
                      << "value at " << i << " "
                      << "device_value (" << device_value << ") "
                      << "!= "
                      << "host_value (" << host_value << ")"
                      << std::endl;
            return -1;
        }
    }

    return 0;
}


/**
 * @file mapped_vector.hpp
 * @author David Albert (david.albert@insa-lyon.fr)
 * @brief
 * @version 0.1
 * @date 07/01/2021
 *
 * @copyright Copyright (c) 2021
 *
 */
#pragma once

#include <iostream>
#include <unordered_map>
#include <cmath>
#include <string>
#include <vector>
#include <assert.h>

#include <boost/serialization/base_object.hpp>

#include <sdm/types.hpp>
#include <sdm/utils/linear_algebra/vector_interface.hpp>
#include <sdm/utils/struct/vector.hpp>
#include <sdm/utils/struct/pair.hpp>
#include <sdm/utils/struct/recursive_map.hpp>

namespace sdm
{
    /**
     * @brief Mapped vectors are vectors with specific type of indexes. They are represented by a map.
     *
     * @tparam TIndex the type of index
     * @tparam T the type of value (default : `double`)
     *
     * Using map structure allows to keep only necessary values.
     * Moreover, it allows to see vectors not only as a mapping from integer to value but also as a mapping from any type of index to values.
     *
     */
    template <class TIndex, class T = double, class Hash = std::hash<TIndex>, class KeyEqual = std::equal_to<TIndex>>
    class MappedVector : public std::unordered_map<TIndex, T, Hash, KeyEqual>, public VectorInterface<TIndex, T>
    {
    public:
        using iterator = typename std::unordered_map<TIndex, T, Hash, KeyEqual>::iterator;
        using const_iterator = typename std::unordered_map<TIndex, T, Hash, KeyEqual>::const_iterator;

        using type = typename RecursiveMap<TIndex, T>::type;
        using value_type = typename RecursiveMap<TIndex, T>::value_type;
        using value_list_type = typename RecursiveMap<TIndex, T>::value_list_type;

        static double PRECISION;

        MappedVector();
        MappedVector(T default_value);
        MappedVector(long size, T default_value);
        MappedVector(const MappedVector &);
        MappedVector(std::initializer_list<value_list_type>);
        virtual ~MappedVector();

        T norm_1() const;
        T norm_2() const;

        T min();
        TIndex argmin();

        T max();
        TIndex argmax();

        T at(const TIndex &) const;
        T getValueAt(const TIndex &) const;
        void setValueAt(const TIndex &, const T &);
        void addValueAt(const TIndex &, const T &);

        bool isExist(const TIndex&) const;

        /**
         * @brief This method implements a non-commutative dot product
         * @comment: It is worth noticing that sometimes arg1.dot(arg2) !=  arg2.dot(arg1)
         * @return T
         */
        T operator^(const MappedVector &) const;
        T operator*(const MappedVector &) const;

        MappedVector add(const MappedVector &other, double coef_this = 1., double coef_other = 1.) const
        {
            MappedVector sum;
            for (const auto &item : this->getIndexes())
            {
                sum.setValueAt(item, coef_this * this->getValueAt(item) + coef_other * other.getValueAt(item));
            }
            for (const auto &item : other.getIndexes())
            {
                sum.setValueAt(item, coef_this * this->getValueAt(item) + coef_other * other.getValueAt(item));
            }
            sum.finalize();
            return sum;
        }

        bool operator!=(const MappedVector &) const;
        bool operator<(const MappedVector &) const;
        bool operator==(const MappedVector &other) const;
        bool isEqual(const MappedVector &other, double precision) const;

        /**
         * @brief This method implements a non-commutative dot product
         * @comment: It is worth noticing that sometimes arg1.dot(arg2) !=  arg2.dot(arg1)
         * @return T
         */
        T dot(const MappedVector &) const;

        T getDefault() const;
        void setDefault(double default_value);

        void setupIndexes();
        void finalize();

        std::vector<TIndex> getIndexes() const;

        static void setPrecision(double);

        std::string str() const;
        size_t size() const;

        friend std::ostream &operator<<(std::ostream &os, const MappedVector &vect)
        {
            os << vect.str();
            return os;
        }

        friend class boost::serialization::access;

        template <class Archive>
        void serialize(Archive &archive, const unsigned int);

    protected:
        T default_value_ = 0.0;
        long size_ = -1;

        std::vector<TIndex> v_indexes = {};

        bool bmin = false, bmax = false;
        std::pair<TIndex, T> pmin, pmax;

        const std::pair<TIndex, T> &getMin();
        const std::pair<TIndex, T> &getMax();
    };
} // namespace sdm
#include <sdm/utils/linear_algebra/mapped_vector.tpp>


/**
 * @file   CartanMatrixTest.cpp
 * @author ALIKAWA Hidehisa <alleyhide@gmail.com>
 * @date   2018/09/01

 *
 * @brief  for the tests of Cartan matrix
 *
 * Released under the MIT license
 */

#include <iostream>
#include <boost/numeric/ublas/io.hpp>

#include "gweyl.hpp"

int main(int argc, char** argv)
{
    //std::cout << argv[0] << " and " << argv[1] << " and " << argv[2] << std::endl;

    char t = argv[1][0];

    //std::cout << "Type is " << t << std::endl;

    gweyl::Type X;
    switch (t){
    case 'A':
        X=gweyl::Type::A;
        break;
    case 'B':
        X=gweyl::Type::B;
        break;
    case 'C':
        X=gweyl::Type::C;
        break;
    case 'D':
        X=gweyl::Type::D;
        break;
    case 'E':
        X=gweyl::Type::E;
        break;
    case 'F':
        X=gweyl::Type::F;
        break;
    case 'G':
        X=gweyl::Type::G;
        break;
    default:
        std::cout << "Error type " << std::to_string(t);
        return -1;
    }

    unsigned n = atoi(argv[2]);

    try {
        //gweyl::matrix CarMat = gweyl::CartanMatrix(X, n);
        gweyl::Cartan T(X, n);
        gweyl::matrix CarMat = T.CartanMatrix();
        std::cout << CarMat << std::endl;
    }catch (std::exception &e){
        std::cout << "Exeption is caught \n what(): ";
        std::cout << e.what();
        std::cout << "\nError test CartanMatrix" << std::endl;
    }

    return 0;
}


#include <iostream>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>

int main()
{
    namespace ba = boost::accumulators;

    ba::accumulator_set<double, ba::features<ba::tag::mean, ba::tag::variance>, int> acc;
    acc(8, ba::weight=1);
    acc(9, ba::weight=1);
    acc(10, ba::weight=4);
    acc(11, ba::weight=1);
    acc(12, ba::weight=1);
    std::cout << ba::mean(acc) << ":" << ba::variance(acc) << std::endl;
}


/* ----------------------------------------------------------------------------
 * Copyright 2020, Jesus Tordesillas Torres, Aerospace Controls Laboratory
 * Massachusetts Institute of Technology
 * All Rights Reserved
 * Authors: Jesus Tordesillas, et al.
 * See LICENSE file for the license information
 * -------------------------------------------------------------------------- */

#ifndef SOLVER_GUROBI_HPP
#define SOLVER_GUROBI_HPP
#include <Eigen/Dense>
#include "gurobi_c++.h"
#include <sstream>
#include <Eigen/Dense>
#include <type_traits>
#include <fstream>
#include "termcolor.hpp"

#include <decomp_ros_utils/data_ros_utils.h>
#include <unsupported/Eigen/Polynomials>
#include "faster_types.hpp"
using namespace termcolor;

// TODO: This function is the same as solvePolyOrder2 but with other name (weird conflicts...)
inline double solvePolynomialOrder2(Eigen::Vector3f& coeff)
{
  // std::cout << "solving\n" << coeff.transpose() << std::endl;
  double a = coeff(0);
  double b = coeff(1);
  double c = coeff(2);
  double dis = b * b - 4 * a * c;
  if (dis >= 0)
  {
    double x1 = (-b - sqrt(dis)) / (2 * a);  // x1 will always be smaller than x2
    double x2 = (-b + sqrt(dis)) / (2 * a);

    if (x1 >= 0)
    {
      return x1;
    }
    if (x2 >= 0)
    {
      return x2;
    }
  }
  printf("No solution found to the equation\n");
  return std::numeric_limits<float>::max();
}

class mycallback : public GRBCallback
{
public:
  bool should_terminate_;
  mycallback();  // constructor
  // void abortar();

protected:
  void callback();
};

class SolverGurobi
{
public:
  SolverGurobi();

  // void setQ(double q);
  void setN(int N);
  void setX0(state& data);
  // void set_u0(double u0[]);
  void setXf(state& data);
  void resetX();
  void setBounds(double max_values[3]);
  bool genNewTraj();
  bool callOptimizer();
  double getDTInitial();

  void setDC(double dc);
  void setPolytopes(std::vector<LinearConstraint3D> polytopes);
  void setPolytopesConstraints();
  void findDT(double factor);
  void fillX();
  void setObjective();
  void setConstraintsXf();
  void setConstraintsX0();
  void setDynamicConstraints();
  void setForceFinalConstraint(bool forceFinalConstraint);

  // For the jackal
  void setWMax(double w_max);
  bool isWmaxSatisfied();

  void setMaxConstraints();
  void createVars();
  void setThreads(int threads);
  void setVerbose(int verbose);

  void StopExecution();
  void ResetToNormalState();

  void setDistances(vec_Vecf<3>& samples, std::vector<double> dist_near_obs);

  // void setSamplesPenalize(vec_Vecf<3>& samples_penalize);

  void setDistanceConstraints();

  void setMode(int mode);
  void setFactorInitialAndFinalAndIncrement(double factor_initial, double factor_final, double factor_increment);

  GRBLinExpr getPos(int t, double tau, int ii);
  GRBLinExpr getVel(int t, double tau, int ii);
  GRBLinExpr getAccel(int t, double tau, int ii);
  GRBLinExpr getJerk(int t, double tau, int ii);

  GRBLinExpr getA(int t, int ii);
  GRBLinExpr getB(int t, int ii);
  GRBLinExpr getC(int t, int ii);
  GRBLinExpr getD(int t, int ii);

  // Getters of the Normalized coefficients
  GRBLinExpr getAn(int t, int ii);
  GRBLinExpr getBn(int t, int ii);
  GRBLinExpr getCn(int t, int ii);
  GRBLinExpr getDn(int t, int ii);

  std::vector<GRBLinExpr> getCP0(int t);
  std::vector<GRBLinExpr> getCP1(int t);
  std::vector<GRBLinExpr> getCP2(int t);
  std::vector<GRBLinExpr> getCP3(int t);

  std::vector<state> X_temp_;
  double dt_;  // time step found by the solver
  int trials_ = 0;
  int temporal_ = 0;
  double runtime_ms_ = 0;
  double factor_that_worked_ = 0;
  int N_ = 10;
  mycallback cb_;

protected:
  double cost_;

  double xf_[3 * 3];
  double x0_[3 * 3];
  double v_max_;
  double a_max_;
  double j_max_;
  double DC;
  // double q_;  // weight to the 2nd term in the cost function
  double** x_;
  double** u_;

  int N_of_polytopes_ = 3;

  GRBEnv* env = new GRBEnv();
  GRBModel m = GRBModel(*env);

  std::vector<GRBConstr> at_least_1_pol_cons;  // Constraints at least in one polytope
  std::vector<GRBGenConstr> polytopes_cons;    // Used for the whole trajectory
  std::vector<GRBConstr> polytope_cons;        // Used for the rescue path
  std::vector<GRBConstr> dyn_cons;
  std::vector<GRBConstr> init_cons;
  std::vector<GRBConstr> final_cons;

  std::vector<GRBQConstr> distances_cons;

  std::vector<std::vector<GRBVar>> b;  // binary variables
  std::vector<std::vector<GRBVar>> x;
  std::vector<std::vector<GRBVar>> u;

  vec_Vecf<3> samples_;           // Samples along the rescue path
  vec_Vecf<3> samples_penalize_;  // Samples along the rescue path

  std::vector<double> dist_near_obs_;
  std::vector<LinearConstraint3D> polytopes_;

  std::ofstream times_log;

  int mode_;
  bool forceFinalConstraint_ = true;
  double factor_initial_ = 2;
  double factor_final_ = 2;
  double factor_increment_ = 2;

  int total_not_solved = 0;
  double w_max_ = 1;
};
#endif


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// 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, EVecPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <gtest/gtest.h>

#include <Eigen/Core>
#include <smooth/so3.hpp>

#include "smooth/feedback/ocp_to_nlp.hpp"

template<typename T, std::size_t N>
using Vec = Eigen::Vector<T, N>;

TEST(OcpToNlp, Derivatives2)
{
  // objective
  auto theta = []<typename T>(T tf, Vec<T, 2> x0, Vec<T, 2> xf, Vec<T, 1> q) -> T {
    return (tf - 2) * (tf - 2) + x0.cwiseProduct(xf).squaredNorm() + xf.squaredNorm() + q.sum();
  };

  // dynamics
  auto f = []<typename T>(T t, Vec<T, 2> x, Vec<T, 1> u) -> Vec<T, 2> {
    return Vec<T, 2>{{x.y() + t, x.x() * u.x() * u.x()}};
  };

  // integrals
  auto g = []<typename T>(T t, Vec<T, 2> x, Vec<T, 1> u) -> Vec<T, 1> {
    return Vec<T, 1>{{t + t * x.squaredNorm() + u.squaredNorm()}};
  };

  // running constraint
  auto cr = []<typename T>(T t, Vec<T, 2> x, Vec<T, 1> u) -> Vec<T, 4> {
    Vec<T, 4> ret(4);
    ret << t, t * x * u.x(), u.cwiseAbs2();
    return ret;
  };

  // end constraint
  auto ce = []<typename T>(T tf, Vec<T, 2> x0, Vec<T, 2> xf, Vec<T, 1> q) -> Vec<T, 6> {
    Vec<T, 6> ret(6);
    ret << tf, x0.cwiseProduct(xf), xf, q.cwiseAbs2();
    return ret;
  };

  const smooth::feedback::OCP<
    Vec<double, 2>,
    Vec<double, 1>,
    decltype(theta),
    decltype(f),
    decltype(g),
    decltype(cr),
    decltype(ce)>
    ocp{
      .theta = theta,
      .f     = f,
      .g     = g,
      .cr    = cr,
      .crl   = Vec<double, 4>::Constant(4, -1),
      .cru   = Vec<double, 4>::Constant(4, 1),
      .ce    = ce,
      .cel   = Vec<double, 6>::Constant(6, -1),
      .ceu   = Vec<double, 6>::Constant(6, 1),
    };

  smooth::feedback::Mesh<3, 3> mesh;
  mesh.refine_ph(0, 4);
  mesh.refine_ph(0, 4);

  auto nlp = ocp_to_nlp(ocp, mesh);

  using nlp_t = std::decay_t<decltype(nlp)>;
  static_assert(smooth::feedback::HessianNLP<nlp_t>);

  srand(5);
  const Eigen::VectorXd x      = Eigen::VectorXd::Random(nlp.n());
  const Eigen::VectorXd lambda = Eigen::VectorXd::Random(nlp.m());

  // Analytic derivatives
  const auto & df_dx   = nlp.df_dx(x);
  const auto & d2f_dx2 = nlp.d2f_dx2(x);
  const auto & dg_dx   = nlp.dg_dx(x);
  const auto & d2g_dx2 = nlp.d2g_dx2(x, lambda);

  // Numerical derivatives (of base function)
  const auto [fval, df_dx_num, d2f_dx2_num] =
    smooth::diff::dr<2>([&](const auto & xvar) { return nlp.f(xvar); }, smooth::wrt(x));
  const auto [gval, dg_dx_num] =
    smooth::diff::dr<1>([&](const auto & xvar) { return nlp.g(xvar); }, smooth::wrt(x));
  const auto g_l_fun = [&](Eigen::VectorXd xvar) -> double { return lambda.dot(nlp.g(xvar)); };
  const auto [u1_, u2_, d2g_dx2_num] = smooth::diff::dr<2>(g_l_fun, smooth::wrt(x));

  ASSERT_TRUE(Eigen::MatrixXd(df_dx).isApprox(df_dx_num, 1e-4));
  ASSERT_TRUE(Eigen::MatrixXd(dg_dx).isApprox(dg_dx_num, 1e-4));
  ASSERT_TRUE(Eigen::MatrixXd(Eigen::MatrixXd(d2f_dx2).selfadjointView<Eigen::Upper>())
                .isApprox(d2f_dx2_num, 1e-3));
  ASSERT_TRUE(Eigen::MatrixXd(Eigen::MatrixXd(d2g_dx2).selfadjointView<Eigen::Upper>())
                .isApprox(d2g_dx2_num, 1e-3));
}


//=======================================================================
// Copyright (c)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file knapsack_test.cpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-09-20
 */

#include "test_utils/logger.hpp"
#include "test_utils/knapsack_tags_utils.hpp"

#include "paal/dynamic/knapsack_unbounded.hpp"
#include "paal/dynamic/knapsack_0_1.hpp"
#include "paal/dynamic/knapsack_unbounded_fptas.hpp"
#include "paal/dynamic/knapsack_0_1_fptas.hpp"
#include "paal/utils/floating.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/fusion/include/for_each.hpp>

#include <fstream>

using namespace paal;

namespace {
const int capacity = 6;
using Objects = std::vector<std::pair<int, int>>;
Objects objects{ { 1, 3 }, { 2, 2 }, { 3, 65 }, { 1, 1 }, { 2, 2 }, { 4, 3 },
                 { 1, 1 }, { 10, 23 } };
auto size = [](std::pair<int, int> object) { return object.first; };
auto value = [](std::pair<int, int> object) { return object.second; };
}

static const double OPT = 130;
static const double OPT_0_1 = 70;
static const double OPT_CAP = 6;
static const double EPSILON = 1. / 4.;
static const double VALUE_MULTIPLIER = 1. - EPSILON;
static const double SIZE_MULTIPLIER = 1. + EPSILON;
static const utils::compare<double> compare(0.001);

template <typename MaxValue>
void check(MaxValue maxValue, pd::unbounded_tag, double valMultiplier = 1,
           double capMultiplier = 1) {
    BOOST_CHECK(compare.ge(maxValue.first, OPT * valMultiplier));
    BOOST_CHECK(compare.ge(OPT_CAP * capMultiplier, maxValue.second));
}

template <typename MaxValue>
void check(MaxValue maxValue, pd::zero_one_tag, double valMultiplier = 1,
           double capMultiplier = 1) {
    BOOST_CHECK(compare.ge(maxValue.first, OPT_0_1 * valMultiplier));
    BOOST_CHECK(compare.ge(OPT_CAP * capMultiplier, maxValue.second));
}

template <typename IntegralTag, typename IsZeroOne,
          typename RetrieveSolution = pd::retrieve_solution_tag>
void detail_knapsack_and_check() {
    auto maxValue = detail_knapsack<IntegralTag, IsZeroOne, RetrieveSolution>(
        objects, capacity, size, value);
    check(maxValue, IsZeroOne());
}

BOOST_AUTO_TEST_CASE(KnapsackOverloads) {
    detail_knapsack_and_check<pd::integral_value_tag, pd::unbounded_tag>();
    detail_knapsack_and_check<pd::integral_size_tag, pd::unbounded_tag>();
    detail_knapsack_and_check<pd::integral_value_and_size_tag,
                              pd::unbounded_tag>();
    detail_knapsack_and_check<pd::integral_value_tag, pd::zero_one_tag,
                              pd::retrieve_solution_tag>();
    detail_knapsack_and_check<pd::integral_size_tag, pd::zero_one_tag,
                              pd::retrieve_solution_tag>();
    detail_knapsack_and_check<pd::integral_value_and_size_tag, pd::zero_one_tag,
                              pd::retrieve_solution_tag>();
    detail_knapsack_and_check<pd::integral_value_tag, pd::zero_one_tag,
                              pd::no_retrieve_solution_tag>();
    detail_knapsack_and_check<pd::integral_size_tag, pd::zero_one_tag,
                              pd::no_retrieve_solution_tag>();
    detail_knapsack_and_check<pd::integral_value_and_size_tag, pd::zero_one_tag,
                              pd::no_retrieve_solution_tag>();
}

// Knapsack
BOOST_AUTO_TEST_CASE(Knapsack) {
    Objects result;
    LOGLN("Knapsack");
    auto maxValue = paal::knapsack_unbounded(
        objects, capacity, std::back_inserter(result), size, value);

    check(maxValue, pd::unbounded_tag{});
    print_result(maxValue, result, pd::retrieve_solution_tag());
}

// Knapsack 0/1
BOOST_AUTO_TEST_CASE(Knapsack_0_1) {
    Objects result;
    LOGLN("Knapsack 0/1");
    auto maxValue = paal::knapsack_0_1(objects, capacity,
                                       std::back_inserter(result), size, value);
    check(maxValue, pd::zero_one_tag());
    print_result(maxValue, result, pd::retrieve_solution_tag());
}

BOOST_AUTO_TEST_CASE(Knapsack_0_1_no_output) {
    Objects result;
    LOGLN("Knapsack 0/1 no output");
    auto maxValue =
        paal::knapsack_0_1_no_output(objects, capacity, size, value);

    check(maxValue, pd::zero_one_tag());
    print_max_value(maxValue);
}

// Knapsack fptas value
BOOST_AUTO_TEST_CASE(Knapsack_fptas_value) {
    Objects result;
    LOGLN("Knapsack fptas value");
    auto maxValue = paal::knapsack_unbounded_on_value_fptas(
        EPSILON, objects, capacity, std::back_inserter(result), size, value);

    check(maxValue, pd::unbounded_tag{}, VALUE_MULTIPLIER);
    print_result(maxValue, result, pd::retrieve_solution_tag());
}

// TODO this tests is very weak because it runs standard algorithm no fptas
// Knapsack fptas size
BOOST_AUTO_TEST_CASE(Knapsack_fptas_size) {
    Objects result;
    LOGLN("Knapsack fptas size");
    auto maxValue = paal::knapsack_unbounded_on_size_fptas(
        EPSILON, objects, capacity, std::back_inserter(result), size, value);

    print_result(maxValue, result, pd::retrieve_solution_tag());
    check(maxValue, pd::unbounded_tag{}, 1., SIZE_MULTIPLIER);
}

// Knapsack 0/1: no output iterator size fptas
BOOST_AUTO_TEST_CASE(Knapsack_0_1_no_output_size_fptas) {
    Objects result;
    LOGLN("Knapsack 0/1 no output size fptas");
    auto maxValue = paal::knapsack_0_1_no_output_on_size_fptas(
        EPSILON, objects, capacity, size, value);

    check(maxValue, pd::zero_one_tag(), 1, SIZE_MULTIPLIER);
    print_max_value(maxValue);
}

// Knapsack 0/1: no output iterator value fptas
BOOST_AUTO_TEST_CASE(Knapsack_0_1_no_output_value_fptas) {
    Objects result;
    LOGLN("Knapsack 0/1 no output value fptas");
    auto maxValue = paal::knapsack_0_1_no_output_on_value_fptas(
        EPSILON, objects, capacity, size, value);

    check(maxValue, pd::zero_one_tag(), VALUE_MULTIPLIER);
    print_max_value(maxValue);
}

// Knapsack 0/1 value fptas
BOOST_AUTO_TEST_CASE(Knapsack_0_1_value_fptas) {
    Objects result;
    LOGLN("Knapsack 0/1 on value fptas");
    auto maxValue = paal::knapsack_0_1_on_value_fptas(
        EPSILON, objects, capacity, std::back_inserter(result), size, value);

    check(maxValue, pd::zero_one_tag(), VALUE_MULTIPLIER);
    print_result(maxValue, result, pd::retrieve_solution_tag());
}

// Knapsack 0/1  size fptas
BOOST_AUTO_TEST_CASE(Knapsack_0_1_size_fptas) {
    Objects result;
    LOGLN("Knapsack 0/1 on size fptas");
    auto maxValue = paal::knapsack_0_1_on_size_fptas(
        EPSILON, objects, capacity, std::back_inserter(result), size, value);

    check(maxValue, pd::zero_one_tag(), 1, SIZE_MULTIPLIER);
    print_result(maxValue, result, pd::retrieve_solution_tag());
}


/* D.R. Reynolds
   Math 6321 @ SMU
   Fall 2020  */

// Inclusions
#include <stdlib.h>
#include <iostream>
#include <armadillo>
using namespace arma;

// Example routine to show how to perform C++ output from Armadillo's
// `mat' and 'vec' classes, followed by input/plotting in Matlab/Python
int main(int argc, char **argv) {

  // get problem size from command line, otherwise set to 201
  int N = 201;
  if (argc > 1)
    N = atoi(argv[1]);
  std::cout << "\nRunning I/O test using vectors of size N = " << N << std::endl;

  // create x data
  vec x = linspace(-1.0, 1.0, N);

  // create function data (first 5 odd-degree Chebyshev polynomials)
  mat T(N,5);
  for (int j=0; j<5; j++)
    for (int i=0; i<N; i++)
      T(i,j) = cos((j*2+1.0) * acos(x(i)));

  // save data to disk
  x.save("x.txt", arma::raw_ascii);
  T.save("T.txt", arma::raw_ascii);

  std::cout << "Completed writing data to disk: x.txt and T.txt\n\n";
  return 0;
} // end main


#include <boost/test/unit_test.hpp>

#include <crave/ConstrainedRandom.hpp>

#include <boost/format.hpp>

#include <set>
#include <iostream>

// using namespace std;
using namespace crave;

using boost::format;

BOOST_FIXTURE_TEST_SUITE(Distribution_t, Context_Fixture)

struct s_crv_variable_dist_t1 : public crv_sequence_item {
  s_crv_variable_dist_t1(crv_object_name) {}
  crv_variable<int> v;
  crv_constraint con = {
      dist(v(), make_distribution(range<int>(0, 5), range<int>(50, 65), range<int>(100, 125)))};
};

BOOST_AUTO_TEST_CASE(crv_variable_dist_t1) {
  s_crv_variable_dist_t1 item("item");
  std::map<int, int> s;
  int total = 100000;
  for (int i = 0; i < total; i++) {
    BOOST_REQUIRE(item.randomize());
    BOOST_REQUIRE((0 <= item.v && item.v <= 5) || (50 <= item.v && item.v <= 65) || (100 <= item.v && item.v <= 125));
    ++s[item.v];
  }
  for (int i = 0; i <= 200; i++)
    if ((0 <= i && i <= 5) || (50 <= i && i <= 65) || (100 <= i && i <= 125))
      BOOST_REQUIRE(s.find(i) != s.end());
}

struct s_crv_variable_dist_t2 : public crv_sequence_item {
  s_crv_variable_dist_t2(crv_object_name) {}
  crv_variable<int> v;
  crv_constraint con;
};

BOOST_AUTO_TEST_CASE(crv_variable_dist_t2) {
  s_crv_variable_dist_t2 item("item");
  BOOST_REQUIRE_THROW(
      item.con = {dist(item.v(), make_distribution(range<int>(0, 10), range<int>(50, 75), range<int>(30, 51)))},
      std::runtime_error);
}

struct s_crv_variable_dist_t3 : public crv_sequence_item {
  s_crv_variable_dist_t3(crv_object_name) {}
  crv_variable<char> v;
  crv_constraint con = {dist(v(), make_distribution(weighted_range<char>(1, 5, 50), weighted_range<char>(10, 20, 20),
                        weighted_range<char>(-50, -50, 30)))};
};

BOOST_AUTO_TEST_CASE(crv_variable_dist_t3) {
  s_crv_variable_dist_t3 item("item");
  int cnt1 = 0, cnt2 = 0, cnt3 = 0;
  int total = 50000;
  for (int i = 0; i < total; i++) {
    BOOST_REQUIRE(item.randomize());
    BOOST_REQUIRE((1 <= item.v && item.v <= 5) || (10 <= item.v && item.v <= 20) || (item.v == -50));
    if (1 <= item.v && item.v <= 5) cnt1++;
    if (10 <= item.v && item.v <= 20) cnt2++;
    if (item.v == -50) cnt3++;
  }
  BOOST_REQUIRE_LT(cnt2, cnt3);
  BOOST_REQUIRE_LT(cnt3, cnt1);
}

struct s_crv_variable_dist_t4 : public crv_sequence_item {
  s_crv_variable_dist_t4(crv_object_name) {}
  crv_variable<int> v;
  crv_constraint x = {dist(v(), make_distribution(range<int>(0, 10), range<int>(50, 75), range<int>(100, 200)))};
  crv_constraint y = {dist(v(), make_distribution(range<int>(5000, 6000)))};
};

BOOST_AUTO_TEST_CASE(crv_variable_dist_t4) {
  s_crv_variable_dist_t4 item("item");
  item.x.deactivate();
  int total = 10000;
  for (int i = 0; i < total; i++) {
    BOOST_REQUIRE(item.randomize());
    BOOST_REQUIRE(5000 <= item.v && item.v <= 6000);
  }
}

struct s_dist_of_boolean25 : crv_sequence_item {
  s_dist_of_boolean25(crv_object_name) {}
  crv_variable<bool> a;
  crv_constraint con = {dist(a(), distribution<bool>::create(0.25))};
};

BOOST_AUTO_TEST_CASE(dist_of_boolean25) {
  s_dist_of_boolean25 item("item");
  int counter = 0;
  for (unsigned i = 0; i < 1000; i++) {
    BOOST_REQUIRE(item.randomize());
    if (item.a) {
      ++counter;
    } else {
      --counter;
    }
  }

  BOOST_REQUIRE_LT(counter, 0);
}

struct s_dist_of_boolean50 : crv_sequence_item {
  s_dist_of_boolean50(crv_object_name) {}
  crv_variable<bool> a;
  crv_constraint con = {dist(a(), distribution<bool>::create(0.5))};
};

BOOST_AUTO_TEST_CASE(dist_of_boolean50) {
  s_dist_of_boolean50 item("item");
  int counter = 0;
  for (unsigned i = 0; i < 1000; i++) {
    BOOST_REQUIRE(item.randomize());
    if (item.a) {
      ++counter;
    } else {
      --counter;
    }
  }

  BOOST_REQUIRE_LT(counter, 280);
  BOOST_REQUIRE_GT(counter, -280);
}

struct s_dist_of_boolean75 : public crv_sequence_item {
  s_dist_of_boolean75(crv_object_name) {}
  crv_variable<bool> a;
  crv_constraint con = {dist(a(), distribution<bool>::create(0.75))};
};

BOOST_AUTO_TEST_CASE(dist_of_boolean75) {
  s_dist_of_boolean75 item("item");
  int counter = 0;
  for (unsigned i = 0; i < 1000; i++) {
    BOOST_REQUIRE(item.randomize());
    if (item.a) {
      ++counter;
    } else {
      --counter;
    }
  }

  BOOST_REQUIRE_GT(counter, 0);
}

BOOST_AUTO_TEST_SUITE_END()  // Context

//  vim: ft=cpp:ts=2:sw=2:expandtab


#define BOOST_TEST_MODULE matrix

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>

#include <mla/matrix/all.h++>

typedef boost::mpl::list<
	mla::matrix::DenseRowMajor<float>,
	mla::matrix::DenseRowMajor<double>,
	mla::matrix::Diagonal<float>,
	mla::matrix::Diagonal<double>,
	mla::matrix::SparseDOK<float>,
	mla::matrix::SparseDOK<double>,
	mla::matrix::SparseCRS<float>,
	mla::matrix::SparseCRS<double>
> matrix_type_list;

BOOST_AUTO_TEST_SUITE(test_matrix)

BOOST_AUTO_TEST_CASE_TEMPLATE( Declaration, MatrixType, matrix_type_list )
{
	size_t row_size = 3;
	size_t column_size = 3;

	MatrixType m(row_size,column_size);

	BOOST_CHECK_EQUAL(m.rows(), row_size);
	BOOST_CHECK_EQUAL(m.columns(), column_size);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( Clearing, MatrixType, matrix_type_list )
{
	size_t row_size = 3;
	size_t column_size = 3;

	MatrixType m(row_size,column_size);

	m.setZero();

	for(unsigned int i = 0; i < row_size; i++)
	{
		for(unsigned int j = 0; j < column_size; j++)
		{
			BOOST_CHECK_EQUAL(m.getValue(i, j), (typename MatrixType::scalar_type)0);
		}
	}

}

BOOST_AUTO_TEST_CASE_TEMPLATE( set_eye_square, MatrixType, matrix_type_list )
{
	size_t row_size = 3;
	size_t column_size = 3;

	MatrixType m(row_size,column_size);

	m.setEye();

	for(unsigned int i = 0; i < row_size; i++)
	{
		for(unsigned int j = 0; j < column_size; j++)
		{
			BOOST_CHECK_EQUAL(m.getValue(i, j), (typename MatrixType::scalar_type)(i==j?1:0) );
		}
	}

}

BOOST_AUTO_TEST_CASE_TEMPLATE( set_eye_rectangular_row, MatrixType, matrix_type_list )
{
	size_t row_size = 3;
	size_t column_size = 5;

	MatrixType m(row_size,column_size);

	m.setEye();

	for(unsigned int i = 0; i < row_size; i++)
	{
		for(unsigned int j = 0; j < column_size; j++)
		{
			BOOST_CHECK_EQUAL(m.getValue(i, j), (typename MatrixType::scalar_type)(i==j?1:0) );
		}
	}

}

BOOST_AUTO_TEST_CASE_TEMPLATE( set_eye_rectangular_column, MatrixType, matrix_type_list )
{
	size_t row_size = 5;
	size_t column_size = 3;

	MatrixType m(row_size, column_size);

	m.setEye();

	for(unsigned int i = 0; i < row_size; i++)
	{
		for(unsigned int j = 0; j < column_size; j++)
		{
			BOOST_CHECK_EQUAL(m.getValue(i, j), (typename MatrixType::scalar_type)(i==j?1:0) );
		}
	}

}

BOOST_AUTO_TEST_CASE_TEMPLATE( reference_assigning_one, MatrixType, matrix_type_list )
{
	if( mla::matrix::Traits<MatrixType>::is_writeable() )
	{
		MatrixType m(3,3);
		m.setZero();

		m(1,1) = 1.0;

		BOOST_CHECK_EQUAL(m.getValue(0, 0), 0.0);
		BOOST_CHECK_EQUAL(m.getValue(0, 1), 0.0);
		BOOST_CHECK_EQUAL(m.getValue(0, 2), 0.0);
		BOOST_CHECK_EQUAL(m.getValue(1, 0), 0.0);
		BOOST_CHECK_EQUAL(m.getValue(1, 1), 1.0);
		BOOST_CHECK_EQUAL(m.getValue(1, 2), 0.0);
		BOOST_CHECK_EQUAL(m.getValue(2, 0), 0.0);
		BOOST_CHECK_EQUAL(m.getValue(2, 1), 0.0);
		BOOST_CHECK_EQUAL(m.getValue(2, 2), 0.0);
	}
}

BOOST_AUTO_TEST_CASE_TEMPLATE( reference_assigning_same_row_ordered, MatrixType, matrix_type_list )
{
	if( mla::matrix::Traits<MatrixType>::is_writeable() )
	{
		MatrixType m(3,3);
		m.setZero();

		m(1,0) = 1.0;
		m(1,1) = 1.0;

		BOOST_CHECK_EQUAL(m.getValue(0, 0), 0.0);
		BOOST_CHECK_EQUAL(m.getValue(0, 1), 0.0);
		BOOST_CHECK_EQUAL(m.getValue(0, 2), 0.0);
		BOOST_CHECK_EQUAL(m.getValue(1, 0), 1.0);
		BOOST_CHECK_EQUAL(m.getValue(1, 1), 1.0);
		BOOST_CHECK_EQUAL(m.getValue(1, 2), 0.0);
		BOOST_CHECK_EQUAL(m.getValue(2, 0), 0.0);
		BOOST_CHECK_EQUAL(m.getValue(2, 1), 0.0);
		BOOST_CHECK_EQUAL(m.getValue(2, 2), 0.0);
	}
}

BOOST_AUTO_TEST_CASE_TEMPLATE( reference_assigning_same_row_unordered, MatrixType, matrix_type_list )
{
	if( mla::matrix::Traits<MatrixType>::is_writeable() )
	{
		MatrixType m(3,3);
		m.setZero();

		m(1,1) = 1.0;
		m(1,0) = 1.0;

		BOOST_CHECK_EQUAL(m.getValue(0, 0), 0);
		BOOST_CHECK_EQUAL(m.getValue(0, 1), 0);
		BOOST_CHECK_EQUAL(m.getValue(0, 2), 0);
		BOOST_CHECK_EQUAL(m.getValue(1, 0), 1);
		BOOST_CHECK_EQUAL(m.getValue(1, 1), 1);
		BOOST_CHECK_EQUAL(m.getValue(1, 2), 0);
		BOOST_CHECK_EQUAL(m.getValue(2, 0), 0);
		BOOST_CHECK_EQUAL(m.getValue(2, 1), 0);
		BOOST_CHECK_EQUAL(m.getValue(2, 2), 0);
	}
}

BOOST_AUTO_TEST_CASE_TEMPLATE( reference_assigning_different_row_ordered, MatrixType, matrix_type_list )
{
	if( mla::matrix::Traits<MatrixType>::is_writeable() )
	{
		MatrixType m(3,3);
		m.setZero();

		m(1,1) = 1.0;
		m(2,1) = 1.0;

		BOOST_CHECK_EQUAL(m.getValue(0, 0), 0);
		BOOST_CHECK_EQUAL(m.getValue(0, 1), 0);
		BOOST_CHECK_EQUAL(m.getValue(0, 2), 0);
		BOOST_CHECK_EQUAL(m.getValue(1, 0), 0);
		BOOST_CHECK_EQUAL(m.getValue(1, 1), 1);
		BOOST_CHECK_EQUAL(m.getValue(1, 2), 0);
		BOOST_CHECK_EQUAL(m.getValue(2, 0), 0);
		BOOST_CHECK_EQUAL(m.getValue(2, 1), 1);
		BOOST_CHECK_EQUAL(m.getValue(2, 2), 0);
	}
}

BOOST_AUTO_TEST_CASE_TEMPLATE( value_setting, MatrixType, matrix_type_list )
{
	if( mla::matrix::Traits<MatrixType>::is_writeable() )
	{
		MatrixType m(3,3);
		m.setZero();

		m.setValue( 1, 1, (typename MatrixType::scalar_type)1);

		BOOST_CHECK_EQUAL(m.getValue(1, 1), 1.0);
		BOOST_CHECK_EQUAL(m.getValue(0, 0), 0.0);
		BOOST_CHECK_EQUAL(m.getValue(0, 1), 0.0);
		BOOST_CHECK_EQUAL(m.getValue(0, 2), 0.0);
		BOOST_CHECK_EQUAL(m.getValue(1, 0), 0.0);
		BOOST_CHECK_EQUAL(m.getValue(1, 1), 1.0);
		BOOST_CHECK_EQUAL(m.getValue(1, 2), 0.0);
		BOOST_CHECK_EQUAL(m.getValue(2, 0), 0.0);
		BOOST_CHECK_EQUAL(m.getValue(2, 1), 0.0);
		BOOST_CHECK_EQUAL(m.getValue(2, 2), 0.0);
	}
}

BOOST_AUTO_TEST_CASE_TEMPLATE( matrix_assigning, MatrixType, matrix_type_list )
{
	if( mla::matrix::Traits<MatrixType>::is_writeable() )
	{
		MatrixType m(3,3), n;
		m.setZero();

		m.setValue( 1, 1, 1.0);

		n = m;

		BOOST_CHECK_EQUAL(n.getValue(1, 1), 1.0);
		BOOST_CHECK_EQUAL(n.getValue(0, 0), 0.0);
		BOOST_CHECK_EQUAL(n.getValue(0, 1), 0.0);
		BOOST_CHECK_EQUAL(n.getValue(0, 2), 0.0);
		BOOST_CHECK_EQUAL(n.getValue(1, 0), 0.0);
		BOOST_CHECK_EQUAL(n.getValue(1, 1), 1.0);
		BOOST_CHECK_EQUAL(n.getValue(1, 2), 0.0);
		BOOST_CHECK_EQUAL(n.getValue(2, 0), 0.0);
		BOOST_CHECK_EQUAL(n.getValue(2, 1), 0.0);
		BOOST_CHECK_EQUAL(m.getValue(2, 2), 0.0);
	}
}

BOOST_AUTO_TEST_SUITE_END()


/*! \file
  \brief An example to demonstrate autoscaling with @b multiple STL containers.
  \details See also demo_2d_autoscaling.cpp, auto_1d_plot.cpp,
  demo_1d_axis_scaling.cpp, demo_2d_autoscaling_vector.cpp and auto_1d_container.cpp.
*/
//  auto_2d_plot.cpp
// Copyright Paul A Bristow 2008, 2021

// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

//[auto_2d_plot_1

/*`First we need a few includes to use Boost.Plot:
*/

#include <boost/svg_plot/svg_2d_plot.hpp>
//  using namespace boost::svg;

#include <utility>
 // using std::pair;
#include <map>
 // using std::map;
#include <set>
//  using std::multiset;
#include <iostream>
//  using std::cout;
//  using std::endl;

#include <limits>
//  using std::numeric_limits;
//] [/auto_2d_plot_1]

//! Getting the max and min of X and Y data-points.
template <typename T> // T an STL container: array, vector ...
void s(T& container, // Container Data-series to plot - entire container.
               // (not necessarily ordered, so will find min and max).
               double* x_min,  double* x_max,
               double* y_min,  double* y_max
               )
{
  typedef typename T::const_iterator iter;
  std::pair<iter, iter> result = boost::minmax_element(container.begin(), container.end());
  // minmax_element is efficient for maps because can use knowledge of being sorted,
  // BUT only if it can be assumed that no values are 'at limits',
  // infinity, NaN, max_value, min_value, denorm_min.
  // Otherwise it is necessary to inspect all values individually.
  std::pair<const double, double> px = *result.first;
  std::pair<const double, double> py = *result.second;
  *x_min = px.first;
  *x_max = py.first;
  *y_min = px.second;
  *y_max = py.second;

  std::cout << "s x_min " << *x_min << ", x_max " << *x_max << std::endl; // x_min 1, x_max 7.3
  std::cout << "s y_min " << *y_min << ", y_max " << *y_max << std::endl; // y_min 3.2, y_max 9.1
} // template <class T> int scale_axis  T an STL container: array, vector ...

int main()
{
//[auto_2d_plot_2
  /*`This example uses a single @c std::map to demonstrate autoscaling.
  We create a @c std::map to hold our data-series.
  */
  std::map<const double, double> my_map;
  /*`
  Inserting some fictional values also sorts the data.
  The index value in [ ] is the X value.
  */
  my_map[1.1] = 3.2;
  my_map[7.3] = 9.1;
  my_map[2.1] = 5.4;

/*`Also include some 'at limits' values that might confuse autoscaling.
*/
  my_map[99.99] = std::numeric_limits<double>::quiet_NaN();
  my_map[999.9] = std::numeric_limits<double>::infinity();
  my_map[999.] = +std::numeric_limits<double>::infinity();

  /*`Next a 2D plot is created using defaults for the very many possible settings.
  */
  try
  { // try'n'catch clocks are needed to ensure error messages from any exceptions are shown.
   /*`Construct `myplot` and add at least a title,
    specify the both X and Y axes are to use autoscaling,
    and add the one data-series to be plotted.
  */
  using  namespace boost::svg;
  svg_2d_plot my_plot;
  my_plot.title("Autoscale example 1"); // Add a title.
  my_plot.xy_autoscale(my_map); // Specify that both X and Y-axes are to use autoscaling,
  my_plot.plot(my_map); // Add the one data-series to be plotted.
  my_plot.write("./auto_2d_plot_1.svg"); // And write the SVG image to a file.

  /*`We can show the ranges used by autoscaling; */
  std::cout << "X min " << my_plot.x_range().first << ", X max " << my_plot.x_range().second << std::endl;
  std::cout << "Y min " << my_plot.y_range().first << ", Y max "  << my_plot.y_range().second << std::endl;

  /*`Had we know that there were no 'at limits' (NaN or infinite) values, we could have chosen to skip the checks.
  This might be important for speed if there are thousands of data values.
  */
  my_plot.autoscale_check_limits(false);  // Skip checks for speed.
/*`The possible cost is that it will fail at run-time if there are any infinite or NaNs.
*/

  svg_2d_plot my_plot_2;
  my_plot_2.title("Autoscale example 2"); // Add a title.
  my_plot_2.plot(my_map); // Add the one data-series to be plotted.

/*`and specify:*/

  my_plot_2.y_autoscale(0.4, 9.3); // autoscale using two doubles.

/*`which will chose a neater scale range from 0 to 10 for the Y axis. */

/*`It is also possible to fully control the factors used by autoscaling with function `scale_axis`.
 For examples, see [../example/demo_1d_axis_scaling.cpp demo_1d_axis_scaling.cpp] */

  my_plot_2.write("./auto_2d_plot_2.svg"); // And write another SVG image to another file.

//] [/auto_2d_plot_2]
  }
  catch(const std::exception& e)
  {
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }
  return 0;
}

/*
//[auto_2d_plot_output
Output:

Autorun "j:\Cpp\SVG\Debug\auto_2d_plot.exe"
Checked: x_min 1.1, x_max 7.3, y_min 3.2, y_max 9.1, 3 'good' values, 3 values at limits.
X min 1, X max 8
Y min 3, Y max 10
X min 1, X max 8
Y min 0, Y max 10
//] [/auto_2d_plot_output]

*/


#include <mex.h>
#include "eigenlab.h"
#include <Eigen/Dense>

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
    plhs[0] = mxCreateDoubleMatrix(2, 2, mxREAL);

    Eigen::MatrixXd mat(3, 2);
    mat << 1, 4,
           2, 5,
           3, 6;

    eigenToMxArray(mat, plhs[0], 3, 2);
    return;
}


//
//  random.h
//  yamcmc++
//
//  Created by Brandon Kelly on 3/2/13.
//  Copyright (c) 2013 Brandon Kelly. All rights reserved.
//

#ifndef __yamcmc____random__
#define __yamcmc____random__

#include <iostream>
//
//  random.hpp
//  yamcmc++
//
//  Created by Dr. Brandon C. Kelly on 11/21/12.
//
//  Header file for a class that generates pseudorandom numbers from common distributions.
//  This is basically a wrapper for BOOST::RANDOM
//

// Standard includes
#include <string>
#include <vector>
// Boost includes
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/exponential_distribution.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/lognormal_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/student_t_distribution.hpp>
#include <boost/random/chi_squared_distribution.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/uniform_int_distribution.hpp>

// Other includes
#include <armadillo>

// Class containing methods to generate random numbers from various
// distributions. These should be self-explanatory, but see
// random.cpp for more details.
class RandomGenerator {
public:
    void SetSeed(unsigned long seed) const; // Set the random number generator seed. Be Careful with this!
    void SaveSeed(std::string seed_filename = "seed.txt") const; // Save the random number generator seed to a file.
    void RecoverSeed(std::string seed_filename = "seed.txt") const;
    double exp(double lambda=1.0);
    double normal(double mu=0.0, double sigma=1.0); // Univariate normal
    arma::vec normal(arma::mat covar); // Multivariate normal
    double lognormal(double logmean=0.0, double frac_sigma=1.0);
    double uniform(double lowbound=0.0, double upbound=1.0);
    int uniform(int lowbound, int upbound);
    double powerlaw(double lower, double upper, double slope);
    double tdist(double dof=1.0, double mean=0.0, double scale=1.0);
    double chisqr(int dof=1);
    double scaled_inverse_chisqr(int dof=1, double ssqr=1.0);
    double gamma(double alpha=1.0, double beta=1.0);
    double invgamma(double alpha=1.0, double beta=1.0);
    int uniform_integer(int lowbound, int upbound);
    // Additional methods to be added later
    arma::vec mtdist(arma::mat covar, double dof=1.0);
    double beta();
    double weibull();
private:
    // Private functors for the various distributions.
    boost::random::exponential_distribution<> exp_;
    boost::random::normal_distribution<> normal_;
    boost::random::lognormal_distribution<> lognormal_;
    boost::random::uniform_real_distribution<> uniform_;
    boost::random::uniform_int_distribution<> uniform_integer_;
    boost::random::student_t_distribution<> tdist_;
    boost::random::chi_squared_distribution<> chisqr_;
    boost::random::gamma_distribution<> gamma_;
};

#endif /* defined(__yamcmc____random__) */


#include <doctest/doctest.h>  // for ResultBuilder, CHECK, TestCase
// #include <__config>                // for std
#include <boost/utility/string_view.hpp>  // for boost::string_view
#include <ckpttn/HierNetlist.hpp>         // for HierNetlist, SimpleHierNetlist
#include <ckpttn/netlist.hpp>             // for Netlist, SimpleNetlist
#include <cstdint>                        // for uint8_t
#include <memory>                         // for unique_ptr
#include <py2cpp/set.hpp>                 // for set
#include <vector>                         // for vector, operator==

using namespace std;

extern auto create_test_netlist() -> SimpleNetlist;  // import create_test_netlist
extern auto create_dwarf() -> SimpleNetlist;         // import create_dwarf
extern auto readNetD(boost::string_view netDFileName) -> SimpleNetlist;
extern void readAre(SimpleNetlist& H, boost::string_view areFileName);
// extern tuple<py::set<node_t>, int>
// min_net_cover_pd(SimpleNetlist &, const vector<int> &);

using node_t = SimpleNetlist::node_t;
extern auto create_contraction_subgraph(const SimpleNetlist&, const py::set<node_t>&)
    -> unique_ptr<SimpleHierNetlist>;

//
// Primal-dual algorithm for minimum vertex cover problem
//

// TEST_CASE("Test min_net_cover_pd dwarf", "[test_min_cover]") {
//     auto H = create_dwarf();
//     auto [S, cost] = min_net_cover_pd(H, H.module_weight);
//     CHECK(cost == 3);
// }

// TEST_CASE("Test min_net_cover_pd ibm01", "[test_min_cover]") {
//     auto H = readNetD("../../testcases/ibm01.net");
//     readAre(H, "../../testcases/ibm01.are");
//     auto [S, cost] = min_net_cover_pd(H, H.net_weight);
//     CHECK(cost == 4053);
// }

TEST_CASE("Test contraction subgraph dwarf") {
    const auto H = create_dwarf();
    const auto H2 = create_contraction_subgraph(H, py::set<node_t>{});
    // auto H3 = create_contraction_subgraph(*H2, py::set<node_t> {});
    CHECK(H2->number_of_modules() < 7);
    CHECK(H2->number_of_nets() == 3);
    // CHECK(H2->number_of_pins() < 14);
    CHECK(H2->get_max_net_degree() <= 3);

    auto part = vector<uint8_t>(H.number_of_modules(), 0);
    auto part2 = vector<uint8_t>(H2->number_of_modules(), 0);
    auto part3 = vector<uint8_t>(H2->number_of_modules(), 0);
    part2[0] = part2[2] = 1;
    part2[1] = 2;
    H2->projection_down(part2, part);
    H2->projection_up(part, part3);
    CHECK(part2 == part3);
}

TEST_CASE("Test contraction subgraph ibm01") {
    auto H = readNetD("../../testcases/ibm01.net");
    readAre(H, "../../testcases/ibm01.are");
    auto H2 = create_contraction_subgraph(H, py::set<node_t>{});
    auto H3 = create_contraction_subgraph(*H2, py::set<node_t>{});
    CHECK(H2->number_of_modules() < H.number_of_modules());
    CHECK(H2->number_of_nets() < H.number_of_nets());
    // CHECK(H2->number_of_pins() < H.number_of_pins());
    CHECK(H2->get_max_net_degree() <= H.get_max_net_degree());

    auto part2 = vector<uint8_t>(H2->number_of_modules(), 0);
    auto part3 = vector<uint8_t>(H3->number_of_modules(), 0);
    auto part4 = vector<uint8_t>(H3->number_of_modules(), 0);
    auto i = uint8_t(0);

    for (auto& item : part3) {
        item = ++i % 6;
    }
    H3->projection_down(part3, part2);
    H3->projection_up(part2, part4);
    CHECK(part3 == part4);
}

TEST_CASE("Test contraction subgraph ibm18") {
    auto H = readNetD("../../testcases/ibm18.net");
    readAre(H, "../../testcases/ibm18.are");
    auto H2 = create_contraction_subgraph(H, py::set<node_t>{});
    auto H3 = create_contraction_subgraph(*H2, py::set<node_t>{});
    CHECK(H2->number_of_modules() < H.number_of_modules());
    CHECK(H2->number_of_nets() < H.number_of_nets());
    // CHECK(H2->number_of_pins() < H.number_of_pins());
    CHECK(H2->get_max_net_degree() <= H.get_max_net_degree());

    auto part2 = vector<uint8_t>(H2->number_of_modules(), 0);
    auto part3 = vector<uint8_t>(H3->number_of_modules(), 0);
    auto part4 = vector<uint8_t>(H3->number_of_modules(), 0);
    for (auto i = 0u; i != H3->number_of_modules(); ++i) {
        part3[i] = uint8_t(i);
    }
    H3->projection_down(part3, part2);
    H3->projection_up(part2, part4);
    CHECK(part3 == part4);
}


#ifndef GENETIC_CPP
#define GENETIC_CPP

#include "Game/Render.hpp"
#include "Genetic/Individual.hpp"
#include <Eigen/Dense>
#include <algorithm>
#include <random>
#include <utility>
#include <vector>

namespace GeneticUtils {
    // RNGs
    extern std::mt19937 gen;
    extern std::normal_distribution<double> randn;
    extern std::uniform_real_distribution<double> rand;
}; // namespace GeneticUtils

class GeneticAlgo {
protected:
    static int generations;
    static int popSize;
    static int offspringsSize;
    static double etaX;
    static double mutationProb;

    // Genetic operators
    // CROSSOVER
    static std::pair<Eigen::MatrixXd, Eigen::MatrixXd> simulatedBinaryCrossover(const Eigen::MatrixXd &, const Eigen::MatrixXd &);
    static std::pair<Eigen::VectorXd, Eigen::VectorXd> simulatedBinaryCrossover(const Eigen::VectorXd &, const Eigen::VectorXd &);
    static std::pair<Eigen::MatrixXd, Eigen::MatrixXd> singlePointCrossover(const Eigen::MatrixXd &, const Eigen::MatrixXd &);
    static std::pair<Eigen::VectorXd, Eigen::VectorXd> singlePointCrossover(const Eigen::VectorXd &, const Eigen::VectorXd &);

    // MUTATION
    static void gaussianMutation(Eigen::MatrixXd &);
    static void gaussianMutation(Eigen::VectorXd &);

    int currentGen;                     // current generation number
    std::vector<Individual> population; // current population
    std::vector<Individual> offsprings; // new offsprings
    std::vector<Individual> globalBest; // best individuals

    void updateAndLog(int &, bool, bool); // used for updating global best and logging

public:
    GeneticAlgo();
    void calculateFitness();                    // find fitness of offsprings by running the game
    void elitismSelection();                    // Select the best individuals from offsprings to serve as new population
    void crossoverAndMutation(double totalFit); // Perform roulette selection, crossover and mutation to generate 2 new offsprings
    void nextGeneration();                      // Generate the offsprings for next generation
    void start(bool = true, bool = false);      // start the GA
};

#endif