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

/// @project        Library/Mathematics
/// @file           Library/Mathematics/Geometry/2D/Objects/MultiPolygon.cpp
/// @author         Lucas Brémond <lucas@loftorbital.com>
/// @license        Apache License 2.0

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

#include <Library/Mathematics/Geometry/2D/Transformation.hpp>
#include <Library/Mathematics/Geometry/2D/Objects/MultiPolygon.hpp>

#include <Library/Core/Types/String.hpp>
#include <Library/Core/Types/Size.hpp>
#include <Library/Core/Types/Index.hpp>
#include <Library/Core/Error.hpp>
#include <Library/Core/Utilities.hpp>

#include <boost/geometry/io/wkt/wkt.hpp>
#include <boost/geometry/strategies/transform/matrix_transformers.hpp>
#include <boost/geometry/strategies/transform.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry.hpp>
#include <boost/numeric/ublas/matrix.hpp>

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

namespace library
{
namespace math
{
namespace geom
{
namespace d2
{
namespace objects
{

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

using boost::geometry::cs::cartesian ;
using boost::geometry::model::point ;
using boost::geometry::model::ring ;
using boost::geometry::model::polygon ;
using boost::geometry::model::multi_polygon ;

using library::core::types::Index ;
using library::core::types::Size ;
using library::core::types::String ;

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

class MultiPolygon::Impl
{

    public:

                                Impl                                        ( ) ;

                                Impl                                        (   const   Array<Polygon>&             aPolygonArray                               ) ;

                                ~Impl                                       ( ) = default ;

        bool                    operator ==                                 (   const   MultiPolygon::Impl&         aMultiPolygon                               ) const ;

        bool                    isDefined                                   ( ) const ;

        bool                    contains                                    (   const   Point&                      aPoint                                      ) const ;

        bool                    contains                                    (   const   PointSet&                   aPointSet                                   ) const ;

        Size                    getPolygonCount                             ( ) const ;

        Array<Polygon>          getPolygons                                 ( ) const ;

        MultiPolygon::Impl      unionWith                                   (   const   MultiPolygon::Impl&         aMultiPolygon                               ) const ;

        String                  toString                                    (   const   Object::Format&             aFormat,
                                                                                const   Integer&                    aPrecision                                  ) const ;

        void                    applyTransformation                         (   const   Transformation&             aTransformation                             ) ;

    private:

        typedef                 point<double, 2, cartesian>                     BoostPoint ;
        typedef                 ring<Impl::BoostPoint>                          BoostRing ;
        typedef                 polygon<Impl::BoostPoint>                       BoostPolygon ;
        typedef                 multi_polygon<Impl::BoostPolygon>               BoostMultiPolygon ;

    Impl::BoostMultiPolygon multiPolygon_ ;

        static Impl::BoostMultiPolygon BoostMultiPolygonFromPolygons        (   const   Array<Polygon>&             aPolygonArray                               ) ;

} ;

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

                                MultiPolygon::Impl::Impl                    ( )
                                :   multiPolygon_()
{

}

                                MultiPolygon::Impl::Impl                    (   const   Array<Polygon>&             aPolygonArray                               )
                                :   multiPolygon_(MultiPolygon::Impl::BoostMultiPolygonFromPolygons(aPolygonArray))
{

}

bool                            MultiPolygon::Impl::operator ==             (   const   MultiPolygon::Impl&         aMultiPolygon                               ) const
{
    return boost::geometry::equals(multiPolygon_, aMultiPolygon.multiPolygon_) ;
}

bool                            MultiPolygon::Impl::isDefined               ( ) const
{
    return !multiPolygon_.empty() ;
}

bool                            MultiPolygon::Impl::contains                (   const   Point&                      aPoint                                      ) const
{

    try
    {
        return boost::geometry::covered_by(MultiPolygon::Impl::BoostPoint(aPoint.x(), aPoint.y()), multiPolygon_) ;
    }
    catch (const std::exception& anException)
    {
        throw library::core::error::RuntimeError("Error when checking if multi-polygon contains point: [{}]", anException.what()) ;
    }

    return false ;

}

bool                            MultiPolygon::Impl::contains                (   const   PointSet&                   aPointSet                                   ) const
{

    (void) aPointSet ;

    throw library::core::error::runtime::ToBeImplemented("MultiPolygon::contains (PointSet&)") ;

    return false ;

}

Size                            MultiPolygon::Impl::getPolygonCount         ( ) const
{
    return multiPolygon_.size() ;
}

// Array<Polygon>                  MultiPolygon::Impl::getPolygons             ( ) const
// {

//     Array<Polygon> polygons = Array<Polygon>::Empty() ;

//     for (const auto& polygon : multiPolygon_)
//     {

//         Array<Point> outerRing =
//         Array<Array<Point>> innerRings =

//         const Polygon polygon = { outerRing, innerRings } ;

//         polygons.add(polygon) ;

//     }

//     return polygons ;

// }

MultiPolygon::Impl              MultiPolygon::Impl::unionWith               (   const   MultiPolygon::Impl&         aMultiPolygon                               ) const
{

    try
    {

        MultiPolygon::Impl multiPolygonUnion ;

        boost::geometry::union_(multiPolygon_, aMultiPolygon.multiPolygon_, multiPolygonUnion.multiPolygon_) ;

        return multiPolygonUnion ;

    }
    catch (const std::exception& anException)
    {
        throw library::core::error::RuntimeError("Cannot compute union of multi-polyons: [{}].", anException.what()) ;
    }

}

String                          MultiPolygon::Impl::toString                (   const   Object::Format&             aFormat,
                                                                                const   Integer&                    aPrecision                                  ) const
{

    switch (aFormat)
    {

        case Object::Format::Standard:
        case Object::Format::WKT:
        {

            std::stringstream stringStream ;

            if (aPrecision.isDefined())
            {
                stringStream << std::fixed << std::setprecision(aPrecision) << boost::geometry::wkt(multiPolygon_) ;
            }
            else
            {
                stringStream << boost::geometry::wkt(multiPolygon_) ;
            }

            return stringStream.str() ;

        }

        default:
            throw library::core::error::runtime::Wrong("Format") ;
            break ;

    }

    return String::Empty() ;

}

void                            MultiPolygon::Impl::applyTransformation     (   const   Transformation&             aTransformation                             )
{

    using library::math::obj::Matrix3d ;

    MultiPolygon::Impl::BoostMultiPolygon transformedMultiPolygon ;

    const Matrix3d transformationMatrix = aTransformation.getMatrix() ;

    const boost::geometry::strategy::transform::matrix_transformer<double, 2, 2> transform = { transformationMatrix(0, 0), transformationMatrix(0, 1), transformationMatrix(0, 2),
                                                                                transformationMatrix(1, 0), transformationMatrix(1, 1), transformationMatrix(1, 2),
                                                                                transformationMatrix(2, 0), transformationMatrix(2, 1), transformationMatrix(2, 2) } ;

    boost::geometry::transform(multiPolygon_, transformedMultiPolygon, transform) ;

    multiPolygon_ = transformedMultiPolygon ;

}

MultiPolygon::Impl::BoostMultiPolygon MultiPolygon::Impl::BoostMultiPolygonFromPolygons ( const Array<Polygon>&     aPolygonArray                               )
{

    MultiPolygon::Impl::BoostMultiPolygon boostMultiPolygon ;

    for (const auto& polygon : aPolygonArray)
    {

        MultiPolygon::Impl::BoostPolygon boostPolygon ;

        for (const auto& point : polygon.getOuterRing())
        {
            boost::geometry::append(boostPolygon, MultiPolygon::Impl::BoostPoint(point.x(), point.y())) ;
        }

        for (Index innerRingIndex = 0; innerRingIndex < polygon.getInnerRingCount(); ++innerRingIndex)
        {

            const auto innerRing = polygon.getInnerRingAt(innerRingIndex) ;

            MultiPolygon::Impl::BoostRing boostRing ;

            for (const auto& innerRingPoint : innerRing)
            {
                boost::geometry::append(boostRing, MultiPolygon::Impl::BoostPoint(innerRingPoint.x(), innerRingPoint.y())) ;
            }

            boost::geometry::correct(boostRing) ;

            boostPolygon.inners().push_back(boostRing) ;

        }

        boostMultiPolygon.push_back(boostPolygon) ;

    }

    boost::geometry::correct(boostMultiPolygon) ;

    return boostMultiPolygon ;

}

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

                                MultiPolygon::MultiPolygon                  (   const   Array<Polygon>&             aPolygonArray                               )
                                :   Object(),
                                    implUPtr_(std::make_unique<MultiPolygon::Impl>(aPolygonArray))
{

}

                                MultiPolygon::MultiPolygon                  (   const   MultiPolygon&               aMultiPolygon                               )
                                :   Object(aMultiPolygon),
                                    implUPtr_((aMultiPolygon.implUPtr_ != nullptr) ? new MultiPolygon::Impl(*aMultiPolygon.implUPtr_) : nullptr)
{

}

                                MultiPolygon::~MultiPolygon                 ( )
{

}

MultiPolygon&                   MultiPolygon::operator =                    (   const   MultiPolygon&               aMultiPolygon                               )
{

    if (this != &aMultiPolygon)
    {

        Object::operator = (aMultiPolygon) ;

        implUPtr_.reset((aMultiPolygon.implUPtr_ != nullptr) ? new MultiPolygon::Impl(*aMultiPolygon.implUPtr_) : nullptr) ;

    }

    return *this ;

}

MultiPolygon*                   MultiPolygon::clone                         ( ) const
{
    return new MultiPolygon(*this) ;
}

bool                            MultiPolygon::operator ==                   (   const   MultiPolygon&               aMultiPolygon                               ) const
{

    if ((!this->isDefined()) || (!aMultiPolygon.isDefined()))
    {
        return false ;
    }

    return (*implUPtr_) == (*aMultiPolygon.implUPtr_) ;

}

bool                            MultiPolygon::operator !=                   (   const   MultiPolygon&               aMultiPolygon                               ) const
{
    return !((*this) == aMultiPolygon) ;
}

bool                            MultiPolygon::isDefined                     ( ) const
{
    return (implUPtr_ != nullptr) && implUPtr_->isDefined() ;
}

bool                            MultiPolygon::contains                      (   const   Point&                      aPoint                                      ) const
{

    if (!aPoint.isDefined())
    {
        throw library::core::error::runtime::Undefined("Point") ;
    }

    if (!this->isDefined())
    {
        throw library::core::error::runtime::Undefined("Multi-polygon") ;
    }

    return implUPtr_->contains(aPoint) ;

}

bool                            MultiPolygon::contains                      (   const   PointSet&                   aPointSet                                   ) const
{

    if (!aPointSet.isDefined())
    {
        throw library::core::error::runtime::Undefined("Point set") ;
    }

    if (!this->isDefined())
    {
        throw library::core::error::runtime::Undefined("Multi-polygon") ;
    }

    return implUPtr_->contains(aPointSet) ;

}

Size                            MultiPolygon::getPolygonCount               ( ) const
{

    if (!this->isDefined())
    {
        throw library::core::error::runtime::Undefined("Multi-polygon") ;
    }

    return implUPtr_->getPolygonCount() ;

}

void                            MultiPolygon::print                         (           std::ostream&               anOutputStream,
                                                                                bool                        displayDecorators                           ) const
{

    displayDecorators ? library::core::utils::Print::Header(anOutputStream, "Multi-polygon") : void () ;

    if (implUPtr_ != nullptr)
    {

        // [TBI]

    }
    else
    {
        library::core::utils::Print::Line(anOutputStream) << "Undefined" ;
    }

    displayDecorators ? library::core::utils::Print::Footer(anOutputStream) : void () ;

}

MultiPolygon                    MultiPolygon::unionWith                     (   const   MultiPolygon&               aMultiPolygon                               ) const
{

    if (!aMultiPolygon.isDefined())
    {
        throw library::core::error::runtime::Undefined("Multi-polygon") ;
    }

    if (!this->isDefined())
    {
        throw library::core::error::runtime::Undefined("Multi-polygon") ;
    }

    // [TBM] Could be improved to avoid unnecessary copies

    MultiPolygon unionMultiPolygon = MultiPolygon::Undefined() ;

    unionMultiPolygon.implUPtr_ = std::make_unique<MultiPolygon::Impl>(implUPtr_->unionWith(*aMultiPolygon.implUPtr_)) ;

    return unionMultiPolygon ;

}

String                          MultiPolygon::toString                      (   const   Object::Format&             aFormat,
                                                                                const   Integer&                    aPrecision                                  ) const
{

    if (!this->isDefined())
    {
        throw library::core::error::runtime::Undefined("Multi-polygon") ;
    }

    return implUPtr_->toString(aFormat, aPrecision) ;

}

void                            MultiPolygon::applyTransformation           (   const   Transformation&             aTransformation                             )
{

    if (!aTransformation.isDefined())
    {
        throw library::core::error::runtime::Undefined("Transformation") ;
    }

    if (!this->isDefined())
    {
        throw library::core::error::runtime::Undefined("Multi-polygon") ;
    }

    implUPtr_->applyTransformation(aTransformation) ;

}

MultiPolygon                    MultiPolygon::Undefined                     ( )
{
    return { Array<Polygon>::Empty() } ;
}

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

}
}
}
}
}

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


#include "prior/world.hpp"
#include <Eigen/Core>
#include <glog/logging.h>

namespace obsidian
{
  namespace prior
  {

    uint prod(std::pair<uint, uint> in)
    {
      return in.first * in.second;
    }

    // get the number of partition elements
    uint WorldParamsPrior::size()
    {
      // Just get the size of the objects
      uint size = 0;
      uint nLayers = propertyPrior.size();
      CHECK_EQ(ctrlptPrior.size(), nLayers);
      for (uint i = 0; i < nLayers; i++)
      {
        uint sizeRock = prod(propertyPrior[i].shape);
        for (uint j = 0; j < sizeRock; j++)
          size += propMasks[i](j);
        for (uint j = 0; j < ctrlptPrior[i].shape.first; j++)
          for (uint k = 0; k < ctrlptPrior[i].shape.second; k++)
          {
            size += ctrlptMasks[i](j, k);
          }
      }
      return size;
    }

    double whiten(double x, double mu, double sigmaSq, double min, double max)
    {
      double sd = std::sqrt(sigmaSq);
      double range = max - min;
      double scale = std::min(sd, range / 5.0);
      return (x - mu) / scale;
    }

    double unwhiten(double x, double mu, double sigmaSq, double min, double max)
    {
      double sd = std::sqrt(sigmaSq);
      double range = max - min;
      double scale = std::min(sd, range / 5.0);
      return (x * scale) + mu;
    }

    Eigen::VectorXd WorldParamsPrior::thetaMinBound()
    {
      return thetaMin;
    }

    Eigen::VectorXd WorldParamsPrior::thetaMaxBound()
    {
      return thetaMax;
    }

    // used by sample... almost the same as size()
    Eigen::VectorXd WorldParamsPrior::deconstruct(const WorldParams& params)
    {
      uint nLayers = propertyPrior.size();
      CHECK_EQ(nLayers, params.rockProperties.size());
      CHECK_EQ(nLayers, params.controlPoints.size());
      std::vector<double> theta;
      for (uint i = 0; i < nLayers; i++)
      {
        uint sizeRock = prod(propertyPrior[i].shape);
        CHECK_EQ(sizeRock, params.rockProperties[i].rows());

        for (uint j = 0; j < sizeRock; j++)
        {
          if (propMasks[i](j)) // we're using this property
            theta.push_back(
                whiten(params.rockProperties[i](j), propertyPrior[i].mu[j], propertyPrior[i].sigma(j, j), propMins[i](j), propMaxs[i](j)));
        }
      }
      for (uint i = 0; i < nLayers; i++)
      {
        uint sizeCtrl = prod(ctrlptPrior[i].shape);
        CHECK_EQ(sizeCtrl, params.controlPoints[i].rows() * params.controlPoints[i].cols());
        for (uint j = 0; j < ctrlptPrior[i].shape.first; j++)
          for (uint k = 0; k < ctrlptPrior[i].shape.second; k++)
          {
            auto ej = j * ctrlptPrior[i].shape.second + k;
            //CHECK(ej>=0);
            //CHECK(ej < sizeCtrl);
            if (ctrlptMasks[i](j, k)) // we're using this control point
              theta.push_back(
                  whiten(params.controlPoints[i](j, k), ctrlptPrior[i].mu(ej), ctrlptPrior[i].sigma(ej, ej), ctrlptMins[i](j, k),
                         ctrlptMaxs[i](j, k)));
          }
      }

      // std::vector to Eigen vector
      Eigen::VectorXd vTheta(theta.size());
      for (uint i = 0; i < theta.size(); i++)
        vTheta(i) = theta[i];
      return vTheta;
    }

    // build a WorldParams object from a flat vector
    WorldParams WorldParamsPrior::reconstruct(const Eigen::VectorXd & theta)
    {
      uint nLayers = propertyPrior.size();
      CHECK_EQ(ctrlptPrior.size(), nLayers);
      std::vector<Eigen::VectorXd> propTheta(nLayers);
      std::vector<Eigen::MatrixXd> geomTheta(nLayers);
      uint count = 0;
      for (uint i = 0; i < nLayers; i++)
      {
        uint sizeRock = prod(propertyPrior[i].shape);
        Eigen::VectorXd rockValues(sizeRock);
        for (uint j = 0; j < sizeRock; j++)
        {
          if (propMasks[i](j)) // we're using this property
          {
            rockValues(j) = unwhiten(theta(count), propertyPrior[i].mu(j), propertyPrior[i].sigma(j, j), propMins[i](j), propMaxs[i](j));
            count++;
          } else
          {
            rockValues(j) = propertyPrior[i].mu(j);
          }
        }
        propTheta[i] = rockValues;
      }

      for (uint i = 0; i < nLayers; i++)
      {
        std::pair<uint, uint> layerShape = ctrlptPrior[i].shape;
        Eigen::MatrixXd geomValues(layerShape.first, layerShape.second);
        for (uint j = 0; j < layerShape.first; j++)
          for (uint k = 0; k < layerShape.second; k++)
          {
            auto ej = j * ctrlptPrior[i].shape.second + k;
            if (ctrlptMasks[i](j, k)) // we're using this control point
            {
              geomValues(j, k) = unwhiten(theta(count), ctrlptPrior[i].mu(ej), ctrlptPrior[i].sigma(ej, ej), ctrlptMins[i](j, k),
                                          ctrlptMaxs[i](j, k));
              count++;
            } else
            {
              // TODO check I'm indexing correctly here
              geomValues(j, k) = ctrlptPrior[i].mu(j * layerShape.second + k);
            }
          }
        geomTheta[i] = geomValues;
      }

      WorldParams wParams = { propTheta, geomTheta };
      return wParams;
    }

    Eigen::VectorXd WorldParamsPrior::sample(std::mt19937 &gen)
    {
      std::vector<bool> uniformFlags;
      for (auto i : classes)
        uniformFlags.push_back(i == BoundaryClass::Warped);
      WorldParams theta = { distrib::drawVectorFrom(propertyPrior, gen, propMins, propMaxs), distrib::drawFrom(ctrlptPrior, gen, ctrlptMins,
                                                                                ctrlptMaxs, uniformFlags) };
      return deconstruct(theta);
    }

    // Evaluate log likelihood of theta under this prior
    double WorldParamsPrior::evaluatePDF(const Eigen::VectorXd& theta)
    {

      uint nLayers = propertyPrior.size();
      CHECK_EQ(ctrlptPrior.size(), nLayers);
      // Rebuild the vector into an object
      WorldParams wParams = reconstruct(theta);

      // Now we go and check each of these against their prior
      double logPDF = 0.0;
      for (uint i = 0; i < nLayers; i++)
      {
        logPDF += distrib::logPDF(wParams.rockProperties[i], propertyPrior[i], propMins[i], propMaxs[i]);
        bool isUniform = classes[i] == BoundaryClass::Warped;
        if (isUniform)
        {
          logPDF += distrib::uniformLogPDF(wParams.controlPoints[i], ctrlptPrior[i], ctrlptMins[i], ctrlptMaxs[i]);
        } else
        {
          logPDF += distrib::logPDF(wParams.controlPoints[i], ctrlptPrior[i], ctrlptMins[i], ctrlptMaxs[i]);
        }
      }
      return logPDF;
    }

  }
} // namespace obsidian


#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_ros/transforms.h>
#include <pcl_ros/point_cloud.h>
#include <boost/asio.hpp>
#include <std_msgs/UInt16.h>
#include <cyglidar_pcl.h>
#include <cmath>
#include <thread>

#define PAYLOAD_SIZE            6

#define DATABUFFER_SIZE_2D      249
#define DATASET_SIZE_2D         121
#define BASE_DEPTH_3D           3000
#define BASE_DEPTH_2D           16000
#define BASE_ANGLE_2D           120

#define DATABUFFER_SIZE_3D      14407
#define DATASET_SIZE_3D         9600

#define COLOR_MIN               0
#define COLOR_MAX               255

#define RIGHT_ANGLE             90
#define HALF_ANGLE              180
#define MATH_PI                 3.14159265

#define DIVISOR                 0.001
#define FOCAL_LENGTH            40.5

const float RADIAN = MATH_PI / HALF_ANGLE;

typedef pcl::PointCloud<pcl::PointXYZRGBA> PointXYZRGBA;
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr scan_2D;
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr scan_3D;
tf::TransformListener *tf_listener;

ros::Publisher pub;
int version_num;
bool version_changed;

uint8_t *cloudBuffer = new uint8_t[DATABUFFER_SIZE_3D - PAYLOAD_SIZE];
float *cloudSetBuffer = new float[DATASET_SIZE_3D];
uint8_t *cloudBuffer_2D = new uint8_t[DATABUFFER_SIZE_2D - PAYLOAD_SIZE];
float *cloudSetBuffer_2D = new float[DATASET_SIZE_2D];

uint8_t* bufferPtr_2D;
uint8_t* bufferPtr01;
uint8_t* bufferPtr02;

uint8_t FIRST, SECOND, THIRD;
uint16_t LSB, MSB;

float centerX, centerY, width, height;
float actualX = 0.0, actualY = 0.0;

float angleRadian = RADIAN * -RIGHT_ANGLE;

float data1, data2, actualDistance, length;
uint8_t currentBuffer;

static std::vector<uint32_t> colorArray;
uint32_t rgb_setup;
uint8_t r_setup, g_setup, b_setup;
void colorBuffer()
{
    int colors = 0;
    r_setup = 255;
    g_setup = 0;
    b_setup = 0;

    for (int colors = 0; colors < 2; colors++)
    {
        for (int i = 0; i < 3; i++)
        {
            for (int colorCnt = 0; colorCnt < COLOR_MAX + 1; colorCnt++)
            {
                switch (colors)
                {
                    // RED -> YELLOW
                    case 0:
                        g_setup++;
                        break;
                    // YELLOW -> BLUE
                    case 1:
                        r_setup--;
                        g_setup--;
                        b_setup++;
                        break;
                    // BLUE -> RED
                    case 2:
                        r_setup++;
                        b_setup--;
                        break;
                }

                rgb_setup = ((uint32_t)r_setup << 16 | (uint32_t)g_setup << 8 | (uint32_t)b_setup);
                colorArray.push_back(rgb_setup);

                if (colorCnt == COLOR_MAX)
                {
                    colors++;
                }
            }
        }
    }

    ROS_INFO("COLORS STORED (%d)", colorArray.size());
}

bool drawing = false;
int distanceCnt_2D;
float angleStep_2D, point_angle_var_2D, point_angle_2D;
float tempX_2D, tempY_2D;
uint8_t cloudScatter_2D()
{
    if (!drawing)
    {
        drawing = true;
        memcpy(cloudBuffer_2D, &bufferPtr_2D[PAYLOAD_SIZE], sizeof(uint8_t) * DATABUFFER_SIZE_2D);

        distanceCnt_2D = 0;
        angleStep_2D = 0;

        for (int dataLength = 0; dataLength < (DATABUFFER_SIZE_2D - PAYLOAD_SIZE) - 1; dataLength+=2)
        {
            LSB = cloudBuffer_2D[dataLength];
            MSB = cloudBuffer_2D[dataLength + 1];

            data1 = (float)((MSB << 8) | LSB);

            cloudSetBuffer_2D[distanceCnt_2D++] = data1;
        }

        point_angle_var_2D = 0.0;
        angleStep_2D = ((float)BASE_ANGLE_2D / (float)DATASET_SIZE_2D);

        for (int idx = 0; idx < DATASET_SIZE_2D; idx++)
        {
            point_angle_2D = (float)(((BASE_ANGLE_2D / 2 * -1) + point_angle_var_2D) * RADIAN);
            point_angle_var_2D += angleStep_2D;

            actualDistance = (cloudSetBuffer_2D[idx]);
            actualX = (sin(point_angle_2D) * actualDistance);
            actualY = (cos(point_angle_2D) * actualDistance); // depth

            tempX_2D = actualX;
            tempY_2D = actualY;
            actualX = (tempX_2D * cos(angleRadian)) + (tempY_2D * -sin(angleRadian));
            actualY = (tempX_2D * sin(angleRadian)) + (tempY_2D * cos(angleRadian));

            scan_2D.get()->points[idx].x = actualX * DIVISOR;
            scan_2D.get()->points[idx].y = actualY * DIVISOR;
            scan_2D.get()->points[idx].z = 0.0;

            if (cloudSetBuffer_2D[idx] < (float)BASE_DEPTH_2D)
            {
                scan_2D.get()->points[idx].r = 255;
                scan_2D.get()->points[idx].g = 255;
                scan_2D.get()->points[idx].b = 0;
                scan_2D.get()->points[idx].a = 255;
            }
            else
            {
                scan_2D.get()->points[idx].a = 0;
            }
        }

        drawing = false;
    }
}

int distanceCnt_3D, index_3D;
float tempX_3D, tempY_3D;
float yRecovery;
float x_3D, y_3D, h_3D, fh_3D, dRatio_3D, focalRatio_3D;
uint32_t rgb_3D;
uint8_t cloudScatter_3D()
{
    if (!drawing)
    {
        drawing = true;

        switch (currentBuffer)
        {
            case 0x00:
                memcpy(cloudBuffer, &bufferPtr02[PAYLOAD_SIZE], sizeof(uint8_t) * DATABUFFER_SIZE_3D);
                break;
            case 0x01:
                memcpy(cloudBuffer, &bufferPtr01[PAYLOAD_SIZE], sizeof(uint8_t) * DATABUFFER_SIZE_3D);
                break;
        }

        distanceCnt_3D = 0;
        for (int dataLength = 0; dataLength < (DATABUFFER_SIZE_3D - PAYLOAD_SIZE) - 1; dataLength+=3)
        {
            FIRST = cloudBuffer[dataLength];
            SECOND = cloudBuffer[dataLength + 1];
            THIRD = cloudBuffer[dataLength + 2];

            data1 = (float)((FIRST << 4) | (SECOND >> 4));
            data2 = (float)(((SECOND & 0xf) << 8) | THIRD);

            cloudSetBuffer[distanceCnt_3D++] = data1;
            cloudSetBuffer[distanceCnt_3D++] = data2;
        }

        index_3D = 0;
        for (int yy = 0; yy < height; yy++)
        {
            for (int xx = 0; xx < width; xx++)
            {
                index_3D = (xx + (yy * width));

                x_3D = (xx - (width / 2));
                y_3D = ((height - yy) - (height / 2));

                h_3D = sqrt(pow(x_3D, 2) + pow(y_3D, 2));
                fh_3D = sqrt(pow(h_3D, 2) + pow(FOCAL_LENGTH, 2));
                dRatio_3D = cloudSetBuffer[index_3D] / fh_3D;

                actualDistance = (FOCAL_LENGTH * dRatio_3D);
                focalRatio_3D = actualDistance / FOCAL_LENGTH;

                actualX = (x_3D * focalRatio_3D);
                actualY = (y_3D * focalRatio_3D);

                if (cloudSetBuffer[index_3D] < (float)BASE_DEPTH_3D)
                {
                    rgb_3D = colorArray[(int)actualDistance % colorArray.size()];
                    scan_3D.get()->points[index_3D].rgb = *reinterpret_cast<float*>(&rgb_3D);
                    scan_3D.get()->points[index_3D].a = 255;

                    tempX_3D = actualX;
                    tempY_3D = actualDistance;
                    actualX = (tempX_3D * cos(angleRadian)) + (tempY_3D * -sin(angleRadian));
                    actualDistance = (tempX_3D * sin(angleRadian)) + (tempY_3D * cos(angleRadian));

                    scan_3D.get()->points[index_3D].x = actualX * DIVISOR;
                    scan_3D.get()->points[index_3D].y = actualDistance * DIVISOR;
                    scan_3D.get()->points[index_3D].z = (actualY + 180) * DIVISOR;
                }
                else
                {
                    scan_3D.get()->points[index_3D].x = 0.0;
                    scan_3D.get()->points[index_3D].y = 0.0;
                    scan_3D.get()->points[index_3D].z = 0.0;
                    scan_3D.get()->points[index_3D].r = 0;
                    scan_3D.get()->points[index_3D].g = 0;
                    scan_3D.get()->points[index_3D].b = 0;
                    scan_3D.get()->points[index_3D].a = 0;
                }
            }
        }

        drawing = false;
    }
}

void running()
{
    ros::NodeHandle nh;
    ros::NodeHandle priv_nh("~");

    std::string port;
    int baud_rate;
    std::string frame_id;

    priv_nh.param("port", port, std::string("/dev/ttyUSB0"));
    priv_nh.param("baud_rate", baud_rate, 3000000);
    priv_nh.param("version_num", version_num, 0);
    priv_nh.param("frame_id", frame_id, std::string("laser_link"));

    ROS_INFO("READY");

    colorBuffer();

    pub = nh.advertise<sensor_msgs::PointCloud2>("scan", 1);
    scan_2D = pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>);
    scan_3D = pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>);

    // PointCloud2 for 2D
    scan_2D.get()->is_dense = false;
    scan_2D.get()->header.frame_id = frame_id;

    scan_2D.get()->points.resize(DATABUFFER_SIZE_2D);
    for (int point = 0; point < DATABUFFER_SIZE_2D; point++)
    {
        scan_2D.get()->points[point].a = 0;
    }

    // PointCloud2 for 3D
    scan_3D.get()->width = 160;
    scan_3D.get()->height = 60;
    scan_3D.get()->is_dense = false;
    scan_3D.get()->header.frame_id = frame_id;

    width = (float)(scan_3D.get()->width);
    height = (float)(scan_3D.get()->height);
    centerX = width / 2.0;
    centerY = height / 2.0;

    scan_3D.get()->points.resize(DATABUFFER_SIZE_3D);
    for (int point = 0; point < DATABUFFER_SIZE_3D; point++)
    {
        scan_3D.get()->points[point].a = 0;
    }

    actualDistance = 0.0;

    length = 0.0;

    drawing = false;
    currentBuffer = 0x00;

    version_changed = false;
    boost::asio::io_service io;
    try
    {
        cyglidar_pcl_driver::cyglidar_pcl laser(port, baud_rate, io);
        laser.packet(version_num);

        while (ros::ok())
        {
            uint8_t* result = laser.poll(version_num);
            if (result[0] == 0x5A && result[1] == 0x77 && result[2] == 0xFF)
            {
                switch (result[5])
                {
                    case 0x01: // 2D
                        bufferPtr_2D = &result[0];
                        cloudScatter_2D();

                        pcl_conversions::toPCL(ros::Time::now(), scan_2D->header.stamp);
                        pub.publish(scan_2D);
                        break;
                    case 0x08: // 3D
                        switch (currentBuffer)
                        {
                            case 0x00:
                                bufferPtr01 = &result[0];
                                currentBuffer = 0x01;
                                break;
                            case 0x01:
                                bufferPtr02 = &result[0];
                                currentBuffer = 0x00;
                                break;
                        }
                        cloudScatter_3D();

                        pcl_conversions::toPCL(ros::Time::now(), scan_3D->header.stamp);
                        pub.publish(scan_3D);
                        break;
                }
            }
        }
        laser.close();
    }
    catch (boost::system::system_error ex)
    {
        ROS_ERROR("[Error] instantiating laser object. \
        Are you sure you have the correct port and baud rate? \
        Error was %s", ex.what());
    }
}

int main(int argc, char **argv)
{
    // Initialize ROS
    ros::init(argc, argv, "line_laser");

    //std::thread first(getVersion);
    std::thread second(running);

    //first.join();
    second.join();

    return 0;
}


#include <algorithm>
#include <cmath>
#include <Eigen/Core>
#include <iostream>
#include <unsupported/Eigen/Polynomials>

#include "PWvlet.h"
#include "util.h"

using namespace Eigen;
using namespace std;

/*initialise static members*/
map<complexKey, MatrixXd> PWvlet::G;
map<complexKey, MatrixXd> PWvlet::P;
map<int, MatrixXd> PWvlet::H;
Matrix<long long, Dynamic, Dynamic> PWvlet::W;
map<int, Matrix<long long, Dynamic, Dynamic>> PWvlet::Hi;

map<int, MatrixXd> PWvlet::fTrans = map<int, MatrixXd>();

const set<complexKey> PWvlet::valid = { {4, 0, 0},
	{5, 0, 0}, {5, 0, 1},
	{6, 0, 0}, {6, 0, 1}, {6, 0, 2}, {6, 1, 1},
	{7, 0, 0}, {7, 0, 1}, {7, 0, 2}, {7, 0, 3}, {7, 1, 1}, {7, 1, 2},
	{8, 0, 0}, {8, 0, 1}, {8, 0, 2}, {8, 0, 3}, {8, 1, 1}, {8, 1, 2}, {8, 1, 3}, {8, 2, 2},
	{9, 0, 0}, {9, 0, 1}, {9, 0, 2}, {9, 0, 3}, {9, 1, 1}, {9, 1, 2}, {9, 1, 3}, {9, 2, 2},
	{10, 0, 0}, {10, 0, 1}, {10, 0, 2}, {10, 0, 3}, {10, 1, 1}, {10, 1, 2}, {10, 1, 3} };/*,
	//need to look at evaluation accuracy for further values (e.g., use KahanSum to re-evaluate the numerator and
	//	denominator of the GoF when this is greater than 1, recalculate a in the same way under these circumstances
	{11, 0, 0}, {11, 0, 1}, {11, 0, 2}, {11, 1, 1}, {11, 1, 2},
	{12, 0, 0}, {12, 0, 1}, {12, 0, 2}, {12, 1, 1}, {12, 1, 2},
	{13, 0, 0}, {13, 0, 1}, {13, 0, 2}, {13, 1, 1}, {13, 1, 2},
	{14, 0, 0}, {14, 0, 1}, {14, 0, 2}, {14, 1, 1}, {14, 1, 2},
	{15, 0, 0}, {15, 0, 1}, {15, 0, 2}, {15, 1, 1}, {15, 1, 2},
	{16, 0, 0}, {16, 0, 1}, {16, 0, 2}, {16, 1, 1}, {16, 1, 2} , {16, 1, 3} };*/

PWvlet::PWvlet(int n1, int cNo1, int wCNo1) :
	wvlet(n1, cNo1, wCNo1)
{
	if (n1 - wCNo - cNo - 1 < 3)
	{
		cerr << "Fewer than 3 degrees of freedom after conditions\n";
		exit(-1);
	}
	complexKey cK = { n, cNo, wCNo };
	if (!valid.count(cK))
	{
		cerr << "Numerical problems with this combination of parameters\n";
		exit(-1);
	}
	makeH(n);
	makeW(n, wCNo);
	makeP(n, cNo, wCNo);
}

PWvlet::PWvlet(dataWin1 dW, int n1, int cNo1, int wCNo1) :
	wvlet(dW, n1, cNo1, wCNo1)
{
	if (n1 - wCNo - cNo - 1 < 3)
	{
		cerr << "Fewer than 3 degrees of freedom after conditions\n";
		exit(-1);
	}
	complexKey cK = { n, cNo, wCNo };
	if (!valid.count(cK))
	{
		cerr << "Numerical problems with this combination of parameters\n";
		exit(-1);
	}
	makeH(n);
	makeW(n, wCNo);
	makeP(n, cNo, wCNo);
	int WLen = dW.getLen();
	maintainFTrans(n, WLen);
	MatrixXd Hi1 = Hi[n].cast<double>();
	MatrixXd K = fTrans[WLen];
	K.conservativeResize((UINT64)n + 1ULL, WLen);
	para = P[cK] * (Hi1 * (K * dW.Vec));
}

void PWvlet::populate(const dataWin1& dW)
{
	if (n - wCNo - cNo - 1 < 3)
	{
		cerr << "Fewer than 3 degrees of freedom after conditions\n";
		exit(-1);
	}
	complexKey cK = { n, cNo, wCNo };
	if (!valid.count(cK))
	{
		cerr << "Numerical problems with this combination of parameters\n";
		exit(-1);
	}
	makeH(n);
	makeW(n, wCNo);
	makeP(n, cNo, wCNo);
	int WLen= dW.getLen();
	maintainFTrans(n, WLen);
	MatrixXd K = fTrans[dW.getLen()];
	K.conservativeResize(n + 1ULL, WLen);
	para = P[cK] * (Hi[n].cast<double>() * (K * dW.Vec));
}

/*make static members*/
void PWvlet::makeH(int n1, bool all)
{
	all = true;
	if (n1 > 16)
	{
		cerr << "Maximum size of n before long long wraps in calculation of H^{-1} is 16\n";
		exit(-1);
	}

	/*possibilities are:	all = false:
								H[0] incomplete - complete and return
								H[0] complete - return
							all = true
								H[0] incomplete (=> H[0+] incomplete) - complete H[0] and H[0+] and return
								H[0] complete, H[0+] incomplete - complete H[0+] and return
								H[0+] complete (=> H[0] complete) - return
	Be aware that referring to H[j] in code calls it into existence! - e.g.
																			int i = H.size();
																			int j = H[0].cols();
																			int k = H.size();
	results in k > 0, even if i== 0 - expected second line would throw an error if i== 0.
	Possible source of problems if code transported to different compiler, so code as if it would throw an error*/

	long long Hn = 0;
	if (H.size())
		Hn = H[0].cols();
	if ((Hn > n1) && !all)
		return;
	long long i, j, k, ell, s;
	if (Hn <= n1)
	{
		if (Hn == 0)
		{
			H[0].resize(n1 + 1LL, n1 + 1LL);
			for (i = 0; i <= n1; i++)
				for (j = 0; j <= n1; j++)
					H[0](i, j) = 1.0 / (1.0 + i + j);
		}
		else
		{
			H[0].conservativeResize(n1 + 1LL, n1 + 1LL);
			for (i = 0; i <= n1; i++)
				for (j = (i < Hn) ? Hn : 0; j <= n1; j++)
					H[0](i, j) = 1.0 / (1.0 + i + j);
		}
		Matrix<long long, Dynamic, Dynamic> Hitmp(n1 + 1, n1 + 1);
		Hitmp.setZero();
		Hitmp(0, 1) = -(n1 * (n1 + 1) * (n1 + 1) * (n1 + 2)) / 2;
		Hitmp(1, 0) = Hitmp(0, 1);
		long long nfac, dfac, g;
		for (i = 0; i < n1; i++)
		{
			if (i > 0)
			{
				nfac = (n1 + 1LL + i) * (n1 + 2LL + i) * ((long long)n1 - i) * (n1 + 1LL - i);
				dfac = i * (i + 1LL) * (i + 1LL) * (i + 1LL);
				g = gcd(nfac, dfac);
				nfac /= g;
				dfac /= g;
				Hitmp(i, i + 1LL) = nfac * (Hitmp(i - 1LL, i) / dfac);
				Hitmp(i + 1LL, i) = Hitmp(i, i + 1LL);
			}
			for (j = i + 2; j <= n1; j++)
			{
				nfac = (i + j) * (n1 + 1LL - j) * (n1 + 1LL + j);
				dfac = j * j * (i + j + 1LL);
				g = gcd(nfac, dfac);
				nfac /= g;
				dfac /= g;
				Hitmp(i, j) = -nfac * (Hitmp(i, j - 1LL) / dfac);
				Hitmp(j, i) = Hitmp(i, j);
			}
		}
		Hitmp(0, 0) = (n1 + 1LL) * (n1 + 1LL);
		for (i = 1; i < n1 + 1LL; i++)
		{
			nfac = (2LL * i - 1LL) * (n1 + 1LL - i) * (n1 + 1LL + i) * (n1 + 1LL - i) * (n1 + 1LL + i);
			dfac = (2LL * i + 1LL) * i * i * i * i;
			g = gcd(nfac, dfac);
			nfac /= g;
			dfac /= g;
			Hitmp(i, i) = nfac * (Hitmp(i - 1LL, i - 1LL) / dfac);
		}
		Hi[n1] = Hitmp;
	}
	if (!all || H.size()== (2 * n1 + 1))
		return;
	double G0 = 1.0/ (n1 + 1.0);
	for (s = 1; s<= (2 * n1); s++)
	{
		MatrixXd F = MatrixXd::Zero(n1 + 1, n1 + 1);
		MatrixXd G = MatrixXd::Zero(n1 + 1, n1 + 1);
		if (s <= n1)
			for (k = 1; k <= n1; k++)
			{
				F(k, s) = (s % 2 ? -1 : 1) / (k + 1.0);
				for (ell = s + 1; ell <= n1; ell++)
					F(k, ell) = ell * (k + ell - s) * F(k, ell - 1) / ((ell - s) * (k + ell + 1.0 - s));
			}
		if ((s > 1) && (s<= n1 + 1))
		{
			G(s - 1, 1) = 1.0/s;
			for (ell = 2; ell < s - 1; ell++)
			{
				G(s - ell, ell) = -ell * G(s + 1 - ell, ell - 1)/(s + 1.0 - ell);
				for (k = s - ell + 1; k <= s - 1; k++)
					G(k, ell) = -k * G(k - 1, ell) /(k + ell + 1.0 - s);
			}
			G(1, s - 1) = (s % 2 ? -1 : 1)/ ((double)s);
			for (k = 2; k <= s - 1; k++)
				G(k, s - 1) = -G(k - 1, s - 1);
			for (ell = s; ell <= n1; ell++)
			{
				G(1, ell) = ell * G(1, ell - 1)/(ell + 2.0 - s);
				for (k = 2; k <= s - 1; k++)
					G(k, ell) = -k * G(k - 1, ell) / (k + ell + 1.0 - s);
			}
		}
		else
			if (s > 1)
			{
				G0 *= -(s - n1) / ((double)s);
				G(n1, s - n1) = G0;
				for (ell = s - n1 + 1; ell<= n1; ell++)
				{
					G(s - ell, ell) = -(ell * G(s - ell + 1, ell - 1))/(s + 1.0 - ell);
					for (k = s - ell + 1; k <= n1; k++)
						G(k, ell) = -(k * G(k - 1, ell))/(k + ell + 1.0 - s);
				}
			}

		H[s] = G + F;
	}
}

void PWvlet::makeW(int n1, int w1)
{
	long long nW = W.cols(), wRows = W.rows();
	if ((nW > n1) && (wRows>= w1))
		return;
	long long i, j;
	if (nW == 0)
	{
		W.setZero(w1, n1 + 1ULL);
		for (i = 0; i < w1; i++)
		{
			if (i == 0)
				W(0, 1) = 1;
			else
				W(i, i + 1LL) = (i + 1LL) * W(i - 1LL, i);
			for (j = i + 2; j < n1 + 1LL; j++)
				W(i, j) = j * W(i, j - 1LL)/(j - i);
		}
		return;
	}
	if (nW < n1 + 1LL)
	{
		//extend rows of W
		W.conservativeResize(wRows, n1 + 1ULL);
		W.block(0, nW, wRows, n1 + 1LL - nW) = Matrix<long long, Dynamic, Dynamic>::Zero(wRows, (n1 - nW) + 1ULL);
		for (i = 0; i < wRows; i++)
			for (j = nW; j < n1 + 1LL; j++)
				W(i, j) = j * W(i, j - 1LL) / (j - i);
	}
	if (wRows < w1)
	{
		W.conservativeResize(w1, n1 + 1ULL);
		W.block(wRows, 0, w1 - wRows, n1 + 1) =
			Matrix<long long, Dynamic, Dynamic>::Zero(w1 - wRows, n1 + 1ULL);
		for (i = wRows; i < w1; i++)
		{
			W(i, i) = 1;
			for (j = i + 1; j < n1 + 1LL; j++)
				W(i, j) = j * W(i, j - 1ULL) / (j - i);
		}
	}
}

void PWvlet::makeG(int n1, int c1, int w1)
{
	makeH(n1);
	makeW(n1, w1);
	complexKey cK = { n1, c1, w1 };
	if (G.count(cK))
		return;

	MatrixXd Hinv = Hi[n1].cast<double>();

	if ((c1 == 0) && (w1 == 0))
	{
		MatrixXd tmp = Hinv;
		tmp(0, 0) -= 1.0;
		G[cK] = tmp;
		return;
	}

	int i;
	Matrix<double, Dynamic, Dynamic> B =
		Matrix<double, Dynamic, Dynamic>::Zero((UINT64)c1 + (UINT64)w1 + 1ULL, n1 + 1ULL);
	long long j0 = Hi[n1](0, 0);
	Matrix<double, Dynamic, Dynamic> HiB = Matrix<double, Dynamic, Dynamic>::Zero(n1 + 1LL, (1LL + w1) + c1);
	Matrix<double, Dynamic, Dynamic> BHiBi = Matrix<double, Dynamic, Dynamic>::Zero((1LL + w1) + c1, (1LL + w1) + c1);
	if (c1 == 0)
	{
		Matrix<long long, Dynamic, Dynamic> Wtilde = Matrix<long long, Dynamic, Dynamic>::Zero(w1, n1);
		Matrix<long long, Dynamic, Dynamic> J = (Hi[n1]).block(1, 1, n1, n1);
		Vector<long long, Dynamic> j = (Hi[n1]).block(1, 0, n1, 1);
		Wtilde.block(0, 0, w1, n1) = W.block(0, 1, w1, n1);
		for (i = 0; i < w1; i++)
		{
			Vector<long long, Dynamic> tmp = Wtilde.row(i).transpose();
			remfac(tmp);
			Wtilde.row(i) = tmp.transpose();
		}
		Matrix<long long, Dynamic, Dynamic> HiB_LL = Matrix<long long, Dynamic, Dynamic>::Zero(n1 + 1LL, w1 + 1LL);
		HiB_LL(0, 0) = 1;
		HiB_LL.block(1, 1, n1, w1) = (Wtilde * J).transpose();
		HiB_LL.block(0, 1, 1, w1) = (Wtilde * j).transpose();
		Matrix<long long, Dynamic, Dynamic> tmp = Wtilde * J;
		BHiBi(0, 0) = 1.0;
		Matrix<double, Dynamic, Dynamic> Jtilde = (Wtilde * J * (Wtilde.transpose())).cast<double>();
		BHiBi.block(1, 1, w1, w1) = Jtilde.inverse();
		HiB = HiB_LL.cast<double>();
	}
	else
		if (c1 == 1)
		{
			Matrix<long long, Dynamic, Dynamic> Wtilde = Matrix<long long, Dynamic, Dynamic>::Zero(w1, n1);
			Matrix<long long, Dynamic, Dynamic> J = (Hi[n1]).block(1, 1, n1, n1);
			Vector<long long, Dynamic> j = (Hi[n1]).block(1, 0, n1, 1);
			Wtilde.block(0, 0, w1, n1) = W.block(0, 1, w1, n1);
			for (i = 0; i < w1; i++)
			{
				Vector<long long, Dynamic> tmp = Wtilde.row(i).transpose();
				remfac(tmp);
				Wtilde.row(i) = tmp.transpose();
			}
			Matrix<long long, Dynamic, Dynamic> HiB_LL = Matrix<long long, Dynamic, Dynamic>::Zero(n1 + 1LL, w1 + 2LL);
			HiB_LL(0, 0) = 1;
			HiB_LL(0, 1) = j0;
			HiB_LL.block(1, 1, n1, 1) = j;
			HiB_LL.block(0, 2, 1, c1 + w1 - 1) = (Wtilde * j).transpose();
			HiB_LL.block(1, 2, n1, c1 + w1 - 1) = (Wtilde * J).transpose();
			Matrix<double, Dynamic, Dynamic> K = (Wtilde * J * Wtilde.transpose()).cast<double>().inverse();
			Vector<double, Dynamic> k = (Wtilde * j).cast<double>();
			double k0 = j0 - (k.transpose()) * (K * k);
			k = K * k;
			BHiBi(0, 0) = k0;
			BHiBi(0, 1) = -1.0;
			BHiBi(1, 0) = -1.0;
			BHiBi(1, 1) = 1;
			BHiBi.block(0, 2, 1, w1) = k.transpose();
			BHiBi.block(2, 0, w1, 1) = k;
			BHiBi.block(1, 2, 1, w1) = -k.transpose();
			BHiBi.block(2, 1, w1, 1) = -k;
			BHiBi.block(2, 2, w1, w1) = k * k.transpose();
			BHiBi /= k0 - 1;
			BHiBi.block(2, 2, w1, w1) += K;

			HiB = HiB_LL.cast<double>();
		}
		else
		{
			Matrix<long long, Dynamic, Dynamic> Wtilde = Matrix<long long, Dynamic, Dynamic>::Zero(w1, n1 + 1LL - c1);
			Vector<long long, Dynamic> j1 = (Hi[n1]).block(1, 0, c1 - 1, 1),
				j2 = (Hi[n1]).block(c1, 0, n1 + 1 - c1, 1);
			Matrix<long long, Dynamic, Dynamic> J11 = (Hi[n1]).block(1, 1, c1 - 1, c1 - 1),
				J21 = (Hi[n1]).block(c1, 1, n1 + 1 - c1, c1 - 1),
				J22 = (Hi[n1]).block(c1, c1, n1 + 1 - c1, n1 + 1 - c1);
			Wtilde.block(0, 0, w1, n1 + 1 - c1) = W.block(0, c1, w1, n1 + 1 - c1);
			for (i = 0; i < w1; i++)
			{
				Vector<long long, Dynamic> tmp = Wtilde.row(i).transpose();
				remfac(tmp);
				Wtilde.row(i) = tmp.transpose();
			}
			Matrix<long long, Dynamic, Dynamic> HiB_LL =
				Matrix<long long, Dynamic, Dynamic>::Zero(n1 + 1LL, (c1 + 1LL) + w1);

			HiB_LL(0, 0) = 1;
			HiB_LL(0, 1) = j0;
			HiB_LL.block(0, 2, 1, c1 - 1) = j1.transpose();
			HiB_LL.block(0, c1 + 1LL, 1, w1) = (Wtilde * j2).transpose();

			HiB_LL.block(1, 1, c1 - 1, 1) = j1;
			HiB_LL.block(1, 2, c1 - 1LL, c1 - 1LL) = J11;
			HiB_LL.block(1, c1 + 1LL, c1 - 1LL, w1) = (Wtilde * J21).transpose();

			HiB_LL.block(c1, 1, n1 - c1 + 1, 1) = j2;
			HiB_LL.block(c1, 2, n1 - c1 + 1, c1 - 1) = J21;
			HiB_LL.block(c1, c1 + 1, n1 + 1 - c1, w1) = J22 * Wtilde.transpose();

			Matrix<long long, Dynamic, Dynamic> J_LL(w1 + c1 - 1, w1 + c1 - 1);
			Vector<long long, Dynamic> j_LL(w1 + (c1 - 1LL));
			J_LL.block(0, 0, c1 - 1LL, c1 - 1LL) = J11;
			J_LL.block(c1 - 1LL, 0, w1, c1 - 1LL) = Wtilde * J21;
			J_LL.block(0, c1 - 1LL, c1 - 1LL, w1) = J_LL.block(c1 - 1LL, 0, w1, c1 - 1).transpose();
			J_LL.block(c1 - 1LL, c1 - 1LL, w1, w1) = (Wtilde * J22) * Wtilde.transpose();
			j_LL.head(c1 - 1LL) = j1;
			j_LL.tail(w1) = Wtilde * j2;
			Matrix<double, Dynamic, Dynamic> K = J_LL.cast<double>().inverse();
			Vector<double, Dynamic> j = j_LL.cast<double>();
			Vector<double, Dynamic> k = K * j;
			double k0 = j0 - j.transpose() * k;
			BHiBi(0, 0) = k0;
			BHiBi(0, 1) = -1.0;
			BHiBi(1, 0) = -1.0;
			BHiBi(1, 1) = 1;
			BHiBi.block(0, 2, 1, w1 + c1 - 1) = k.transpose();
			BHiBi.block(2, 0, w1 + c1 - 1, 1) = k;
			BHiBi.block(1, 2, 1, w1 + c1 - 1) = -k.transpose();
			BHiBi.block(2, 1, w1 + c1 - 1, 1) = -k;
			BHiBi.block(2, 2, w1 + c1 - 1, w1 + c1 - 1) = k * k.transpose();
			BHiBi /= k0 - 1;
			BHiBi.block(2, 2, w1 + c1 - 1, w1 + c1 - 1) += K;

			HiB = HiB_LL.cast<double>();
		}
	MatrixXd G0 = Hinv - HiB * (BHiBi * (HiB.transpose())), G1 = G0.transpose();
	G[cK] = (G0 + G1) / 2;
}

void PWvlet::makeP(int n1, int c1, int w1)
{
	makeH(n1);
	makeW(n1, w1);
	complexKey cK = { n1, c1, w1 };
	if (P.count(cK))
		return;

	int i;
	if ((c1 == 0) && (w1 == 0))
	{
		MatrixXd tmp = MatrixXd::Identity(n1 + 1, n1 + 1);
		for (i = 0; i < n1 + 1; i++)
			tmp(0, i) -= H[0](0, i);
		P[cK] = tmp;
		return;
	}

	MatrixXd Hinv = Hi[n1].cast<double>();

	Matrix<double, Dynamic, Dynamic> B =
		Matrix<double, Dynamic, Dynamic>::Zero((UINT64)c1 + (UINT64)w1 + 1ULL, n1 + 1ULL);
	for (i = 0; i < n1 + 1; i++)
		B(0, i) = 1.0 / (i + 1.0);
	long long j0 = Hi[n1](0, 0);
	Matrix<double, Dynamic, Dynamic> HiB = Matrix<double, Dynamic, Dynamic>::Zero(n1 + 1LL, (1LL + w1) + c1);
	Matrix<double, Dynamic, Dynamic> BHiBi = Matrix<double, Dynamic, Dynamic>::Zero((1LL + w1) + c1, (1LL + w1) + c1);
	if (c1 == 0)
	{
		Matrix<long long, Dynamic, Dynamic> Wtilde = Matrix<long long, Dynamic, Dynamic>::Zero(w1, n1);
		Matrix<long long, Dynamic, Dynamic> J = (Hi[n1]).block(1, 1, n1, n1);
		Vector<long long, Dynamic> j = (Hi[n1]).block(1, 0, n1, 1);
		Wtilde.block(0, 0, w1, n1) = W.block(0, 1, w1, n1);
		for (i = 0; i < w1; i++)
		{
			Vector<long long, Dynamic> tmp = Wtilde.row(i).transpose();
			remfac(tmp);
			Wtilde.row(i) = tmp.transpose();
		}
		B.block(1, 1, w1, n1) = Wtilde.cast<double>();
		Matrix<long long, Dynamic, Dynamic> HiB_LL = Matrix<long long, Dynamic, Dynamic>::Zero(n1 + 1LL, w1 + 1LL);
		HiB_LL(0, 0) = 1;
		HiB_LL.block(1, 1, n1, w1) = (Wtilde * J).transpose();
		HiB_LL.block(0, 1, 1, w1) = (Wtilde * j).transpose();
		Matrix<long long, Dynamic, Dynamic> tmp = Wtilde * J;
		BHiBi(0, 0) = 1.0;
		Matrix<double, Dynamic, Dynamic> Jtilde = (Wtilde * J * (Wtilde.transpose())).cast<double>();
		BHiBi.block(1, 1, w1, w1) = Jtilde.inverse();
		HiB = HiB_LL.cast<double>();
	}
	else
		if (c1 == 1)
		{
			Matrix<long long, Dynamic, Dynamic> Wtilde = Matrix<long long, Dynamic, Dynamic>::Zero(w1, n1);
			Matrix<long long, Dynamic, Dynamic> J = (Hi[n1]).block(1, 1, n1, n1);
			Vector<long long, Dynamic> j = (Hi[n1]).block(1, 0, n1, 1);
			Wtilde.block(0, 0, w1, n1) = W.block(0, 1, w1, n1);
			for (i = 0; i < w1; i++)
			{
				Vector<long long, Dynamic> tmp = Wtilde.row(i).transpose();
				remfac(tmp);
				Wtilde.row(i) = tmp.transpose();
			}
			B(1, 0) = 1;
			B.block(2, 1, w1, n1) = Wtilde.cast<double>();
			Matrix<long long, Dynamic, Dynamic> HiB_LL = Matrix<long long, Dynamic, Dynamic>::Zero(n1 + 1LL, w1 + 2LL);
			HiB_LL(0, 0) = 1;
			HiB_LL(0, 1) = j0;
			HiB_LL.block(1, 1, n1, 1) = j;
			HiB_LL.block(0, 2, 1, c1 + w1 - 1) = (Wtilde * j).transpose();
			HiB_LL.block(1, 2, n1, c1 + w1 - 1) = (Wtilde * J).transpose();
			Matrix<double, Dynamic, Dynamic> K = (Wtilde * J * Wtilde.transpose()).cast<double>().inverse();
			Vector<double, Dynamic> k = (Wtilde * j).cast<double>();
			double k0 = j0 - (k.transpose()) * (K * k);
			k = K * k;
			BHiBi(0, 0) = k0;
			BHiBi(0, 1) = -1.0;
			BHiBi(1, 0) = -1.0;
			BHiBi(1, 1) = 1;
			BHiBi.block(0, 2, 1, w1) = k.transpose();
			BHiBi.block(2, 0, w1, 1) = k;
			BHiBi.block(1, 2, 1, w1) = -k.transpose();
			BHiBi.block(2, 1, w1, 1) = -k;
			BHiBi.block(2, 2, w1, w1) = k * k.transpose();
			BHiBi /= k0 - 1;
			BHiBi.block(2, 2, w1, w1) += K;

			HiB = HiB_LL.cast<double>();
		}
		else
		{
			Matrix<long long, Dynamic, Dynamic> Wtilde = Matrix<long long, Dynamic, Dynamic>::Zero(w1, n1 + 1LL - c1);
			Vector<long long, Dynamic> j1 = (Hi[n1]).block(1, 0, c1 - 1, 1),
				j2 = (Hi[n1]).block(c1, 0, n1 + 1 - c1, 1);
			Matrix<long long, Dynamic, Dynamic> J11 = (Hi[n1]).block(1, 1, c1 - 1, c1 - 1),
				J21 = (Hi[n1]).block(c1, 1, n1 + 1 - c1, c1 - 1),
				J22 = (Hi[n1]).block(c1, c1, n1 + 1 - c1, n1 + 1 - c1);
			Wtilde.block(0, 0, w1, n1 + 1 - c1) = W.block(0, c1, w1, n1 + 1 - c1);
			for (i = 0; i < w1; i++)
			{
				Vector<long long, Dynamic> tmp = Wtilde.row(i).transpose();
				remfac(tmp);
				Wtilde.row(i) = tmp.transpose();
			}
			B(1, 0) = 1;
			B.block(2, 1, c1 - 1, c1 - 1) = MatrixXd::Identity(c1 - 1, c1 - 1);
			B.block(c1 + 1, c1, w1, n1 + 1 - c1) = Wtilde.cast<double>();
			Matrix<long long, Dynamic, Dynamic> HiB_LL =
				Matrix<long long, Dynamic, Dynamic>::Zero(n1 + 1LL, (c1 + 1LL) + w1);

			HiB_LL(0, 0) = 1;
			HiB_LL(0, 1) = j0;
			HiB_LL.block(0, 2, 1, c1 - 1) = j1.transpose();
			HiB_LL.block(0, c1 + 1LL, 1, w1) = (Wtilde * j2).transpose();

			HiB_LL.block(1, 1, c1 - 1, 1) = j1;
			HiB_LL.block(1, 2, c1 - 1LL, c1 - 1LL) = J11;
			HiB_LL.block(1, c1 + 1LL, c1 - 1LL, w1) = (Wtilde * J21).transpose();

			HiB_LL.block(c1, 1, n1 - c1 + 1, 1) = j2;
			HiB_LL.block(c1, 2, n1 - c1 + 1, c1 - 1) = J21;
			HiB_LL.block(c1, c1 + 1, n1 + 1 - c1, w1) = J22 * Wtilde.transpose();

			Matrix<long long, Dynamic, Dynamic> J_LL(w1 + c1 - 1, w1 + c1 - 1);
			Vector<long long, Dynamic> j_LL(w1 + (c1 - 1LL));
			J_LL.block(0, 0, c1 - 1LL, c1 - 1LL) = J11;
			J_LL.block(c1 - 1LL, 0, w1, c1 - 1LL) = Wtilde * J21;
			J_LL.block(0, c1 - 1LL, c1 - 1LL, w1) = J_LL.block(c1 - 1LL, 0, w1, c1 - 1).transpose();
			J_LL.block(c1 - 1LL, c1 - 1LL, w1, w1) = (Wtilde * J22) * Wtilde.transpose();
			j_LL.head(c1 - 1LL) = j1;
			j_LL.tail(w1) = Wtilde * j2;
			Matrix<double, Dynamic, Dynamic> K = J_LL.cast<double>().inverse();
			Vector<double, Dynamic> j = j_LL.cast<double>();
			Vector<double, Dynamic> k = K * j;
			double k0 = j0 - j.transpose() * k;
			BHiBi(0, 0) = k0;
			BHiBi(0, 1) = -1.0;
			BHiBi(1, 0) = -1.0;
			BHiBi(1, 1) = 1;
			BHiBi.block(0, 2, 1, w1 + c1 - 1) = k.transpose();
			BHiBi.block(2, 0, w1 + c1 - 1, 1) = k;
			BHiBi.block(1, 2, 1, w1 + c1 - 1) = -k.transpose();
			BHiBi.block(2, 1, w1 + c1 - 1, 1) = -k;
			BHiBi.block(2, 2, w1 + c1 - 1, w1 + c1 - 1) = k * k.transpose();
			BHiBi /= k0 - 1;
			BHiBi.block(2, 2, w1 + c1 - 1, w1 + c1 - 1) += K;

			HiB = HiB_LL.cast<double>();
		}
		P[cK] = MatrixXd::Identity(n1 + 1, n1 + 1) - HiB * (BHiBi * B);
}

void PWvlet::maintainFTrans(int n1, int N)
{
	if (N < 3)
	{
		cerr << "Too few points in data window\n";
		exit(3);
	}
	int nr, nc;
	MatrixXd F2tildeF;
	if (fTrans.count(N))
	{
		F2tildeF = fTrans.at(N);
		nr = (int)F2tildeF.rows();
		if (nr == (n1 + 1))
			return;
		nc = N;
	}
	else
		nr = nc = 0;
	F2tildeF.conservativeResize(n1 + 1ULL, N);
	int i, j, k;
	double Delta = 1.0 / (N - 1.0);
	if (nc == 0)
		nr = 0;
	double* terms = new double[n1 + 1LL];
	for (i = nr; i <= n1; i++)
	{
		if (i)
		{
			F2tildeF(i, 0) = pow(0.5 * Delta, i + 1) / (1.0 + i);
			for (j = 1; j < N - 1; j++)
			{
				terms[0] = pow(Delta, 1.0 + i) / (1.0 + i);
				for (k = 1; k <= i; k++)
					terms[k] = ((i + 2LL - k) * (j - 0.5) * terms[k - 1]) /k;
				F2tildeF(i, j) = KahanSum(terms, i + 1);
			}
			terms[0] = Delta / 2;
			for (k = 1; k <= i; k++)
				terms[k] = -(i + 1 - k) * Delta * terms[k - 1] / 2 / (k + 1.0);
			int i1 = i / 2, i2 = i - i1 - 1;
			double* terms1 = new double[i1 + 1], *terms2 = new double[i2 + 1];
			for (k = 0; k <= i1; k++)
				terms1[k] = terms[2 * k];
			for (k = 0; k <= i2; k++)
				terms2[k] = terms[2 * k + 1];
			F2tildeF(i, N - 1ULL) = KahanSum(terms1, i1 + 1) + KahanSum(terms2, i2 + 1);
			delete[] terms1, terms2;
		}
		else
		{
			F2tildeF(i, 0) = Delta / 2;
			for (j = 1; j < N - 1; j++)
				F2tildeF(i, j) = Delta;
			F2tildeF(i, N - 1) = Delta / 2;
		}
	}
	delete[] terms;
	fTrans[N] = F2tildeF;
}

/*provide override for drawable function*/
VectorXf PWvlet::points(int N)	//convert to values to be plotted against time
{
	VectorXf ret(N);
	int i;
	for (i = 0; i < N; i++)
		ret(i) = poly_eval(para, i/(N - 1.f));
	return ret;
}

/*make inner products*/
double PWvlet::IP(const dataWin1& dW) const	//L2 IP
{
	int N = dW.WLen;
	maintainFTrans(n, N);
	MatrixXd K = fTrans[N];
	K.conservativeResize((UINT64)n + 1ULL, dW.getLen());
	return dW.Vec.transpose() * (K.transpose() * para);
}

set<int> PWvlet::getValid()
{
	set<int> ret;
	for (auto item : valid)
		ret.insert(item.n);
	return ret;
}

set<pair<int, int>> PWvlet::getValid(int deg)
{
	set<pair<int, int>> ret;
	pair<int, int> val;
	for (auto item : valid)
		if (item.n== deg)
		{
			val.first = item.c;
			val.second = item.w;
			ret.insert(val);
		}
	return ret;
}


///////////////////////////////////////////////////////////////////////////////
//                                                                           //
//                 polar v1.0 - August the 15th 2019                         //
//                                                                           //
///////////////////////////////////////////////////////////////////////////////

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

#ifndef POLARIZABILITY_H
#define POLARIZABILITY_H

#include <complex>
#include <cmath>
#include <Eigen/Core>
#include "states.hpp"

class Polarizability
{

private:
   Eigen::Matrix3cd alpha_ee = Eigen::Matrix3cd::Zero(3,3),
                    alpha_em = Eigen::Matrix3cd::Zero(3,3),
                    alpha_me = Eigen::Matrix3cd::Zero(3,3),
                    alpha_mm = Eigen::Matrix3cd::Zero(3,3);

   double omega = 0.0,
          gamma = 1.0;

   // support function implementing the polarizability formula
   // (just the contribtion from a given excited state)
   std::complex<double> alpha_ij_function(const double&, const double&,
                                          const double&, const double&,
                                          const double&, const bool&, const bool&) const;

public:
   Polarizability(const States&, const double&, const double&);
   Eigen::Matrix3cd get_ee() const;
   Eigen::Matrix3cd get_mm() const;
   Eigen::Matrix3cd get_me() const;
   Eigen::Matrix3cd get_em() const;
   void Print() const;

};

#endif // POLARIZABILITY_H


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


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_COMPUTER_VISION_NELDER_MEAD_OPT_FUNDAMENTAL_HPP
#define PIC_COMPUTER_VISION_NELDER_MEAD_OPT_FUNDAMENTAL_HPP

#include "../util/eigen_util.hpp"
#include "../util/std_util.hpp"
#include "../util/matrix_3_x_3.hpp"
#include "../util/nelder_mead_opt_base.hpp"

#ifndef PIC_DISABLE_EIGEN
#ifndef PIC_EIGEN_NOT_BUNDLED
   #include "../externals/Eigen/Dense"
#else
    #include <Eigen/Dense>
#endif
#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

class NelderMeadOptFundamental: public NelderMeadOptBase<float>
{
public:
    std::vector< Eigen::Vector2f > m0, m1;

    /**
     * @brief NelderMeadOptFundamental
     * @param m0
     * @param m1
     * @param inliers
     */
    NelderMeadOptFundamental(std::vector< Eigen::Vector2f > &m0,
                             std::vector< Eigen::Vector2f > &m1,
                             std::vector< unsigned int> inliers) : NelderMeadOptBase()
    {
        filterInliers(m0, inliers, this->m0);
        filterInliers(m1, inliers, this->m1);
    }

    /**
     * @brief Fundamental
     * @param F
     * @param p
     * @return
     */
    double FundamentalDistance(Eigen::Matrix3d &F, Eigen::Vector3d &p0, Eigen::Vector3d  &p1)
    {
        Eigen::Vector3d F_p0 = F * p0;

        double norm = F_p0[0] * F_p0[0] + F_p0[1] * F_p0[1];
        if(norm > 0.0) {
            norm = sqrt(norm);
            F_p0 /= norm;
        }

        //computing distance

        return F_p0.dot(p1);
    }

    /**
     * @brief function
     * @param x
     * @param n
     * @return
     */
    float function(float *x, unsigned int n)
    {
        Eigen::Matrix3d F = getMatrixdFromLinearArray(x, 3, 3);
        Eigen::Matrix3d F_t = Eigen::Transpose<Eigen::Matrix3d>(F);

        double err = 0.0;
        for(unsigned int i = 0; i < m0.size(); i++) {

            Eigen::Vector3d p0 = Eigen::Vector3d(m0[i][0], m0[i][1], 1.0);
            Eigen::Vector3d p1 = Eigen::Vector3d(m1[i][0], m1[i][1], 1.0);

            double tmp_err;

            // | p1^t F p0 | error
            tmp_err = FundamentalDistance(F, p0, p1);
            err += tmp_err * tmp_err;

            // | p0^t F^t p1 | error
            tmp_err = FundamentalDistance(F_t, p1, p0);
            err += tmp_err * tmp_err;
        }

        return float(err);
    }
};
#endif

}

#endif // PIC_COMPUTER_VISION_NELDER_MEAD_OPT_FUNDAMENTAL_HPP


#define BOOST_TEST_MODULE Units
#include <boost/test/unit_test.hpp>

#include "sycomore/units.h"

BOOST_AUTO_TEST_CASE(BasicLiteral)
{
    using namespace sycomore::units;

    sycomore::Quantity const length = 100_cm;
    BOOST_TEST(length.magnitude == 1.);
    BOOST_TEST(length.dimensions == sycomore::Length);
}

BOOST_AUTO_TEST_CASE(DefinedBasicUnit)
{
    using namespace sycomore::units;

    sycomore::Quantity const length = 100*cm;
    BOOST_TEST(length.magnitude == 1.);
    BOOST_TEST(length.dimensions == sycomore::Length);
}

BOOST_AUTO_TEST_CASE(DerivedLiteral)
{
    using namespace sycomore::units;

    sycomore::Quantity const force = 1_N;
    BOOST_TEST(force.magnitude == 1.);
    BOOST_TEST(force.dimensions == sycomore::Force);
}

BOOST_AUTO_TEST_CASE(DefinedDerivedUnit)
{
    using namespace sycomore::units;

    sycomore::Quantity const force = 1*kN;
    BOOST_TEST(force.magnitude == 1000.);
    BOOST_TEST(force.dimensions == sycomore::Force);
}


#ifndef LATTICE_GRAPHENE_SQUARE_OPEN_BOUNDARY_HPP
#define LATTICE_GRAPHENE_SQUARE_OPEN_BOUNDARY_HPP

#include <string>
#include <vector>
#include <boost/lexical_cast.hpp>

namespace lattice {

class GrapheneSquareOpenBoundary{
public:
  static std::string name() { return "Graphene Square Open Boundary : input Ns is the number of hexagonal :(2*Ns) * (2*Ns+1) square";}
  static int set_num_particles(int Ns){return 2*Ns*(2*Ns+1);}
  GrapheneSquareOpenBoundary(int LatticeSize) : dim_(2*LatticeSize) {}
  void create_table(std::vector<std::vector < int > >& table){
  //for bulk
    for (int i = 0; i < dim_ + 1; ++i){
      for (int j = 0; j < dim_; ++j){
        int l = numberize(i,j);
        if(i == 0) table[l][0] = l;
        else table[l][0] = numberize(i-1,j);
        if(i == dim_) table[l][1] = l;
        else table[l][1] = numberize(i+1,j);
        if((i+j)%2 == 0){
          if(j == dim_ -1 ) table[l][2] = l;
          else table[l][2] = numberize(i,j+1);
        }
        else {
          if(j == 0) table[l][2] = l;
          else table[l][2] = numberize(i,j-1);
        }
      }
    }
  }

  int numberize(const int i, const int j){
    int i_t = (i + dim_+1);
    int j_t = (j + dim_);
    i_t %= (dim_+1) ;
    j_t %= dim_ ;
    return i_t + j_t * (dim_+1);
  }

  int latticize(int d, const int l){ //d is direction(0 ~ ) ex. 0 denotes x, 1 denotes y,~
    int j = l/(dim_+1);
    int i = (l - (dim_+1) *j);
    if(d==0) return i;
    else return j;
  }

  int target_number(const int i, const int observe){
    int sight_x = 0;
    int sight_y = 0;
    sight_x = latticize(0,observe);
    sight_y = latticize(1,observe);
    return  numberize(sight_x + i, sight_y);
  }

  int number_adjacent() {return 3;}

private:
  int dim_;
};//Regular cube end

} //end namespace

#endif //LLATTICE_GRAPHENE_SQUARE_OPEN_BOUNDARY_HPP


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

#ifndef NETKET_BOND_HAMILTONIAN_CC
#define NETKET_BOND_HAMILTONIAN_CC

#include <Eigen/Dense>
#include <array>
#include <unordered_map>
#include <vector>
#include "Utils/json_helper.hpp"
#include "local_operator.hpp"

namespace netket {

// BondHamiltonian on an arbitrary graph
template <class G>
class GraphHamiltonian : public AbstractHamiltonian {
  std::vector<LocalOperator> operators_;
  Hilbert hilbert_;

  // Arbitrary graph
  const G &graph_;

  // const std::size_t nvertices_;
  const int nvertices_;

 public:
  using MatType = LocalOperator::MatType;

  explicit GraphHamiltonian(const G &graph, const json &pars)
      : hilbert_(pars), graph_(graph), nvertices_(graph.Nsites()) {
    auto pars_hamiltonian = pars["Hamiltonian"];

    // Ensure that at least one of SiteOps and BondOps was initialized
    if (!FieldExists(pars_hamiltonian, "SiteOps") and
        FieldExists(pars_hamiltonian, "BondOps")) {
      pars_hamiltonian["SiteOps"] = std::vector<MatType>();

    } else if (!FieldExists(pars_hamiltonian, "BondOps") and
               FieldExists(pars_hamiltonian, "SiteOps")) {
      pars_hamiltonian["BondOps"] = std::vector<MatType>();
    } else if (!FieldExists(pars_hamiltonian, "BondOps") and
               !FieldExists(pars_hamiltonian, "SiteOps")) {
      throw InvalidInputError("Must input at least SiteOps or BondOps");
    }

    // Ensure that parameters are arrays
    if (!pars_hamiltonian["SiteOps"].is_array()) {
      throw InvalidInputError(
          "Hamiltonian: Bond operators object is not an array!");
    }
    if (!pars_hamiltonian["BondOps"].is_array()) {
      throw InvalidInputError(
          "Hamiltonian: Bond operators object is not an array!");
    }

    // Check BondOpColors
    if (!FieldExists(pars_hamiltonian, "BondOpColors")) {
      if (pars_hamiltonian["BondOps"].size() == 0) {
        pars_hamiltonian["BondOpColors"] = std::vector<int>();
      } else {
        pars_hamiltonian["BondOpColors"] =
            std::vector<int>(pars_hamiltonian["BondOps"].size(), 0);
      }
    }

    if (!pars_hamiltonian["BondOpColors"].is_array()) {
      throw InvalidInputError("Hamiltonian.BondOpColors is not an array");
    }

    // Save operators and bond colors
    auto sop = pars_hamiltonian["SiteOps"].get<std::vector<MatType>>();
    auto bop = pars_hamiltonian["BondOps"].get<std::vector<MatType>>();
    auto op_color = pars_hamiltonian["BondOpColors"].get<std::vector<int>>();

    // Site operators
    if (sop.size() > 0) {
      for (int i = 0; i < nvertices_; i++) {
        for (std::size_t j = 0; j < sop.size(); j++) {
          operators_.push_back(
              LocalOperator(hilbert_, sop[j], std::vector<int>{i}));
        }
      }
    }

    // Bond operators
    if (bop.size() != op_color.size()) {
      throw InvalidInputError(
          "The bond Hamiltonian definition is inconsistent."
          "The sizes of BondOps and BondOpColors do not match.");
    }

    if (bop.size() > 0) {
      // Use EdgeColors to populate operators
      for (auto const &kv : graph_.EdgeColors()) {
        for (std::size_t c = 0; c < op_color.size(); c++) {
          if (op_color[c] == kv.second && kv.first[0] < kv.first[1]) {
            std::vector<int> edge = {kv.first[0], kv.first[1]};
            operators_.push_back(LocalOperator(hilbert_, bop[c], edge));
          }
        }
      }
    }

    InfoMessage() << "Size of operators_ " << operators_.size() << std::endl;
  }

  void FindConn(const Eigen::VectorXd &v,
                std::vector<std::complex<double>> &mel,
                std::vector<std::vector<int>> &connectors,
                std::vector<std::vector<double>> &newconfs) const override {
    connectors.clear();
    newconfs.clear();
    mel.resize(0);

    for (std::size_t i = 0; i < operators_.size(); i++) {
      operators_[i].AddConn(v, mel, connectors, newconfs);
    }
  }

  const Hilbert &GetHilbert() const override { return hilbert_; }
};
}  // namespace netket
#endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "polar_svd.h"
#include <Eigen/SVD>
#include <Eigen/Geometry>

// Adapted from Olga's CGAL mentee's ARAP code
template <
  typename DerivedA,
  typename DerivedR,
  typename DerivedT>
IGL_INLINE void igl::polar_svd(
  const Eigen::PlainObjectBase<DerivedA> & A,
  Eigen::PlainObjectBase<DerivedR> & R,
  Eigen::PlainObjectBase<DerivedT> & T)
{
  DerivedA U;
  DerivedA V;
  Eigen::Matrix<typename DerivedA::Scalar,DerivedA::RowsAtCompileTime,1> S;
  return igl::polar_svd(A,R,T,U,S,V);
}

template <
  typename DerivedA,
  typename DerivedR,
  typename DerivedT,
  typename DerivedU,
  typename DerivedS,
  typename DerivedV>
IGL_INLINE void igl::polar_svd(
  const Eigen::PlainObjectBase<DerivedA> & A,
  Eigen::PlainObjectBase<DerivedR> & R,
  Eigen::PlainObjectBase<DerivedT> & T,
  Eigen::PlainObjectBase<DerivedU> & U,
  Eigen::PlainObjectBase<DerivedS> & S,
  Eigen::PlainObjectBase<DerivedV> & V)
{
  using namespace std;
  Eigen::JacobiSVD<DerivedA> svd;
  svd.compute(A, Eigen::ComputeFullU | Eigen::ComputeFullV );
  U = svd.matrixU();
  V = svd.matrixV();
  S = svd.singularValues();
  R = U*V.transpose();
  const auto & SVT = S.asDiagonal() * V.adjoint();
  // Check for reflection
  if(R.determinant() < 0)
  {
    // Annoyingly the .eval() is necessary
    auto W = V.eval();
    W.col(V.cols()-1) *= -1.;
    R = U*W.transpose();
    T = W*SVT;
  }else
  {
    T = V*SVT;
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::polar_svd<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::polar_svd<Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, 2, 2, 0, 2, 2> >(Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >&);
template void igl::polar_svd<Eigen::Matrix<double, 3, 3, 0, 3, 3>, Eigen::Matrix<double, 3, 3, 0, 3, 3>, Eigen::Matrix<double, 3, 3, 0, 3, 3> >(Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 3, 0, 3, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 3, 0, 3, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 3, 0, 3, 3> >&);
template void igl::polar_svd<Eigen::Matrix<float, 2, 2, 0, 2, 2>, Eigen::Matrix<float, 2, 2, 0, 2, 2>, Eigen::Matrix<float, 2, 2, 0, 2, 2> >(Eigen::PlainObjectBase<Eigen::Matrix<float, 2, 2, 0, 2, 2> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, 2, 2, 0, 2, 2> >&, Eigen::PlainObjectBase<Eigen::Matrix<float, 2, 2, 0, 2, 2> >&);
template void igl::polar_svd<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, 2, 2, 0, 2, 2> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >&);
template void igl::polar_svd<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::polar_svd<Eigen::Matrix<float,3,3,0,3,3>,Eigen::Matrix<float,3,3,0,3,3>,Eigen::Matrix<float,3,3,0,3,3>,Eigen::Matrix<float,3,3,0,3,3>,Eigen::Matrix<float,3,1,0,3,1>,Eigen::Matrix<float,3,3,0,3,3> >(Eigen::PlainObjectBase<Eigen::Matrix<float,3,3,0,3,3> > const &,Eigen::PlainObjectBase<Eigen::Matrix<float,3,3,0,3,3> > &,Eigen::PlainObjectBase<Eigen::Matrix<float,3,3,0,3,3> > &,Eigen::PlainObjectBase<Eigen::Matrix<float,3,3,0,3,3> > &,Eigen::PlainObjectBase<Eigen::Matrix<float,3,1,0,3,1> > &,Eigen::PlainObjectBase<Eigen::Matrix<float,3,3,0,3,3> >&);
template void igl::polar_svd<Eigen::Matrix<double, 3, 3, 0, 3, 3>, Eigen::Matrix<double, 3, 3, 0, 3, 3>, Eigen::Matrix<double, 3, 3, 0, 3, 3>, Eigen::Matrix<double, 3, 3, 0, 3, 3>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 3, 0, 3, 3> >(Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 3, 0, 3, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 3, 0, 3, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 3, 0, 3, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 3, 0, 3, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 3, 0, 3, 3> >&);
template void igl::polar_svd<Eigen::Matrix<float, 2, 2, 0, 2, 2>, Eigen::Matrix<float, 2, 2, 0, 2, 2>, Eigen::Matrix<float, 2, 2, 0, 2, 2>, Eigen::Matrix<float, 2, 2, 0, 2, 2>, Eigen::Matrix<float, 2, 1, 0, 2, 1>, Eigen::Matrix<float, 2, 2, 0, 2, 2> >(Eigen::PlainObjectBase<Eigen::Matrix<float, 2, 2, 0, 2, 2> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, 2, 2, 0, 2, 2> >&, Eigen::PlainObjectBase<Eigen::Matrix<float, 2, 2, 0, 2, 2> >&, Eigen::PlainObjectBase<Eigen::Matrix<float, 2, 2, 0, 2, 2> >&, Eigen::PlainObjectBase<Eigen::Matrix<float, 2, 1, 0, 2, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<float, 2, 2, 0, 2, 2> >&);
template void igl::polar_svd<Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, 2, 1, 0, 2, 1>, Eigen::Matrix<double, 2, 2, 0, 2, 2> >(Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 1, 0, 2, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >&);
template void igl::polar_svd<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::polar_svd<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
#endif


#define BOOST_TEST_MODULE ExpectecSquaredJumpDistanceTestSuite

#include <boost/test/included/unit_test.hpp>
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <hops/hops.hpp>

BOOST_AUTO_TEST_SUITE(ExpectedSquaredJumpDistanceTestSuite)
    BOOST_AUTO_TEST_CASE(ComputeAllDraws) {
        double expectedResult = 2./3.;
        std::vector<double> chain1{0, 1, 1, 2};
        std::vector<double> chain2{0, 0, 1, 2};

        Eigen::MatrixXd covariance = Eigen::MatrixXd::Identity(1, 1);

        std::vector<std::vector<Eigen::VectorXd>> chains(2);

        for (size_t i = 0; i < chain1.size(); ++i) {
            Eigen::VectorXd draw(1);
            draw(0) = chain1[i];
            chains[0].push_back(draw);
        }

        for (size_t i = 0; i < chain2.size(); ++i) {
            Eigen::VectorXd draw(1);
            draw(0) = chain2[i];
            chains[1].push_back(draw);
        }

        for (size_t i = 0; i < chains.size(); ++i) {
            double esjd = hops::computeExpectedSquaredJumpDistance<Eigen::VectorXd, Eigen::MatrixXd>(chains[i], covariance);
            BOOST_CHECK_CLOSE(expectedResult, esjd, 0.01);
        }
    }

    BOOST_AUTO_TEST_CASE(ComputeAllDrawsPointerArray) {
        double expectedResult = 2./3.;
        std::vector<Eigen::VectorXd> chain1;
        std::vector<Eigen::VectorXd> chain2;

        Eigen::MatrixXd covariance = Eigen::MatrixXd::Identity(1, 1);

        for (auto& i : {0, 1, 1, 2}) {
            Eigen::VectorXd x(1);
            x(0) = i;
            chain1.push_back(x);
        }

        for (auto& i : {0, 0, 1, 2}) {
            Eigen::VectorXd x(1);
            x(0) = i;
            chain2.push_back(x);
        }

        std::vector<const std::vector<Eigen::VectorXd>*> chains;

        chains.push_back(&chain1);
        chains.push_back(&chain2);

        auto esjds = hops::computeExpectedSquaredJumpDistance<Eigen::VectorXd, Eigen::MatrixXd>(chains, covariance);

        for (size_t i = 0; i < chains.size(); ++i) {
            BOOST_CHECK_CLOSE(expectedResult, esjds[i], 0.01);
        }
    }

    BOOST_AUTO_TEST_CASE(ComputeIncrementally) {
        std::vector<double> expectedResult{1, 0};
        std::vector<double> chain1{0, 1};
        std::vector<double> chain2{0, 0};

        Eigen::MatrixXd covariance = Eigen::MatrixXd::Identity(1, 1);

        std::vector<std::vector<Eigen::VectorXd>> chains(2);

        for (size_t i = 0; i < chain1.size(); ++i) {
            Eigen::VectorXd draw(1);
            draw(0) = chain1[i];
            chains[0].push_back(draw);
        }

        for (size_t i = 0; i < chain2.size(); ++i) {
            Eigen::VectorXd draw(1);
            draw(0) = chain2[i];
            chains[1].push_back(draw);
        }

        std::vector<double> esjds;
        for (size_t i = 0; i < chains.size(); ++i) {
            double esjd = hops::computeExpectedSquaredJumpDistance<Eigen::VectorXd, Eigen::MatrixXd>(chains[i], covariance);
            esjds.push_back(esjd);
            BOOST_CHECK_CLOSE(expectedResult[i], esjd, 0.01);
        }

        chain1 = std::vector<double>({1, 2});
        chain2 = std::vector<double>({1, 2});

        for (size_t i = 0; i < chain1.size(); ++i) {
            Eigen::VectorXd draw(1);
            draw(0) = chain1[i];
            chains[0].push_back(draw);
        }

        for (size_t i = 0; i < chain2.size(); ++i) {
            Eigen::VectorXd draw(1);
            draw(0) = chain2[i];
            chains[1].push_back(draw);
        }

        for (size_t i = 0; i < chains.size(); ++i) {
            double esjd = hops::computeExpectedSquaredJumpDistance<Eigen::VectorXd, Eigen::MatrixXd>(chains[i], 2, esjds[i], 2, covariance);
            BOOST_CHECK_CLOSE(2./3., esjd, 0.01);
        }
    }

    BOOST_AUTO_TEST_CASE(ComputeIncrementallyPointerArray) {
        std::vector<double> expectedResult{1, 0};
        std::vector<const std::vector<Eigen::VectorXd>*> chains;

        Eigen::MatrixXd covariance = Eigen::MatrixXd::Identity(1, 1);

        std::vector<Eigen::VectorXd>chain1;
        std::vector<Eigen::VectorXd>chain2;

        for (auto& i : {0, 1}) {
            Eigen::VectorXd draw(1);
            draw(0) = i;
            chain1.push_back(draw);
        }

        for (auto& i : {0, 0}) {
            Eigen::VectorXd draw(1);
            draw(0) = i;
            chain2.push_back(draw);
        }

        chains.push_back(&chain1);
        chains.push_back(&chain2);

        auto esjds = hops::computeExpectedSquaredJumpDistance<Eigen::VectorXd, Eigen::MatrixXd>(chains, covariance);

        for (size_t i = 0; i < chains.size(); ++i) {
            BOOST_CHECK_CLOSE(expectedResult[i], esjds[i], 0.01);
        }

        for (auto& i : {1, 2}) {
            Eigen::VectorXd draw(1);
            draw(0) = i;
            chain1.push_back(draw);
            chain2.push_back(draw);
        }

        esjds = hops::computeExpectedSquaredJumpDistance<Eigen::VectorXd, Eigen::MatrixXd>(chains, 2, esjds, 2, covariance);
        for (size_t i = 0; i < chains.size(); ++i) {
            BOOST_CHECK_CLOSE(2./3., esjds[i], 0.01);
        }
    }
BOOST_AUTO_TEST_SUITE_END()


#include <matmat.hpp>
#include <iostream>
#include <chrono>
#include <omp.h>
#include <boost/align/aligned_allocator.hpp>
#include <vector>

#define LOOP_COUNT 100

using namespace std;

template <typename T>
using aligned_alloc_64 = boost::alignment::aligned_allocator<T, 64>;

template <typename T>
using aligned_vector = vector<T, aligned_alloc_64<T>>;

int main(int argc, char** argv)
{

  if(argc < 2)
  {
    cout << "matmat threads1 threads2 ..." << endl;
    exit(EXIT_FAILURE);
  }

  int thread_counts[argc - 1];

  for(int i = 1; i < argc; ++i)
    thread_counts[i - 1] = stoi(argv[i]);

  for(int N = 64; N < 8192; N *= 2)
  {

    for(int t = 0; t < argc - 1; ++t)
    {

      for(int trans = 0; trans < 2; ++trans)
      {

	int threads = thread_counts[t];
	omp_set_num_threads(threads);

	//Actual test matrices
	int m = N;
	int n = N;
	int k = N;
	int lda = N;
	int ldb = N;
	int ldc = N;

	aligned_vector<double> A(m * lda);
	aligned_vector<double> B(n * ldb);
	aligned_vector<double> C(m * ldc);

	for(int i = 0; i < m * lda; ++i)
	  A[i] = i;

	for(int i = 0; i < n * ldb; ++i)
	  B[i] = i - 600;

	for(int i = 0; i < m * ldc; ++i)
	  C[i] = 0;

	auto start = chrono::high_resolution_clock::now();

	for(int i = 0; i < LOOP_COUNT; ++i)
	{
	  matmat(1.0, trans, A.data(), m, n, lda, B.data(), k, ldb, 1.0, C.data(), ldc);
	}

	auto end = chrono::high_resolution_clock::now();
	auto elapsed_seconds = chrono::duration<double>(end-start).count();

	cout << "N " << N << " THREADS " << threads << " TRANS " << trans << " RT " << elapsed_seconds << endl;
      }

    }
  }

  return 0;
}


#include "stdafx.h"
#include <catch.hpp>
#include <boost/spirit/home/x3.hpp>
#include <boost/spirit/home/x3/support/ast/variant.hpp>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/fusion/include/std_pair.hpp>
#include <boost/fusion/include/io.hpp>

#include <iostream>
#include <list>
#include <map>

namespace x3 = boost::spirit::x3;

namespace client {
namespace ast
{
///////////////////////////////////////////////////////////////////////////
//  The AST
///////////////////////////////////////////////////////////////////////////
struct nil {};
struct signed_;
struct expression;

struct operand : x3::variant<
	nil
	, unsigned int
	, x3::forward_ast<signed_>
	, x3::forward_ast<expression>
>
{
	using base_type::base_type;
	using base_type::operator=;
};

struct signed_
{
	char sign;
	operand operand_;
};

struct operation
{
	char operator_;
	operand operand_;
};

struct expression
{
	operand first;
	std::list<operation> rest;
};

// print function for debugging
inline std::ostream& operator<<(std::ostream& out, nil) { out << "nil"; return out; }
} // ast
} // client

BOOST_FUSION_ADAPT_STRUCT(client::ast::signed_,
	sign, operand_
)

BOOST_FUSION_ADAPT_STRUCT(client::ast::operation,
	operator_, operand_
)

BOOST_FUSION_ADAPT_STRUCT(client::ast::expression,
	first, rest
)

namespace client
{
///////////////////////////////////////////////////////////////////////////
//  The Virtual Machine
///////////////////////////////////////////////////////////////////////////
enum byte_code
{
	op_neg,     //  negate the top stack entry
	op_add,     //  add top two stack entries
	op_sub,     //  subtract top two stack entries
	op_mul,     //  multiply top two stack entries
	op_div,     //  divide top two stack entries
	op_int,     //  push constant integer into the stack
};

class vmachine
{
public:

	vmachine(unsigned stackSize = 4096)
		: stack(stackSize)
		, stack_ptr(stack.begin())
	{
	}

	int top() const { return stack_ptr[-1]; };
	void execute(std::vector<int> const& code);

private:
	std::vector<int> stack;
	std::vector<int>::iterator stack_ptr;
};

void vmachine::execute(std::vector<int> const& code)
{
	std::vector<int>::const_iterator pc = code.begin();
	stack_ptr = stack.begin();

	while (pc != code.end())
	{
		switch (*pc++)
		{
		case op_neg:
			stack_ptr[-1] = -stack_ptr[-1];
			break;

		case op_add:
			--stack_ptr;
			stack_ptr[-1] += stack_ptr[0];
			break;

		case op_sub:
			--stack_ptr;
			stack_ptr[-1] -= stack_ptr[0];
			break;

		case op_mul:
			--stack_ptr;
			stack_ptr[-1] *= stack_ptr[0];
			break;

		case op_div:
			--stack_ptr;
			stack_ptr[-1] /= stack_ptr[0];
			break;

		case op_int:
			*stack_ptr++ = *pc++;
			break;
		}
	}
}

///////////////////////////////////////////////////////////////////////////
//  The Compiler
///////////////////////////////////////////////////////////////////////////
struct compiler
{
	typedef void result_type;

	std::vector<int>& code;
	compiler(std::vector<int>& code)
		: code(code) {}

	void operator()(ast::nil) const { BOOST_ASSERT(0); }
	void operator()(unsigned int n) const
	{
		code.push_back(op_int);
		code.push_back(n);
	}

	void operator()(ast::operation const& x) const
	{
		boost::apply_visitor(*this, x.operand_);
		switch (x.operator_)
		{
		case '+': code.push_back(op_add); break;
		case '-': code.push_back(op_sub); break;
		case '*': code.push_back(op_mul); break;
		case '/': code.push_back(op_div); break;
		default: BOOST_ASSERT(0); break;
		}
	}

	void operator()(ast::signed_ const& x) const
	{
		boost::apply_visitor(*this, x.operand_);
		switch (x.sign)
		{
		case '-': code.push_back(op_neg); break;
		case '+': break;
		default: BOOST_ASSERT(0); break;
		}
	}

	void operator()(ast::expression const& x) const
	{
		boost::apply_visitor(*this, x.first);
		for (ast::operation const& oper : x.rest)
		{
			(*this)(oper);
		}
	}
};

///////////////////////////////////////////////////////////////////////////////
//  The calculator grammar
///////////////////////////////////////////////////////////////////////////////
namespace calculator_grammar
{
using x3::uint_;
using x3::char_;

struct expression_class;
struct term_class;
struct factor_class;

x3::rule<expression_class, ast::expression> const expression("expression");
x3::rule<term_class, ast::expression> const term("term");
x3::rule<factor_class, ast::operand> const factor("factor");

auto const expression_def =
term
>> *((char_('+') > term)
	| (char_('-') > term)
	)
	;

auto const term_def =
factor
>> *((char_('*') > factor)
	| (char_('/') > factor)
	)
	;

auto const factor_def =
uint_
| '(' > expression > ')'
| (char_('-') > factor)
| (char_('+') > factor)
;

BOOST_SPIRIT_DEFINE(
	expression
	, term
	, factor
);

struct expression_class
{
	//  Our error handler
	template <typename Iterator, typename Exception, typename Context>
	x3::error_handler_result
		on_error(Iterator&, Iterator const& last, Exception const& x, Context const& context)
	{
		std::cout
			<< "Error! Expecting: "
			<< x.which()
			<< " here: \""
			<< std::string(x.where(), last)
			<< "\""
			<< std::endl
			;
		return x3::error_handler_result::fail;
	}
};

auto calculator = expression;
}

using calculator_grammar::calculator;
}

TEST_CASE("calculator example")
{
	//
	// learn to parse value expressions
	// a large example
	// - variable / expression
	// - vm to calculate
	//

	SECTION("run parser")
	{
		typedef std::string::const_iterator iterator_type;
		typedef client::ast::expression ast_expression;
		typedef client::compiler compiler;

		std::string str = "3 * 2 + 1";

		client::vmachine mach;              // Our virtual machine
		std::vector<int> code;              // Our VM code
		auto& calc = client::calculator;    // Our grammar
		ast_expression expression;          // Our program (AST)
		compiler compile(code);             // Compiles the program

		iterator_type iter = str.begin();
		iterator_type const end = str.end();
		boost::spirit::x3::ascii::space_type space;
		bool r = phrase_parse(iter, end, calc, space, expression);

		CHECK(r);
		CHECK(iter == end);

		compile(expression); // code generation

		mach.execute(code);  // execute
	}
}


#pragma once

#include <vector>

#include <Core/RaCore.hpp>
#include <Eigen/Core>

namespace Ra {
namespace Core {
/// This class stores a D-dimensional grid of elements of arbitrary type.
/// in a contiguous memory block. Elements are stored in column-major order.
/// e.g. for a 3x3x3 array the element vector looks like
///  [A000, A100, A200, A010,... A222].
/// Elements are accessible with a D-dimensional Vector, or linearly with
/// iterators, thanks to the std-like interface provided.
template <typename T, uint D>
class Grid
{

  public:
    // public types and constants.
    static const uint Dimension = D; /// Dimension of our grid
    using IdxVector =
        Eigen::Matrix<uint, D, 1>; /// A vector of the size of the grid along each dimension.
    using OffsetVector = Eigen::Matrix<int, D, 1>; /// A vector of signed offsets.

    /// This class implements an iterator though elements of the grid that
    /// can be referenced with a linear index or a D-dimensional uint vector.
    class Iterator
    {
      public:
        //
        // Construction
        //

        /// Constructor from size and linear index.
        explicit Iterator( const IdxVector& size, uint startIdx = 0 );

        /// Constructor from size and vector index.
        Iterator( const IdxVector& size, const IdxVector& startIdx );

        /// Constructor from grid and linear index.
        explicit Iterator( const Grid<T, D>& grid, uint startIdx = 0 );

        /// Constructor from grid and vector index.
        Iterator( const Grid<T, D>& grid, const IdxVector& startIdx );

        /// Default copy constructor and assignment operator.
        Iterator( const Iterator& other ) = default;
        Iterator& operator=( const Iterator& other ) = default;

        /// Cast to the an iterator in a different type grid.
        template <typename T2>
        typename Grid<T2, D>::Iterator cast() const;

        //
        // Basic getters and setters
        //

        /// Set an existing iterator to a linear index value.
        inline void setFromLinear( uint idx );

        /// Set an existing iterator to a vector index value.
        inline void setFromVector( const IdxVector& idx );

        /// Get the current linear index.
        inline uint getLinear() const;

        /// Get the current vector index.
        inline IdxVector getVector() const;

        /// Get the associated grid size.
        inline const IdxVector& getGridSize() const;

        /// Returns true if the current index is out of the grid.
        inline bool isOut() const;

        /// Returns true if the current index is inside the grid.
        inline bool isIn() const;

        //
        // Increment and decrement operators
        //

        /// Advance the iterator to the next-element
        inline Iterator& operator++();
        inline Iterator operator++( int );

        /// Move the iterator back to the previous element
        inline Iterator& operator--();
        inline Iterator operator--( int );

        //
        // Add and substract operators with linear offset
        //

        /// Advance the iterator with an offset of i elements
        inline Iterator& operator+=( uint i );

        /// Rewind the iterator with an offset of i elements
        inline Iterator& operator-=( uint i );

        //
        // Add and substract operators with vector offset
        //

        // Note : these convenience functions do not do any kind
        // of check. If you get out of bounds on one dimension, the iterator
        // might still be valid but not pointing to the element you think
        // it should.

        inline Iterator& operator+=( const IdxVector& idx );
        inline Iterator& operator-=( const IdxVector& idx );
        inline Iterator& operator+=( const OffsetVector& idx );

        /// Returns true if adding offset idx with current iterator will act
        /// as expected and false if it will put it out of bounds.
        bool isValidOffset( const OffsetVector& idx );

        //
        // Comparison operators
        //

        // Note : comparing iterators of different grid sizes will assert.

        inline bool operator==( const Iterator& other ) const;
        inline bool operator<( const Iterator& other ) const;

        //
        // Extra operators
        //

        // Declare binary operators directly (see C++ faq 35.16)
        friend Iterator operator+( const Iterator& it, uint i ) {
            Iterator copy( it );
            return copy += i;
        }
        friend Iterator operator-( const Iterator& it, uint i ) {
            Iterator copy( it );
            return copy -= i;
        }
        friend Iterator operator+( const Iterator& it, const IdxVector& idx ) {
            Iterator copy( it );
            return copy += idx;
        }
        friend Iterator operator-( const Iterator& it, const IdxVector& idx ) {
            Iterator copy( it );
            return copy -= idx;
        }
        friend Iterator operator+( const Iterator& it, const OffsetVector& idx ) {
            Iterator copy( it );
            return copy += idx;
        }

        // Extraneous comparison operators default implementation.
        inline bool operator!=( const Iterator& rhs ) const { return !( *this == rhs ); }
        inline bool operator>( const Iterator& rhs ) const { return rhs < *this; }
        inline bool operator<=( const Iterator& rhs ) const { return !( *this > rhs ); }
        inline bool operator>=( const Iterator& rhs ) const { return !( *this < rhs ); }

      private:
        const IdxVector& m_sizes; /// Size of the underlying grid
        uint m_index;             /// Current linear index of the iterator
    };

  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    //
    // Constructors
    //

    /// Construct a grid of a given size and fill it with the given value.
    Grid( const IdxVector& size = IdxVector::Zero(), const T& val = T() );
    /// Construct a grid of a given size with values in ()-major format
    Grid( const IdxVector& size, const T* values );

    /// Copy constructor and assignment operator.
    Grid( const Grid<T, D>& other ) = default;
    Grid& operator=( const Grid<T, D>& other ) = default;

    //
    // Basic getters
    //

    /// Returns the number of elements stored.
    inline uint size() const;
    /// Returns the size vector (a D-dimensional vector with the size along each dimension).
    inline const IdxVector& sizeVector() const;
    /// Returns true if the grid is empty (i.e. if size() ==0).
    inline bool empty() const;
    /// Erases all data and makes the grid empty.
    inline void clear();

    //
    // Element access
    //

    /// Access an element with a D-dimensional index.
    // Note that since initializer lists are implicitly convertible to
    // Eigen vectors, you can call grid.at({x,y,z})
    inline const T& at( const IdxVector& idx ) const;
    inline T& at( const IdxVector& idx );

    /// Access an element with a linear index.
    inline const T& at( uint idx ) const;
    inline T& at( uint idx );

    /// Access an element with an iterator.
    inline const T& at( const Iterator& it ) const;
    inline T& at( const Iterator& it );

    /// Read only access to the underlying data.
    inline const T* data() const;

    /// Read-write access to the underlying data.
    inline T* data();

    //
    // std::iterators-like interface
    //

    /// Get an iterator on this grid at the first element.
    inline Iterator begin();
    inline Iterator begin() const;

    /// Get an iterator on this grid past the last element.
    inline Iterator end();
    inline Iterator end() const;

  protected:
    /// Indicate the extends of the grid along each dimension.
    IdxVector m_size;
    /// Storage for the grid data.
    std::vector<T> m_data;
};
} // namespace Core
} // namespace Ra

#include <Core/Containers/Grid.inl>