/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
* \file     isovist.cpp
* \author   Collin Johnson
*
* Implementation of classes:
*
*   - Isovist
*   - IsovistField
*/

#include <utils/isovist.h>
#include <math/covariance.h>
#include <math/geometry/convex_hull.h>
#include <math/moments_features.h>
#include <math/trigonometry.h>
#include <math/zernike_moments.h>
#include <math/geometry/shape_fitting.h>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/skewness.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/range/iterator_range.hpp>
#include <cmath>

namespace vulcan
{
namespace utils
{

static math::ZernikeMoments<5> zernike;

double area     (Isovist::Iter begin, Isovist::Iter end);
double perimeter(Isovist::Iter begin, Isovist::Iter end);
double circularity(double area, double perimeter);

template <class BinaryOp> // BinaryOp = double func(Point<double>, Point<double>)
double accumulate_pair(Isovist::Iter begin, Isovist::Iter end, BinaryOp op);

std::string Isovist::scalarName(Scalar scalar)
{
    switch(scalar)
    {
        case kArea:
            return "area";
        case kPerimeter:
            return "perimeter";
        case kCircularity:
            return "circularity";
        case kOrientation:
            return "orientation";
        case kWeightedOrientation:
            return "weighted_orientation";
        case kShapeEccentricity:
            return "eccentricity";
        case kShapeCompactness:
            return "shape compactness";
        case kShapeWaviness:
            return "waviness";
        case kMinDistOrientation:
            return "min_dist_orientation";
        case kMaxThroughDist:
            return "max_through_dist";
        case kMaxThroughDistOrientation:
            return "max_through_dist_orientation";
        case kMinLineDist:
            return "min_line_dist";
        case kMinLineNormal:
            return "min_line_normal";
        case kMinNormalDiff:
            return "min_normal_diff";
        case kDmax:
            return "dist_max";
        case kDmin:
            return "dist_min";
        case kDavg:
            return "dist_avg";
        case kDstd:
            return "dist_std";
        case kDVariation:
            return "dist_variation";
        case kDskewness:
            return "dist_skewness";
        case kShapeDistAvg:
            return "shape_dist_avg";
        case kShapeDistStd:
            return "shape_dist_std";
        case kShapeDistVariation:
            return "shape_dist_variation";
        case kShapeDistCompactness:
            return "shape_dist_compactness";
        case kDeltaAvg:
            return "delta_avg";
        case kDeltaStd:
            return "delta_std";
        case kDeltaVariation:
            return "delta_variation";
        case kDistRelationAvg:
            return "dist_relation_avg";
        case kDistRelationStd:
            return "dist_relation_std";
        case kRayCompactness:
            return "ray compactness";
        case kAngleBetweenMinDists:
            return "angle between min dists";
        case kMinHalfArea:
            return "min half area";
        case kAreaBalance:
            return "area balance";
        default:
            return "unknown";
    }

    return "Unknown";
}

void Isovist::calculateScalars(PointIter begin, PointIter end, std::vector<double>& scalars)
{
    scalars_.resize(kNumScalars + zernike.numMoments());

    calculateDistanceScalars(begin, end, scalars);
    calculateDeltaScalars(begin, end, scalars);
    calculatePolygonScalars(begin, end, scalars);
    calculateMomentsScalars(begin, end, scalars);
}

void Isovist::calculateDistanceScalars(PointIter begin, PointIter end, std::vector<double>& scalars)
{
    using namespace boost::accumulators;

    accumulator_set<double, stats<tag::mean, tag::lazy_variance, tag::skewness, tag::max, tag::min>> distanceAcc;
    accumulator_set<double, stats<tag::mean, tag::max, tag::lazy_variance>> distFromCenterAcc;
    accumulator_set<double, stats<tag::mean, tag::lazy_variance, tag::count>> relationAcc;

    auto shapeCenter = std::accumulate(begin, end, Point<float>(0.0f, 0.0f));
    shapeCenter.x /= std::distance(begin, end);
    shapeCenter.y /= std::distance(begin, end);

    double lastDist = distance_between_points(position_, *(end - 1));
    double minDist = std::numeric_limits<double>::max();
    double minDistOrientation = 0.0;

    for(auto p : boost::make_iterator_range(begin, end))
    {
        double dist = distance_between_points(position_, p);
        distanceAcc(dist);
        distFromCenterAcc(distance_between_points(shapeCenter, p));
        relationAcc(std::abs(dist - lastDist));

        // Keeping track of min dist here for the min dist orientation calculation
        if(dist < minDist)
        {
            minDist = dist;
            minDistOrientation = angle_to_point(position_, p);
        }
        lastDist = dist;
    }

    double secondShortestDist = std::numeric_limits<double>::max();
    double secondShortestOrientation = 0.0;

    // Find the other local minimum. Need to see that it's actually a minimum when doing the computation
    // Only relative distances matter here, so we can use the squared_point_distance for efficiency
    double prevDist = squared_point_distance(position_, *(end-2)); // *begin == *end-1 b/c poly wraps around
    double curDist = squared_point_distance(position_, *begin);

    // Want the min diff to be far enough for the discretization error to not be the source of
    // the next min dist
    const double kMinAngleDiff = std::acos(minDist / (minDist + 0.1));

    for(auto pIt = begin, endIt = end - 1; pIt != endIt; ++pIt)
    {
        double nextDist = (pIt + 1 != endIt) ? squared_point_distance(position_, *(pIt + 1)) :
            squared_point_distance(position_, *begin);

        // If this is a local minima cell, check that it is a little far from the min dist
        // then see if its angle diff is greater than the existing, but it is still within 0.1m
        // of the min dist. If so, it's a discretization error that is causing the miss.
        if((curDist < secondShortestDist)
            && (curDist < nextDist) && (curDist < prevDist))
        {
            // Avoid the angle_to_point except when it matters -- atan2 is expensive!
            double orientation = angle_to_point(position_, *pIt);

            if(angle_diff_abs(orientation, minDistOrientation) > kMinAngleDiff)
            {
                secondShortestDist = curDist;
                secondShortestOrientation = orientation;
            }
        }

        prevDist = curDist;
        curDist = nextDist;
    }

    scalars[kMinDistOrientation] = minDistOrientation;
    scalars[kAngleBetweenMinDists] = angle_diff_abs(minDistOrientation, secondShortestOrientation);

    scalars[kDavg] = mean(distanceAcc);
    scalars[kDstd] = std::sqrt(variance(distanceAcc));
    scalars[kDVariation] = (scalars[kDavg] > 0.0) ? scalars[kDstd] / scalars[kDavg] : 0.0;
    scalars[kDskewness] = skewness(distanceAcc);
    scalars[kDmin] = min(distanceAcc);
    scalars[kDmax] = max(distanceAcc);
    scalars[kShapeDistAvg] = mean(distFromCenterAcc);
    scalars[kShapeDistStd] = std::sqrt(variance(distFromCenterAcc));
    scalars[kShapeDistVariation] = (scalars[kShapeDistAvg] > 0.0) ? scalars[kShapeDistStd] / scalars[kShapeDistAvg] : 0.0;
    scalars[kShapeDistCompactness] = (max(distFromCenterAcc) > 0.0) ? scalars[kShapeDistAvg] / max(distFromCenterAcc) : 1e5;

    scalars[kRayCompactness] = (max(distFromCenterAcc) > 0.0) ? (mean(distFromCenterAcc) / max(distFromCenterAcc)) : 1.0;

    if(count(relationAcc) > 1)
    {
        assert(!std::isnan(mean(relationAcc)));
        scalars[kDistRelationAvg] = mean(relationAcc);
        scalars[kDistRelationStd] = std::sqrt(variance(relationAcc));
    }

    double maxDist = 0.0;
    int maxIdx = 0;

    int otherSide = std::distance(begin, end) / 2;
    for(std::size_t n = 0, end = otherSide; n < end; ++n)
    {
        double dist = squared_point_distance(*(begin + n), *(begin + n + otherSide));
        if(dist > maxDist)
        {
            maxDist = dist;
            maxIdx = n;
        }
    }

    scalars[kMaxThroughDist] = std::sqrt(maxDist);
    scalars[kMaxThroughDistOrientation] = angle_to_point(position_, *(begin + maxIdx));
}

void Isovist::calculateDeltaScalars(PointIter begin, PointIter end, std::vector<double>& scalars)
{
    using namespace boost::accumulators;

    accumulator_set<double, stats<tag::mean, tag::lazy_variance>> deltaAcc;

    auto rayDeltaFunc = [begin, &deltaAcc](int n) {
        deltaAcc(distance_between_points(*(begin + n - 1), *(begin + n)));
    };
    auto rayBegin     = boost::make_counting_iterator<std::size_t>(1);
    auto rayEnd       = boost::make_counting_iterator<std::size_t>(std::distance(begin, end));

    std::for_each(rayBegin, rayEnd, rayDeltaFunc);

    scalars[kDeltaAvg]       = mean(deltaAcc);
    scalars[kDeltaStd]       = std::sqrt(variance(deltaAcc));
    scalars[kDeltaVariation] = (scalars[kDeltaAvg] > 0.0) ? scalars[kDeltaStd] / scalars[kDeltaAvg] : 0.0;
}

void Isovist::calculatePolygonScalars(PointIter begin, PointIter end, std::vector<double>& scalars)
{
    scalars[kArea]        = area(begin, end);
    scalars[kPerimeter]   = perimeter(begin, end);
    scalars[kCircularity] = circularity(scalars[kArea], scalars[kPerimeter]);

    if((scalars[kArea] > 0.0) && (scalars[kPerimeter] > 0.0))
    {
        auto shapeFeatures = math::shape_features(begin, end, scalars[kArea], position_);
        scalars[kWeightedOrientation] = shapeFeatures[math::kShapeWeightedOrientation];
        scalars[kShapeEccentricity] = shapeFeatures[math::kShapeEccentricity];
        scalars[kOrientation] = shapeFeatures[math::kShapeOrientation];
        scalars[kShapeCompactness] = shapeFeatures[math::kShapeCompactness];

        auto hull = math::convex_hull<float>(begin, end);
        scalars[kShapeWaviness] = hull.perimeter() / scalars[kPerimeter];
    }
    else
    {
        scalars[kWeightedOrientation] = 0.0;
        scalars[kShapeEccentricity] = 0.0;
        scalars[kOrientation] = 0.0;
        scalars[kShapeCompactness] = 0.0;
        scalars[kShapeWaviness] = 0.0;
    }
}

void Isovist::calculateMomentsScalars(PointIter begin, PointIter end, std::vector<double>& scalars)
{
    if(scalars[kArea] > 0.0)
    {
        auto zernikeFeatures = zernike.moments<float>(begin,
                                                      end,
                                                      position_,
                                                      scalars[kDmax] * 3);
        std::copy(zernikeFeatures.begin(), zernikeFeatures.end(), scalars.begin() + kNumScalars);
    }
    else
    {
        std::fill(scalars.begin() + kNumScalars, scalars.end(), 0.0);
    }
}

void Isovist::calculateDerivs(std::vector<Point<float>>& endpoints)
{
    // Find the two halves separated by the min dist line. Store those at the end of endpoints. Compute the
    // scalars for each side -- except Zernike. The derivative of each can be easily found with abs of difference.

    std::size_t numPoints = endpoints.size();
    double minDist = HUGE_VAL;
    int minIdx = 0;

    int otherSide = numPoints / 2;
    for(int n = 0, end = otherSide; n < end; ++n)
    {
        double dist = squared_point_distance(endpoints[n], endpoints[n + otherSide]);
        if(dist < minDist)
        {
            minDist = dist;
            minIdx = n;
        }
    }

    scalars_[kMinLineDist] = std::sqrt(minDist);
    scalars_[kMinLineNormal] = angle_sum(angle_to_point(endpoints[minIdx],
                                                                    endpoints[minIdx + otherSide]), M_PI_2);

    // Copy the derivative points on to the end of endpoints. Want to continuous regions, so do the [n,n+otherSide)
    // then [n+otherSide,end) [0,n)
    // add one to num points because the ranges need to wrap around
    endpoints.resize((numPoints + 1) * 2);    // resize to ensure the iterators don't change
    auto halfIt = std::copy(endpoints.begin() + minIdx, endpoints.begin() + minIdx + otherSide, endpoints.begin() + numPoints);
    *halfIt++ = *(endpoints.begin() + numPoints);
    auto endIt = std::copy(endpoints.begin() + minIdx + otherSide, endpoints.begin() + numPoints, halfIt);
    std::copy(endpoints.begin(), endpoints.begin() + minIdx, endIt);
    endpoints.back() = *halfIt;

    std::vector<double> halfScalars(kNumScalars);
    std::vector<double> otherHalfScalars(kNumScalars);
    scalarDerivs_.resize(kNumScalars);

    calculateDistanceScalars(endpoints.begin() + numPoints, halfIt, halfScalars);
    calculateDeltaScalars(endpoints.begin() + numPoints, halfIt, halfScalars);
    calculatePolygonScalars(endpoints.begin() + numPoints, halfIt, halfScalars);
    calculateDistanceScalars(halfIt, endpoints.end(), otherHalfScalars);
    calculateDeltaScalars(halfIt, endpoints.end(), otherHalfScalars);
    calculatePolygonScalars(halfIt, endpoints.end(), otherHalfScalars);

    for(int n = 0; n < kNumScalars; ++n)
    {
        scalarDerivs_[n] = std::abs(halfScalars[n] - otherHalfScalars[n]);
    }

    // Handle wraparound for the angles
    scalarDerivs_[kOrientation] = angle_diff_abs_pi_2(halfScalars[kOrientation], otherHalfScalars[kOrientation]);
    scalarDerivs_[kWeightedOrientation] = angle_diff_abs_pi_2(halfScalars[kWeightedOrientation],
                                                                    otherHalfScalars[kWeightedOrientation]);
    scalarDerivs_[kMinDistOrientation] = angle_diff_abs(halfScalars[kMinDistOrientation],
                                                              otherHalfScalars[kMinDistOrientation]);
    scalarDerivs_[kMaxThroughDistOrientation] = angle_diff_abs(halfScalars[kMaxThroughDistOrientation],
                                                                     otherHalfScalars[kMaxThroughDistOrientation]);

    // Assign features based on the half-isovists
    scalars_[kMinHalfArea] = std::min(halfScalars[kArea], otherHalfScalars[kArea]);
    if(scalars_[kMinHalfArea] > 0.0)
    {
        scalars_[kAreaBalance] = scalars_[kMinHalfArea] / std::max(halfScalars[kArea], otherHalfScalars[kArea]);
    }
    else
    {
        scalars_[kAreaBalance] = 1.0;
    }
}

double area(Isovist::Iter begin, Isovist::Iter end)
{
    double sum = accumulate_pair(begin, end, [](const Point<double>& lhs, const Point<double>& rhs) {
        return lhs.x*rhs.y - lhs.y*rhs.x;
    });
    return std::abs(sum / 2.0);
}

double perimeter(Isovist::Iter begin, Isovist::Iter end)
{
    return accumulate_pair(begin, end, [](const Point<double>& lhs, const Point<double>& rhs) {
        return distance_between_points(lhs, rhs);
    });
}

double circularity(double area, double perimeter)
{
    return (perimeter > 0.0) ? (4.0*M_PI*area) / (perimeter*perimeter) : 1.0;
}

template <class BinaryOp>
double accumulate_pair(Isovist::Iter begin, Isovist::Iter end, BinaryOp op)
{
    if(begin == end)
    {
        return 0.0;
    }

    double sum = 0.0;

    for(auto first = begin, second = begin+1; second < end; ++first, ++second)
    {
        sum += op(*first, *second);
    }

    return sum;
}

} // namespace utils
} // namespace vulcan


// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmConfig.h"

// #define GLM_ENABLE_EXPERIMENTAL
#include <glm/gtx/norm.hpp>
#include <glm/mat3x3.hpp>
#include <glm/vec3.hpp>

// Must come after glm includes due to usage on GPU.
#include "CovarianceVoxel.h"

#include "NdtMap.h"
#include "OccupancyMap.h"
#include "Voxel.h"
#include "VoxelMean.h"
#include "VoxelOccupancy.h"

#include <glm/gtc/type_ptr.hpp>
#include <glm/gtx/matrix_factorisation.hpp>

#ifdef OHM_WITH_EIGEN
#include <Eigen/Dense>
#endif  // OHM_WITH_EIGEN

#include <3esservermacros.h>
#ifdef TES_ENABLE
#include <3esserver.h>
#include <shapes/3esshapes.h>
#endif  // TES_ENABLE

#include <array>

namespace ohm
{
#if OHM_COV_DEBUG
namespace
{
unsigned max_iterations = 0;
double max_error = 0;
}  // namespace
#endif  // OHM_COV_DEBUG

namespace
{
#ifdef OHM_WITH_EIGEN
void covarianceEigenDecompositionEigen(const CovarianceVoxel *cov, glm::dmat3 *eigenvectors, glm::dvec3 *eigenvalues)
{
  // This has been noted to be ~3x faster than the GLM iterative version.
  const glm::dmat3 cov_mat = covarianceMatrix(cov);
  // Both GLM and Eigen are column major. Direct mapping is fine.
  const auto cov_eigen = Eigen::Matrix3d::ConstMapType(glm::value_ptr(cov_mat), 3, 3);

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(cov_eigen);
  Eigen::Vector3d evals = Eigen::Vector3d::Ones();
  Eigen::Matrix3d evecs = Eigen::Matrix3d::Identity();

  if (eigensolver.info() == Eigen::Success)
  {
    evals = eigensolver.eigenvalues();
    evecs = eigensolver.eigenvectors();
  }
  const double det = evecs.determinant();
  if (det < 0.0)
  {
    evecs.col(0) = -evecs.col(0);  // must be valid rotation matrix (determinant=+1)
  }
  else if (det == 0.0)
  {
    evecs = Eigen::Matrix3d::Identity();
  }

  *eigenvalues = glm::dvec3(evals[0], evals[1], evals[2]);

  for (int r = 0; r < 3; ++r)
  {
    for (int c = 0; c < 3; ++c)
    {
      (*eigenvectors)[c][r] = evecs(r, c);
    }
  }
}
#else  // OHM_WITH_EIGEN
void covarianceEigenDecompositionGlm(const CovarianceVoxel *cov, glm::dmat3 *eigenvectors, glm::dvec3 *eigenvalues)
{
  // This has been noted to be ~3x slower than the Eigen solver.
  glm::dmat3 mat = covarianceMatrix(cov);

  *eigenvectors = glm::dmat3(1.0);  // Identity initialisation

  // Use QR decomposition to implement the QR algorithm.
  // For this algorithm, we iterate as follows:
  //
  // P = cov * cov^T  // cov by cov transpose
  // for i = 1 to N
  //   Q, R = qr_decomposition(P)
  //   P = R * Q
  // end
  //
  // The eigenvalues converge in the diagonal of R.
  // Meanwhile the eigenvectors are found by the product all the Q matrices
  //
  // We set a hard iteration limit, but we also check for convergence with last iteration and may early out.
  const unsigned iteration_limit = 20;
  glm::dmat3 q, r;
  glm::dvec3 eigenvalues_last, eigenvalues_current(0);
  const glm::dvec3 delta_threshold(1e-9);
  for (unsigned i = 0; i < iteration_limit; ++i)
  {
    eigenvalues_last = eigenvalues_current;
#if OHM_COV_DEBUG
    max_iterations = std::max(max_iterations, i + 1);
#endif  // OHM_COV_DEBUG

    glm::qr_decompose(mat, q, r);
    // Progressively refine the eigenvectors.
    *eigenvectors = *eigenvectors * q;
    // Update eigenvalues and check for convergence
    eigenvalues_current[0] = r[0][0];
    eigenvalues_current[1] = r[1][1];
    eigenvalues_current[2] = r[2][2];

    mat = r * q;

    const glm::dvec3 eval_delta = glm::abs(eigenvalues_current - eigenvalues_last);
    if (glm::all(glm::lessThanEqual(eval_delta, delta_threshold)))
    {
      break;
    }
  }

#if OHM_COV_DEBUG
  const glm::dvec3 eval_delta = glm::abs(eigenvalues_current - eigenvalues_last);
  max_error = std::max(eval_delta.x, max_error);
  max_error = std::max(eval_delta.y, max_error);
  max_error = std::max(eval_delta.z, max_error);
#endif  // OHM_COV_DEBUG

  *eigenvalues = eigenvalues_current;
}
#endif  // OHM_WITH_EIGEN
}  // namespace

void covarianceEigenDecomposition(const CovarianceVoxel *cov, glm::dmat3 *eigenvectors, glm::dvec3 *eigenvalues)
{
#ifdef OHM_WITH_EIGEN
  covarianceEigenDecompositionEigen(cov, eigenvectors, eigenvalues);
#else   // OHM_WITH_EIGEN
  covarianceEigenDecompositionGlm(cov, eigenvectors, eigenvalues);
#endif  // OHM_WITH_EIGEN
}

void covarianceEstimatePrimaryNormal(const CovarianceVoxel *cov, glm::dvec3 *normal, int preferred_axis)
{
  glm::dmat3 eigenvectors;
  glm::dvec3 eigenvalues;

  covarianceEigenDecomposition(cov, &eigenvectors, &eigenvalues);

  int smallest_eval_index = preferred_axis;
  for (int i = 0; i < 3; ++i)
  {
    if (eigenvalues[i] < eigenvalues[smallest_eval_index])
    {
      smallest_eval_index = i;
    }
  }

  const double length2 = glm::length2(eigenvectors[smallest_eval_index]);
  // const double confidence = 1.0 - length2;
  *normal = (length2 > 0) ? eigenvectors[smallest_eval_index] / glm::sqrt(length2) : eigenvectors[smallest_eval_index];
  // return confidence;
}

bool covarianceUnitSphereTransformation(const CovarianceVoxel *cov, glm::dquat *rotation, glm::dvec3 *scale)
{
  glm::dmat3 eigenvectors;
  glm::dvec3 eigenvalues;

  covarianceEigenDecomposition(cov, &eigenvectors, &eigenvalues);

  const double det = glm::determinant(eigenvectors);
  if (det < 0.0)
  {
    eigenvectors[0] = -eigenvectors[0];  // must be valid rotation matrix (determinant=+1)
  }
  else if (det == 0.0)
  {
    eigenvectors = glm::dmat3(1.0);
  }

  *rotation = glm::dquat(eigenvectors);
  for (int i = 0; i < 3; ++i)
  {
    const double eval = std::abs(eigenvalues[i]);  // abs just in case.
    const double epsilon = 1e-9;
    (*scale)[i] = (eval > epsilon) ? std::sqrt(eval) : eval;
  }

  return true;
}

#if OHM_COV_DEBUG
#include <iostream>
void covDebugStats()
{
  std::cout << "QR algorithm max iterations: " << max_iterations << std::endl;
  std::cout << "QR algorithm max error: " << max_error << std::endl;
}
#endif  // OHM_COV_DEBUG

void integrateNdtHit(NdtMap &map, const Key &key, const glm::dvec3 &sensor, const glm::dvec3 &sample, bool ndt_tm,
                     const float sample_intensity)
{
  OccupancyMap &occupancy_map = map.map();
  Voxel<float> occupancy_voxel(&occupancy_map, occupancy_map.layout().occupancyLayer(), key);
  Voxel<VoxelMean> mean_voxel(occupancy_voxel, occupancy_map.layout().meanLayer());
  Voxel<CovarianceVoxel> cov_voxel(occupancy_voxel, occupancy_map.layout().covarianceLayer());
  const glm::dvec3 voxel_centre = occupancy_map.voxelCentreGlobal(key);

  assert(occupancy_voxel.isValid());
  assert(mean_voxel.isValid());
  assert(cov_voxel.isValid());

  // Keep clang analysis happy
#ifdef __clang_analyzer__
  if (!occupancy_voxel.voxelMemory() || !mean_voxel.voxelMemory() || !cov_voxel.voxelMemory())
  {
    return;
  }
#endif  // __clang_analyzer__

  // NDT-OM
  CovarianceVoxel cov;
  VoxelMean mean;
  float occupancy;
  occupancy_voxel.read(&occupancy);
  mean_voxel.read(&mean);
  cov_voxel.read(&cov);

  float updated_value = occupancy;
  const glm::dvec3 voxel_pos = position(mean, voxel_centre, occupancy_map.resolution());

  // NDT-TM
  if (ndt_tm)
  {
    Voxel<IntensityMeanCov> intensity_voxel(&occupancy_map, occupancy_map.layout().intensityLayer(), key);
    Voxel<HitMissCount> hit_miss_count_voxel(&occupancy_map, occupancy_map.layout().hitMissCountLayer(), key);

    if (intensity_voxel.isLayerValid() && hit_miss_count_voxel.isLayerValid())
    {
      IntensityMeanCov intensity;
      HitMissCount hit_miss_count;
      intensity_voxel.read(&intensity);
      hit_miss_count_voxel.read(&hit_miss_count);

      const bool reinitialise_permeability_with_covariance = true;  // TODO: make a parameter of map
      calculateHitMissUpdateOnHit(&cov, updated_value, &hit_miss_count, sensor, sample, voxel_pos, mean.count,
                                  unobservedOccupancyValue(), reinitialise_permeability_with_covariance,
                                  map.adaptationRate(), map.sensorNoise(), map.reinitialiseCovarianceThreshold(),
                                  map.reinitialiseCovariancePointCount(), map.ndtSampleThreshold());

      hit_miss_count_voxel.write(hit_miss_count);

      calculateIntensityUpdateOnHit(&intensity, updated_value, sample_intensity, map.initialIntensityCovariance(),
                                    mean.count, map.reinitialiseCovarianceThreshold(),
                                    map.reinitialiseCovariancePointCount());

      intensity_voxel.write(intensity);
    }
  }

  if (calculateHitWithCovariance(&cov, &updated_value, sample, voxel_pos, mean.count, occupancy_map.hitValue(),
                                 unobservedOccupancyValue(), float(occupancy_map.resolution()),
                                 map.reinitialiseCovarianceThreshold(), map.reinitialiseCovariancePointCount()))
  {
    // Covariance matrix has reset. Reset the point count to clear the mean value.
    mean.count = 0;
  }

  // Ensure we update the occupancy within the configured map limits.
  occupancyAdjustUp(
    &occupancy, occupancy, updated_value, unobservedOccupancyValue(), occupancy_map.maxVoxelValue(),
    occupancy_map.saturateAtMinValue() ? occupancy_map.minVoxelValue() : std::numeric_limits<float>::lowest(),
    occupancy_map.saturateAtMaxValue() ? occupancy_map.maxVoxelValue() : std::numeric_limits<float>::max(), false);
  occupancy_voxel.write(occupancy);

  // Update the voxel mean.
  updatePosition(&mean, sample, voxel_centre, occupancy_map.resolution());
  mean_voxel.write(mean);
  cov_voxel.write(cov);
}

void integrateNdtMiss(NdtMap &map, const Key &key, const glm::dvec3 &sensor, const glm::dvec3 &sample, bool ndt_tm)
{
  OccupancyMap &occupancy_map = map.map();
  Voxel<float> occupancy_voxel(&occupancy_map, occupancy_map.layout().occupancyLayer(), key);
  Voxel<const VoxelMean> mean_voxel(occupancy_voxel, occupancy_map.layout().meanLayer());
  Voxel<const CovarianceVoxel> cov_voxel(occupancy_voxel, occupancy_map.layout().covarianceLayer());
  const glm::dvec3 voxel_centre = occupancy_map.voxelCentreGlobal(key);

  assert(occupancy_voxel.isValid());
  assert(mean_voxel.isValid());
  assert(cov_voxel.isValid());

  // Keep clang analysis happy
#ifdef __clang_analyzer__
  if (!occupancy_voxel.voxelMemory() || !mean_voxel.voxelMemory() || !cov_voxel.voxelMemory())
  {
    return;
  }
#endif  // __clang_analyzer__

  // NDT-OM
  CovarianceVoxel cov;
  VoxelMean mean;
  float occupancy;
  occupancy_voxel.read(&occupancy);
  mean_voxel.read(&mean);
  cov_voxel.read(&cov);

  float updated_value = occupancy;
#ifdef TES_ENABLE
  const float initial_value = occupancy;
  glm::dvec3 voxel_maximum_likelihood;
#endif  // TES_ENABLE
  const glm::dvec3 voxel_mean = position(mean, voxel_centre, occupancy_map.resolution());
  bool confirm_miss = false;
  calculateMissNdt(&cov, &updated_value, &confirm_miss, sensor, sample, voxel_mean, mean.count,
                   unobservedOccupancyValue(), occupancy_map.missValue(), map.adaptationRate(), map.sensorNoise(),
                   map.ndtSampleThreshold());

  if (ndt_tm && confirm_miss)
  {
    Voxel<HitMissCount> hit_miss_count_voxel(&occupancy_map, occupancy_map.layout().hitMissCountLayer(), key);
    if (hit_miss_count_voxel.isLayerValid())
    {
      HitMissCount hit_miss_count;
      hit_miss_count_voxel.read(&hit_miss_count);
      ++hit_miss_count.miss_count;
      hit_miss_count_voxel.write(hit_miss_count);
    }
  }

  occupancyAdjustDown(
    &occupancy, occupancy, updated_value, unobservedOccupancyValue(), occupancy_map.minVoxelValue(),
    occupancy_map.saturateAtMinValue() ? occupancy_map.minVoxelValue() : std::numeric_limits<float>::lowest(),
    occupancy_map.saturateAtMaxValue() ? occupancy_map.maxVoxelValue() : std::numeric_limits<float>::max(), false);
  occupancy_voxel.write(occupancy);

#ifdef TES_ENABLE
  if (map.trace())
  {
    const glm::dvec3 voxel_centre = occupancy_map.voxelCentreGlobal(key);
    TES_BOX_W(g_tes, TES_COLOUR(OrangeRed), tes::Id(cov_voxel.voxelMemory()),
              tes::Transform(glm::value_ptr(voxel_centre), glm::value_ptr(glm::dvec3(occupancy_map.resolution()))));
    TES_SERVER_UPDATE(g_tes, 0.0f);

    bool drew_surfel = false;
    glm::dquat rot;
    glm::dvec3 scale;
    if (covarianceUnitSphereTransformation(&cov, &rot, &scale))
    {
      TES_SPHERE(g_tes, TES_COLOUR(SeaGreen), tes::Id(cov_voxel.voxelMemory()),
                 tes::Transform(tes::Vector3d(glm::value_ptr(voxel_mean)), tes::Quaterniond(rot.x, rot.y, rot.z, rot.w),
                                tes::Vector3d(glm::value_ptr(scale))));
      drew_surfel = true;
    }

    // Trace the voxel mean, maximum likelihood point and the ellipsoid.
    // Mean
    const float mean_pos_radius = 0.05f;
    const float likely_pos_radius = 0.1f;
    TES_SPHERE(g_tes, TES_COLOUR(OrangeRed), tes::Id(&voxel_mean),
               tes::Spherical(glm::value_ptr(voxel_mean), mean_pos_radius));
    // Maximum likelihood
    TES_SPHERE_W(g_tes, TES_COLOUR(PowderBlue), tes::Id(&voxel_maximum_likelihood),
                 tes::Spherical(glm::value_ptr(voxel_maximum_likelihood), likely_pos_radius));

    std::array<char, 64> text;  // NOLINT(readability-magic-numbers)
    text[0] = '\0';
    sprintf(text.data(), "P %.3f", valueToProbability(occupancy - initial_value));
    TES_TEXT2D_WORLD(g_tes, TES_COLOUR(White), text.data(), tes::Id(),
                     tes::Spherical(tes::Vector3d(glm::value_ptr(voxel_centre))));

    TES_SERVER_UPDATE(g_tes, 0.0f);
    TES_BOX_END(g_tes, tes::Id(cov_voxel.voxelMemory()));
    TES_SPHERE_END(g_tes, tes::Id(&voxel_mean));
    TES_SPHERE_END(g_tes, tes::Id(&voxel_maximum_likelihood));
    if (drew_surfel)
    {
      TES_SPHERE_END(g_tes, tes::Id(cov_voxel.voxelMemory()));
    }
  }
#endif  // TES_ENABLE
}
}  // namespace ohm


#include "cnn/saxe-init.h"
#include "cnn/tensor.h"

#include <random>
#include <cstring>

#include <Eigen/SVD>

using namespace std;

namespace cnn {

inline Eigen::MatrixXf EigenRandomNormal(int dim, real mean, real stddev) {
  normal_distribution<real> distribution(mean, stddev);
  auto b = [&] (real) {return distribution(*rndeng);};
  Eigen::MatrixXf r = Eigen::MatrixXf::NullaryExpr(dim, dim, b);
  return r;
}

void OrthonormalRandom(int dim, real g, Tensor& x) {
  Eigen::MatrixXf m = EigenRandomNormal(dim, 0.0, 0.01);
  Eigen::JacobiSVD<Eigen::MatrixXf> svd(m, Eigen::ComputeFullU);
  *x = svd.matrixU();
}

}


/**
 * @file GINS.hpp
 * @author LauZanMo (LauZanMo@whu.edu.cn)
 * @brief GINS class
 * @version 1.0
 * @date 2021-07-14
 *
 * @copyright Copyright (c) 2021 WHU-Drones
 *
 */
#pragma once

#include <Eigen/Geometry>
#include <iostream>

#include "DataStorage.hpp"
#include "INSMechanization.hpp"
#include "Utils.hpp"

namespace iNav {

using ErrorType = Eigen::Matrix<double, 21, 1>;
using qType = Eigen::Matrix<double, 18, 18>;
using GType = Eigen::Matrix<double, 21, 18>;
using HType = Eigen::Matrix<double, 3, 21>;
using KType = Eigen::Matrix<double, 21, 3>;
using Matrix21d = Eigen::Matrix<double, 21, 21>;

class GINS : INS::INSMechanization {
public:
  GINS(const iNav::NavData& init_nav_data, const IMUParam& init_imu_param,
       const Eigen::Vector3d& l_b, const iNav::IMUData& init_imu_data);
  ~GINS();

  NavData Mechanization(IMUData data, bool record_data = true);
  void Prediction(bool record_data = true);
  NavData GNSSUpdate(GnssData gnss_data, IMUData imu_data);

private:
  Eigen::Vector3d gyro_bias_, acc_bias_, gyro_scalar_, acc_scalar_;
  Eigen::Vector3d l_b_;
  ErrorType delta_x_;
  qType q_;
  GType G_, last_G_;
  Matrix21d P_, Phi_, Q_;
  IMUParam IMU_param_;

  inline Eigen::Vector3d CorrectGyroError(const Eigen::Vector3d& gyro_raw) {
    return Eigen::Vector3d((gyro_raw - gyro_bias_ * delta_time_).array() /
                           (1 + gyro_scalar_.array()));
  }
  inline Eigen::Vector3d CorrectAccError(const Eigen::Vector3d& acc_raw) {
    return Eigen::Vector3d((acc_raw - acc_bias_ * delta_time_).array() /
                           (1 + acc_scalar_.array()));
  }

  Matrix21d SetP(const Eigen::Vector3d& position_std,
                 const Eigen::Vector3d& velocity_std,
                 const Eigen::Vector3d& theta_std, const IMUParam& param);
  qType Setq(const IMUParam& param);
  Matrix21d ComputeF();
  GType ComputeG(const Eigen::Quaterniond& q_b_to_n);
  HType ComputeHr();
  NavData NavDataInterpolation(const NavData& last_nav_data,
                               const NavData& nav_data, const double& ratio,
                               const double& timestamp);
  NavData CorrectNavDataAndIMUError(const NavData& data, ErrorType& error);
  Eigen::Vector3d ComputeZr(const GnssData& gnss_data, const NavData& nav_data);
};

}  // namespace iNav


/*
 * MA.cpp
 *
 *  Created on: 2015年2月16日
 *      Author: fasiondog
 */

#include <boost/algorithm/string.hpp>
#include "MA.h"
#include "SMA.h"
#include "EMA.h"
#include "AMA.h"

namespace hku {

Indicator HKU_API MA(const Indicator& data, int n, const string& type) {
    string str_type(type);
    boost::to_upper(str_type);
    if (str_type == "SMA") {
        return SMA(data, n);
    } else if (str_type == "EMA") {
        return EMA(data, n);
    } else if (str_type == "AMA") {
        return AMA(data, n);
    } else {
        return SMA(data, n);
    }
}

Indicator HKU_API MA(int n, const string& type) {
    string str_type(type);
    boost::to_upper(str_type);
    if (str_type == "SMA") {
        return SMA(n);
    } else if (str_type == "EMA") {
        return EMA(n);
    } else if (str_type == "AMA") {
        return AMA(n);
    } else {
        return SMA(n);
    }
}

} /* namespae hku */


/* =========================================================================
   Copyright (c) 2010-2016, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/*
*   Benchmark:  Sparse matrix operations, i.e. matrix-vector products (sparse.cpp and sparse.cu are identical, the latter being required for compilation using CUDA nvcc)
*
*/

//#define VIENNACL_BUILD_INFO
#ifndef NDEBUG
 #define NDEBUG
#endif

#define VIENNACL_WITH_UBLAS 1

#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>
#include <boost/numeric/ublas/lu.hpp>

#include "viennacl/scalar.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/coordinate_matrix.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/ell_matrix.hpp"
#include "viennacl/hyb_matrix.hpp"
#include "viennacl/sliced_ell_matrix.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/linalg/norm_2.hpp"
#include "viennacl/io/matrix_market.hpp"
#include "viennacl/linalg/ilu.hpp"
#include "viennacl/tools/timer.hpp"

#include <iostream>
#include <vector>

#define BENCHMARK_RUNS          10

inline void printOps(double num_ops, double exec_time)
{
  std::cout << "GFLOPs: " << num_ops / (1000000 * exec_time * 1000) << std::endl;
}

template<typename ScalarType>
int run_benchmark()
{
  viennacl::tools::timer timer;
  double exec_time;

  ScalarType std_factor1 = ScalarType(3.1415);
  ScalarType std_factor2 = ScalarType(42.0);
  viennacl::scalar<ScalarType> vcl_factor1(std_factor1);
  viennacl::scalar<ScalarType> vcl_factor2(std_factor2);

  boost::numeric::ublas::vector<ScalarType> ublas_vec1;
  boost::numeric::ublas::vector<ScalarType> ublas_vec2;

  boost::numeric::ublas::compressed_matrix<ScalarType> ublas_matrix;
  if (!viennacl::io::read_matrix_market_file(ublas_matrix, "../examples/testdata/mat65k.mtx"))
  {
    std::cout << "Error reading Matrix file" << std::endl;
    return 0;
  }
  //unsigned int cg_mat_size = cg_mat.size();
  std::cout << "done reading matrix" << std::endl;

  ublas_vec1 = boost::numeric::ublas::scalar_vector<ScalarType>(ublas_matrix.size1(), ScalarType(1.0));
  ublas_vec2 = ublas_vec1;

  viennacl::compressed_matrix<ScalarType, 1> vcl_compressed_matrix_1;
  viennacl::compressed_matrix<ScalarType, 4> vcl_compressed_matrix_4;
  viennacl::compressed_matrix<ScalarType, 8> vcl_compressed_matrix_8;

  viennacl::coordinate_matrix<ScalarType> vcl_coordinate_matrix_128;

  viennacl::ell_matrix<ScalarType, 1> vcl_ell_matrix_1;
  viennacl::hyb_matrix<ScalarType, 1> vcl_hyb_matrix_1;
  viennacl::sliced_ell_matrix<ScalarType> vcl_sliced_ell_matrix_1;

  viennacl::vector<ScalarType> vcl_vec1(ublas_vec1.size());
  viennacl::vector<ScalarType> vcl_vec2(ublas_vec1.size());

  //cpu to gpu:
  viennacl::copy(ublas_matrix, vcl_compressed_matrix_1);
  #ifndef VIENNACL_EXPERIMENTAL_DOUBLE_PRECISION_WITH_STREAM_SDK_ON_GPU
  viennacl::copy(ublas_matrix, vcl_compressed_matrix_4);
  viennacl::copy(ublas_matrix, vcl_compressed_matrix_8);
  #endif
  viennacl::copy(ublas_matrix, vcl_coordinate_matrix_128);
  viennacl::copy(ublas_matrix, vcl_ell_matrix_1);
  viennacl::copy(ublas_matrix, vcl_hyb_matrix_1);
  viennacl::copy(ublas_matrix, vcl_sliced_ell_matrix_1);
  viennacl::copy(ublas_vec1, vcl_vec1);
  viennacl::copy(ublas_vec2, vcl_vec2);

  ///////////// Matrix operations /////////////////

  std::cout << "------- Matrix-Vector product on CPU ----------" << std::endl;
  timer.start();
  for (int runs=0; runs<BENCHMARK_RUNS; ++runs)
  {
    //ublas_vec1 = boost::numeric::ublas::prod(ublas_matrix, ublas_vec2);
    boost::numeric::ublas::axpy_prod(ublas_matrix, ublas_vec2, ublas_vec1, true);
  }
  exec_time = timer.get();
  std::cout << "CPU time: " << exec_time << std::endl;
  std::cout << "CPU "; printOps(2.0 * static_cast<double>(ublas_matrix.nnz()), static_cast<double>(exec_time) / static_cast<double>(BENCHMARK_RUNS));
  std::cout << ublas_vec1[0] << std::endl;

  std::cout << "------- Matrix-Vector product with compressed_matrix ----------" << std::endl;

  vcl_vec1 = viennacl::linalg::prod(vcl_compressed_matrix_1, vcl_vec2); //startup calculation
  vcl_vec1 = viennacl::linalg::prod(vcl_compressed_matrix_4, vcl_vec2); //startup calculation
  vcl_vec1 = viennacl::linalg::prod(vcl_compressed_matrix_8, vcl_vec2); //startup calculation
  //std_result = 0.0;

  viennacl::backend::finish();
  timer.start();
  for (int runs=0; runs<BENCHMARK_RUNS; ++runs)
  {
    vcl_vec1 = viennacl::linalg::prod(vcl_compressed_matrix_1, vcl_vec2);
  }
  viennacl::backend::finish();
  exec_time = timer.get();
  std::cout << "GPU time align1: " << exec_time << std::endl;
  std::cout << "GPU align1 "; printOps(2.0 * static_cast<double>(ublas_matrix.nnz()), static_cast<double>(exec_time) / static_cast<double>(BENCHMARK_RUNS));
  std::cout << vcl_vec1[0] << std::endl;

  std::cout << "Testing triangular solves: compressed_matrix" << std::endl;

  viennacl::copy(ublas_vec1, vcl_vec1);
  viennacl::linalg::inplace_solve(trans(vcl_compressed_matrix_1), vcl_vec1, viennacl::linalg::unit_lower_tag());
  viennacl::copy(ublas_vec1, vcl_vec1);
  std::cout << "ublas..." << std::endl;
  timer.start();
  boost::numeric::ublas::inplace_solve(trans(ublas_matrix), ublas_vec1, boost::numeric::ublas::unit_lower_tag());
  std::cout << "Time elapsed: " << timer.get() << std::endl;
  std::cout << "ViennaCL..." << std::endl;
  viennacl::backend::finish();
  timer.start();
  viennacl::linalg::inplace_solve(trans(vcl_compressed_matrix_1), vcl_vec1, viennacl::linalg::unit_lower_tag());
  viennacl::backend::finish();
  std::cout << "Time elapsed: " << timer.get() << std::endl;

  ublas_vec1 = boost::numeric::ublas::prod(ublas_matrix, ublas_vec2);

  viennacl::backend::finish();
  timer.start();
  for (int runs=0; runs<BENCHMARK_RUNS; ++runs)
  {
    vcl_vec1 = viennacl::linalg::prod(vcl_compressed_matrix_4, vcl_vec2);
  }
  viennacl::backend::finish();
  exec_time = timer.get();
  std::cout << "GPU time align4: " << exec_time << std::endl;
  std::cout << "GPU align4 "; printOps(2.0 * static_cast<double>(ublas_matrix.nnz()), static_cast<double>(exec_time) / static_cast<double>(BENCHMARK_RUNS));
  std::cout << vcl_vec1[0] << std::endl;

  viennacl::backend::finish();
  timer.start();
  for (int runs=0; runs<BENCHMARK_RUNS; ++runs)
  {
    vcl_vec1 = viennacl::linalg::prod(vcl_compressed_matrix_8, vcl_vec2);
  }
  viennacl::backend::finish();
  exec_time = timer.get();
  std::cout << "GPU time align8: " << exec_time << std::endl;
  std::cout << "GPU align8 "; printOps(2.0 * static_cast<double>(ublas_matrix.nnz()), static_cast<double>(exec_time) / static_cast<double>(BENCHMARK_RUNS));
  std::cout << vcl_vec1[0] << std::endl;

  std::cout << "------- Matrix-Vector product with coordinate_matrix ----------" << std::endl;
  vcl_vec1 = viennacl::linalg::prod(vcl_coordinate_matrix_128, vcl_vec2); //startup calculation
  viennacl::backend::finish();

  viennacl::copy(vcl_vec1, ublas_vec2);
  long err_cnt = 0;
  for (std::size_t i=0; i<ublas_vec1.size(); ++i)
  {
    if ( fabs(ublas_vec1[i] - ublas_vec2[i]) / std::max(fabs(ublas_vec1[i]), fabs(ublas_vec2[i])) > 1e-2)
    {
      std::cout << "Error at index " << i << ": Should: " << ublas_vec1[i] << ", Is: " << ublas_vec2[i] << std::endl;
      ++err_cnt;
      if (err_cnt > 5)
        break;
    }
  }

  viennacl::backend::finish();
  timer.start();
  for (int runs=0; runs<BENCHMARK_RUNS; ++runs)
  {
    vcl_vec1 = viennacl::linalg::prod(vcl_coordinate_matrix_128, vcl_vec2);
  }
  viennacl::backend::finish();
  exec_time = timer.get();
  std::cout << "GPU time: " << exec_time << std::endl;
  std::cout << "GPU "; printOps(2.0 * static_cast<double>(ublas_matrix.nnz()), static_cast<double>(exec_time) / static_cast<double>(BENCHMARK_RUNS));
  std::cout << vcl_vec1[0] << std::endl;

  std::cout << "------- Matrix-Vector product with ell_matrix ----------" << std::endl;
  vcl_vec1 = viennacl::linalg::prod(vcl_ell_matrix_1, vcl_vec2); //startup calculation
  viennacl::backend::finish();

  viennacl::copy(vcl_vec1, ublas_vec2);
  err_cnt = 0;
  for (std::size_t i=0; i<ublas_vec1.size(); ++i)
  {
    if ( fabs(ublas_vec1[i] - ublas_vec2[i]) / std::max(fabs(ublas_vec1[i]), fabs(ublas_vec2[i])) > 1e-2)
    {
      std::cout << "Error at index " << i << ": Should: " << ublas_vec1[i] << ", Is: " << ublas_vec2[i] << std::endl;
      ++err_cnt;
      if (err_cnt > 5)
        break;
    }
  }

  viennacl::backend::finish();
  timer.start();
  for (int runs=0; runs<BENCHMARK_RUNS; ++runs)
  {
    vcl_vec1 = viennacl::linalg::prod(vcl_ell_matrix_1, vcl_vec2);
  }
  viennacl::backend::finish();
  exec_time = timer.get();
  std::cout << "GPU time: " << exec_time << std::endl;
  std::cout << "GPU "; printOps(2.0 * static_cast<double>(ublas_matrix.nnz()), static_cast<double>(exec_time) / static_cast<double>(BENCHMARK_RUNS));
  std::cout << vcl_vec1[0] << std::endl;

  std::cout << "------- Matrix-Vector product with hyb_matrix ----------" << std::endl;
  vcl_vec1 = viennacl::linalg::prod(vcl_hyb_matrix_1, vcl_vec2); //startup calculation
  viennacl::backend::finish();

  viennacl::copy(vcl_vec1, ublas_vec2);
  err_cnt = 0;
  for (std::size_t i=0; i<ublas_vec1.size(); ++i)
  {
    if ( fabs(ublas_vec1[i] - ublas_vec2[i]) / std::max(fabs(ublas_vec1[i]), fabs(ublas_vec2[i])) > 1e-2)
    {
      std::cout << "Error at index " << i << ": Should: " << ublas_vec1[i] << ", Is: " << ublas_vec2[i] << std::endl;
      ++err_cnt;
      if (err_cnt > 5)
        break;
    }
  }

  viennacl::backend::finish();
  timer.start();
  for (int runs=0; runs<BENCHMARK_RUNS; ++runs)
  {
    vcl_vec1 = viennacl::linalg::prod(vcl_hyb_matrix_1, vcl_vec2);
  }
  viennacl::backend::finish();
  exec_time = timer.get();
  std::cout << "GPU time: " << exec_time << std::endl;
  std::cout << "GPU "; printOps(2.0 * static_cast<double>(ublas_matrix.nnz()), static_cast<double>(exec_time) / static_cast<double>(BENCHMARK_RUNS));
  std::cout << vcl_vec1[0] << std::endl;

  std::cout << "------- Matrix-Vector product with sliced_ell_matrix ----------" << std::endl;
  vcl_vec1 = viennacl::linalg::prod(vcl_sliced_ell_matrix_1, vcl_vec2); //startup calculation
  viennacl::backend::finish();

  viennacl::copy(vcl_vec1, ublas_vec2);
  err_cnt = 0;
  for (std::size_t i=0; i<ublas_vec1.size(); ++i)
  {
    if ( fabs(ublas_vec1[i] - ublas_vec2[i]) / std::max(fabs(ublas_vec1[i]), fabs(ublas_vec2[i])) > 1e-2)
    {
      std::cout << "Error at index " << i << ": Should: " << ublas_vec1[i] << ", Is: " << ublas_vec2[i] << std::endl;
      ++err_cnt;
      if (err_cnt > 5)
        break;
    }
  }

  viennacl::backend::finish();
  timer.start();
  for (int runs=0; runs<BENCHMARK_RUNS; ++runs)
  {
    vcl_vec1 = viennacl::linalg::prod(vcl_sliced_ell_matrix_1, vcl_vec2);
  }
  viennacl::backend::finish();
  exec_time = timer.get();
  std::cout << "GPU time: " << exec_time << std::endl;
  std::cout << "GPU "; printOps(2.0 * static_cast<double>(ublas_matrix.nnz()), static_cast<double>(exec_time) / static_cast<double>(BENCHMARK_RUNS));
  std::cout << vcl_vec1[0] << std::endl;

  return EXIT_SUCCESS;
}

int main()
{
  std::cout << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << "               Device Info" << std::endl;
  std::cout << "----------------------------------------------" << std::endl;

#ifdef VIENNACL_WITH_OPENCL
  std::cout << viennacl::ocl::current_device().info() << std::endl;
#endif
  std::cout << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << "## Benchmark :: Sparse" << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << std::endl;
  std::cout << "   -------------------------------" << std::endl;
  std::cout << "   # benchmarking single-precision" << std::endl;
  std::cout << "   -------------------------------" << std::endl;
  run_benchmark<float>();
#ifdef VIENNACL_WITH_OPENCL
  if ( viennacl::ocl::current_device().double_support() )
#endif
  {
    std::cout << std::endl;
    std::cout << "   -------------------------------" << std::endl;
    std::cout << "   # benchmarking double-precision" << std::endl;
    std::cout << "   -------------------------------" << std::endl;
    run_benchmark<double>();
  }
  return 0;
}


#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iostream>
#include <mimkl/kernels.hpp>
#include <mimkl/linear_algebra.hpp>
#include <stdexcept>

int main(int argc, char **argv)
{
    try
    {
        Eigen::Matrix<double, 2, 3> X;
        Eigen::SparseMatrix<double> L(3, 3);
        double p;
        double c;
        Eigen::Matrix<double, 2, 2> K_reference;

        X << 1., 2., 3., 4., 5., 6.;
        mimkl::linear_algebra::fill_sparse_diagonal(L, 1.0);
        p = 1;
        c = 0;
        K_reference << 14., 32., 32., 77.;

        Eigen::Matrix<double, 2, 2> K =
        mimkl::induction::induce_polynomial_kernel<MATRIX(double)>(X, X, L, p, c);
        assert((K - K_reference).norm() == 0.0);

        return EXIT_SUCCESS;
    }
    catch (const std::exception &e)
    {
        std::cerr << e.what();
        return EXIT_FAILURE;
    }
}


#include <boost/test/unit_test.hpp>
#include <iostream>
#include <fc/crypto/rand.hpp>

#include <cmath>

static void check_randomness( const char* buffer, size_t len ) {
    if (len == 0) { return; }
    // count bit runs and 0's / 1's
    unsigned int zc = 0, oc = 0, rc = 0, last = 2;
    for (size_t k = len; k; k--) {
        char c = *buffer++;
        for (int i = 0; i < 8; i++) {
            unsigned int bit = c & 1;
            c >>= 1;
            if (bit) { oc++; } else { zc++; }
            if (bit != last) { rc++; last = bit; }
        }
    }
    BOOST_CHECK_EQUAL( 8*len, zc + oc );
    double E = 1 + (zc + oc) / 2.0;
    double variance = (E - 1) * (E - 2) / (oc + zc - 1);
    double sigma = sqrt(variance);
    std::cout << "rc :"<< rc <<"; E: "<< E <<"; sigma: " <<sigma <<"\n";
    BOOST_CHECK( rc > E - 2* sigma && rc < E + 2* sigma);
}

BOOST_AUTO_TEST_SUITE(fc_crypto)

BOOST_AUTO_TEST_CASE(rand_test)
{
    char buffer[81920];
    fc::rand_bytes( buffer, sizeof(buffer) );
    std::cout <<"\n";
    check_randomness( buffer, sizeof(buffer) );
}

BOOST_AUTO_TEST_SUITE_END()


#include "uniform_bspline_ceres.hpp"

#include <random>

#include <Eigen/Dense>
#include <ceres/ceres.h>
#include <gtest/gtest.h>

#include "test_helper.hpp"
#include "uniform_bspline_ceres.hpp"

namespace {
template <typename Spline_>
class FitCostFunctor {
public:
    static_assert(Spline_::OutputDims == 1, "Invalid number of output dims specified.");

    explicit FitCostFunctor(const ubs::UniformBSplineCeresEvaluator<Spline_>& splineEvaluator, double val)
            : splineEvaluator_(splineEvaluator), val_(val) {
    }

    template <typename T>
    bool operator()(T const* const* paramPointers, T* residual) const {
        splineEvaluator_.evaluate(paramPointers, residual);
        *residual -= T(val_);
        return true;
    }

private:
    ubs::UniformBSplineCeresEvaluator<Spline_> splineEvaluator_;
    double val_;
};

} // namespace

TEST(UniformBSplineCeres, PlaneFitPrior) { // NOLINT(readability-function-size)
    using Spline = ubs::UniformBSpline<double, 3, Eigen::Vector2d, double, Eigen::MatrixXd>;

    Eigen::MatrixXd controlPoints = Eigen::MatrixXd::Random(20, 20);
    Spline spline(controlPoints);

    // Generate measurements.
    ubs::UniformBSplineCeres<Spline> splineCeres(spline);

    std::vector<double*> parameterPointers(splineCeres.ControlPointsSupport);
    ceres::Problem problem;
    const int numMeasurements = 10;

    const Eigen::Vector3d supportPoint(1.0, 2.0, 3.0);
    const Eigen::Vector3d dir1 = Eigen::Vector3d(2.0, 0.12, 0.32).normalized();
    const Eigen::Vector3d dir2 = Eigen::Vector3d(0.1, 1.5, 1.74).normalized();

    Eigen::Matrix2d a = Eigen::Matrix2d::Zero();
    a << dir1.head<2>(), dir2.head<2>();

    const Eigen::Matrix2d aInv = a.inverse();

    // Create ceres problem.
    for (int i = 0; i < numMeasurements; ++i) {
        for (int j = 0; j < numMeasurements; ++j) {
            Eigen::Vector2d planePos{};
            planePos << i, j;
            planePos = planePos / (numMeasurements - 1) * 0.2 - supportPoint.head<2>();

            const Eigen::Vector2d rs = aInv * planePos;
            const Eigen::Vector3d pos = supportPoint + dir1 * rs[0] + dir2 * rs[1];

            const auto data = splineCeres.getPointData(pos.head<2>());
            splineCeres.fillParameterPointers(data, parameterPointers.begin(), parameterPointers.end());
            ubs::UniformBSplineCeresEvaluator<Spline> evaluator = splineCeres.getEvaluator(data);

            auto* costFunctor = new ceres::DynamicAutoDiffCostFunction<FitCostFunctor<Spline>>(
                new FitCostFunctor<Spline>(evaluator, pos[2]));
            for (int k = 0; k < splineCeres.ControlPointsSupport; ++k) {
                costFunctor->AddParameterBlock(1);
            }
            costFunctor->SetNumResiduals(1);

            problem.AddResidualBlock(costFunctor, nullptr, parameterPointers);
        }
    }

    splineCeres.addSmoothnessResiduals<2>(problem);

    // Solve problem.
    ceres::Solver::Options options;
    options.minimizer_progress_to_stdout = false;
    options.initial_trust_region_radius = 1e16;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    // Check result.
    test_util::linspace(std::array<int, 2>{200, 200}, [&](const Eigen::Vector2d& pos) {
        const Eigen::Vector3d splinePos(pos[0], pos[1], spline.evaluate(pos));

        Eigen::Vector2d planePos = pos - supportPoint.head<2>();
        Eigen::Vector2d rs = aInv * planePos;
        const Eigen::Vector3d gtPos = supportPoint + dir1 * rs[0] + dir2 * rs[1];

        for (int i = 0; i < 3; ++i) {
            EXPECT_NEAR(gtPos[i], splinePos[i], 1e-8) << pos;
        }
    });
}


#pragma once
#include <range/v3/algorithm/for_each.hpp>
#include <range/v3/view/cycle.hpp>
#include <range/v3/view/take_while.hpp>
#include <boost/container/flat_set.hpp>
#include <vector>
#include "day1part1.hpp"

constexpr auto day1part2 = [](auto&& input) {
  auto current_frequency = initial_frequency;
  boost::container::flat_set<decltype(current_frequency)> previous_frequencies{};

  std::vector<int> frequency_changes{};
  std::string line;
  while (not input.eof()) {
    std::getline(input, line);
    frequency_changes.push_back(std::stoi(line));
  }

  ranges::v3::for_each(
    ranges::view::cycle(frequency_changes)
      | ranges::view::take_while([&]([[maybe_unused]] const auto frequency_change) {
        auto [iter,inserted] = previous_frequencies.insert(current_frequency);
        return inserted;
      }),
    [&](const auto frequency_change) {
      current_frequency = resulting_frequency(current_frequency, frequency_change);
    }
  );

  return current_frequency;;
};


//  Copyright (c) 2001-2011 Hartmut Kaiser
//  Copyright (c) 2011 Jan Frederick Eick
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/config/warning_disable.hpp>
#include <boost/detail/lightweight_test.hpp>
#include <boost/spirit/include/karma_numeric.hpp>

#include <boost/cstdint.hpp>

#include "test.hpp"

///////////////////////////////////////////////////////////////////////////////
//
//  *** BEWARE PLATFORM DEPENDENT!!! ***
//  *** The following assumes 32 bit boost::uint32_tegers.
//  *** Modify these constants when appropriate.
//
///////////////////////////////////////////////////////////////////////////////

char const* max_unsigned_base2 =  "11111111111111111111111111111111";
char const* max_unsigned_base3 =  "102002022201221111210";
char const* max_unsigned_base4 =  "3333333333333333";
char const* max_unsigned_base5 =  "32244002423140";
char const* max_unsigned_base6 =  "1550104015503";
char const* max_unsigned_base7 =  "211301422353";
char const* max_unsigned_base8 =  "37777777777";
char const* max_unsigned_base9 =  "12068657453";
char const* max_unsigned_base11 = "1904440553";
char const* max_unsigned_base12 = "9ba461593";
char const* max_unsigned_base13 = "535a79888";
char const* max_unsigned_base14 = "2ca5b7463";
char const* max_unsigned_base15 = "1a20dcd80";
char const* max_unsigned_base16 = "ffffffff";
char const* max_unsigned_base17 = "a7ffda90";
char const* max_unsigned_base18 = "704he7g3";
char const* max_unsigned_base19 = "4f5aff65";
char const* max_unsigned_base20 = "3723ai4f";
char const* max_unsigned_base21 = "281d55i3";
char const* max_unsigned_base22 = "1fj8b183";
char const* max_unsigned_base23 = "1606k7ib";
char const* max_unsigned_base24 = "mb994af";
char const* max_unsigned_base25 = "hek2mgk";
char const* max_unsigned_base26 = "dnchbnl";
char const* max_unsigned_base27 = "b28jpdl";
char const* max_unsigned_base28 = "8pfgih3";
char const* max_unsigned_base29 = "76beigf";
char const* max_unsigned_base30 = "5qmcpqf";
char const* max_unsigned_base31 = "4q0jto3";
char const* max_unsigned_base32 = "3vvvvvv";
char const* max_unsigned_base33 = "3aokq93";
char const* max_unsigned_base34 = "2qhxjlh";
char const* max_unsigned_base35 = "2br45qa";
char const* max_unsigned_base36 = "1z141z3";

int
main()
{
    using spirit_test::test;
    using boost::spirit::karma::uint_generator;

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 2)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 2> base2_generator;

        BOOST_TEST(test("1100111100100110010", base2_generator(424242)));
        BOOST_TEST(test("1100111100100110010", base2_generator, 424242));

        BOOST_TEST(test(max_unsigned_base2, base2_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base2, base2_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 3)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 3> base3_generator;

        BOOST_TEST(test("210112221200", base3_generator(424242)));
        BOOST_TEST(test("210112221200", base3_generator, 424242));

        BOOST_TEST(test(max_unsigned_base3, base3_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base3, base3_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 4)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 4> base4_generator;

        BOOST_TEST(test("1213210302", base4_generator(424242)));
        BOOST_TEST(test("1213210302", base4_generator, 424242));

        BOOST_TEST(test(max_unsigned_base4, base4_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base4, base4_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 5)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 5> base5_generator;

        BOOST_TEST(test("102033432", base5_generator(424242)));
        BOOST_TEST(test("102033432", base5_generator, 424242));

        BOOST_TEST(test(max_unsigned_base5, base5_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base5, base5_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 6)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 6> base6_generator;

        BOOST_TEST(test("13032030", base6_generator(424242)));
        BOOST_TEST(test("13032030", base6_generator, 424242));

        BOOST_TEST(test(max_unsigned_base6, base6_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base6, base6_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 7)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 7> base7_generator;

        BOOST_TEST(test("3414600", base7_generator(424242)));
        BOOST_TEST(test("3414600", base7_generator, 424242));

        BOOST_TEST(test(max_unsigned_base7, base7_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base7, base7_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 8)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 8> base8_generator;

        BOOST_TEST(test("1474462", base8_generator(424242)));
        BOOST_TEST(test("1474462", base8_generator, 424242));

        BOOST_TEST(test(max_unsigned_base8, base8_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base8, base8_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 9)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 9> base9_generator;

        BOOST_TEST(test("715850", base9_generator(424242)));
        BOOST_TEST(test("715850", base9_generator, 424242));

        BOOST_TEST(test(max_unsigned_base9, base9_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base9, base9_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 11)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 11> base11_generator;

        BOOST_TEST(test("26a815", base11_generator(424242)));
        BOOST_TEST(test("26a815", base11_generator, 424242));

        BOOST_TEST(test(max_unsigned_base11, base11_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base11, base11_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 12)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 12> base12_generator;

        BOOST_TEST(test("185616", base12_generator(424242)));
        BOOST_TEST(test("185616", base12_generator, 424242));

        BOOST_TEST(test(max_unsigned_base12, base12_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base12, base12_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 13)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 13> base13_generator;

        BOOST_TEST(test("11b140", base13_generator(424242)));
        BOOST_TEST(test("11b140", base13_generator, 424242));

        BOOST_TEST(test(max_unsigned_base13, base13_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base13, base13_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 14)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 14> base14_generator;

        BOOST_TEST(test("b0870", base14_generator(424242)));
        BOOST_TEST(test("b0870", base14_generator, 424242));

        BOOST_TEST(test(max_unsigned_base14, base14_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base14, base14_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 15)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 15> base15_generator;

        BOOST_TEST(test("85a7c", base15_generator(424242)));
        BOOST_TEST(test("85a7c", base15_generator, 424242));

        BOOST_TEST(test(max_unsigned_base15, base15_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base15, base15_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 16)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 16> base16_generator;

        BOOST_TEST(test("67932", base16_generator(424242)));
        BOOST_TEST(test("67932", base16_generator, 424242));

        BOOST_TEST(test(max_unsigned_base16, base16_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base16, base16_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 17)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 17> base17_generator;

        BOOST_TEST(test("515g7", base17_generator(424242)));
        BOOST_TEST(test("515g7", base17_generator, 424242));

        BOOST_TEST(test(max_unsigned_base17, base17_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base17, base17_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 18)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 18> base18_generator;

        BOOST_TEST(test("40d70", base18_generator(424242)));
        BOOST_TEST(test("40d70", base18_generator, 424242));

        BOOST_TEST(test(max_unsigned_base18, base18_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base18, base18_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 19)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 19> base19_generator;

        BOOST_TEST(test("34g3a", base19_generator(424242)));
        BOOST_TEST(test("34g3a", base19_generator, 424242));

        BOOST_TEST(test(max_unsigned_base19, base19_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base19, base19_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 20)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 20> base20_generator;

        BOOST_TEST(test("2d0c2", base20_generator(424242)));
        BOOST_TEST(test("2d0c2", base20_generator, 424242));

        BOOST_TEST(test(max_unsigned_base20, base20_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base20, base20_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 21)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 21> base21_generator;

        BOOST_TEST(test("23h00", base21_generator(424242)));
        BOOST_TEST(test("23h00", base21_generator, 424242));

        BOOST_TEST(test(max_unsigned_base21, base21_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base21, base21_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 22)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 22> base22_generator;

        BOOST_TEST(test("1hibg", base22_generator(424242)));
        BOOST_TEST(test("1hibg", base22_generator, 424242));

        BOOST_TEST(test(max_unsigned_base22, base22_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base22, base22_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 23)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 23> base23_generator;

        BOOST_TEST(test("1bjm7", base23_generator(424242)));
        BOOST_TEST(test("1bjm7", base23_generator, 424242));

        BOOST_TEST(test(max_unsigned_base23, base23_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base23, base23_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 24)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 24> base24_generator;

        BOOST_TEST(test("16gci", base24_generator(424242)));
        BOOST_TEST(test("16gci", base24_generator, 424242));

        BOOST_TEST(test(max_unsigned_base24, base24_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base24, base24_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 25)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 25> base25_generator;

        BOOST_TEST(test("123jh", base25_generator(424242)));
        BOOST_TEST(test("123jh", base25_generator, 424242));

        BOOST_TEST(test(max_unsigned_base25, base25_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base25, base25_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 26)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 26> base26_generator;

        BOOST_TEST(test("o3f0", base26_generator(424242)));
        BOOST_TEST(test("o3f0", base26_generator, 424242));

        BOOST_TEST(test(max_unsigned_base26, base26_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base26, base26_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 27)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 27> base27_generator;

        BOOST_TEST(test("lepi", base27_generator(424242)));
        BOOST_TEST(test("lepi", base27_generator, 424242));

        BOOST_TEST(test(max_unsigned_base27, base27_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base27, base27_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 28)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 28> base28_generator;

        BOOST_TEST(test("j93e", base28_generator(424242)));
        BOOST_TEST(test("j93e", base28_generator, 424242));

        BOOST_TEST(test(max_unsigned_base28, base28_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base28, base28_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 29)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 29> base29_generator;

        BOOST_TEST(test("hbd1", base29_generator(424242)));
        BOOST_TEST(test("hbd1", base29_generator, 424242));

        BOOST_TEST(test(max_unsigned_base29, base29_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base29, base29_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 30)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 30> base30_generator;

        BOOST_TEST(test("flbc", base30_generator(424242)));
        BOOST_TEST(test("flbc", base30_generator, 424242));

        BOOST_TEST(test(max_unsigned_base30, base30_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base30, base30_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 31)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 31> base31_generator;

        BOOST_TEST(test("e7e7", base31_generator(424242)));
        BOOST_TEST(test("e7e7", base31_generator, 424242));

        BOOST_TEST(test(max_unsigned_base31, base31_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base31, base31_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 32)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 32> base32_generator;

        BOOST_TEST(test("cu9i", base32_generator(424242)));
        BOOST_TEST(test("cu9i", base32_generator, 424242));

        BOOST_TEST(test(max_unsigned_base32, base32_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base32, base32_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 33)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 33> base33_generator;

        BOOST_TEST(test("bqir", base33_generator(424242)));
        BOOST_TEST(test("bqir", base33_generator, 424242));

        BOOST_TEST(test(max_unsigned_base33, base33_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base33, base33_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 34)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 34> base34_generator;

        BOOST_TEST(test("aqxo", base34_generator(424242)));
        BOOST_TEST(test("aqxo", base34_generator, 424242));

        BOOST_TEST(test(max_unsigned_base34, base34_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base34, base34_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 35)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 35> base35_generator;

        BOOST_TEST(test("9vb7", base35_generator(424242)));
        BOOST_TEST(test("9vb7", base35_generator, 424242));

        BOOST_TEST(test(max_unsigned_base35, base35_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base35, base35_generator, 0xffffffffu));
    }

    ///////////////////////////////////////////////////////////////////////////
    //  arbitrary radix test (base 36)
    ///////////////////////////////////////////////////////////////////////////
    {
        uint_generator<boost::uint32_t, 36> base36_generator;

        BOOST_TEST(test("93ci", base36_generator(424242)));
        BOOST_TEST(test("93ci", base36_generator, 424242));

        BOOST_TEST(test(max_unsigned_base36, base36_generator(0xffffffffu)));
        BOOST_TEST(test(max_unsigned_base36, base36_generator, 0xffffffffu));
    }

    return boost::report_errors();
}


#pragma once
/**
    Anderson Acceleration for linear iterative problems.
    @author @copyright sentinel (Github: Enigmatisms)
    @ref (paper) Convergence Analysis for Anderson Acceleration
*/
#include <Eigen/Dense>
#include <iostream>
#include <deque>

template <typename T, int Dim>
class AA {
using MatrixDxt = Eigen::Matrix<T, Dim, -1>;
using MatrixDt = Eigen::Matrix<T, Dim, Dim>;
using MatrixXt = Eigen::Matrix<T, -1, -1>;
using VectorDt = Eigen::Matrix<T, Dim, 1>;
using VectorXt = Eigen::Matrix<T, -1, 1>;
public:
    AA(T restart_thresh, T alpha_lim = 10., int dim = 2):
            restart_thresh(restart_thresh), alpha_lim(alpha_lim), dim(dim) {}
    ~AA() {}

    bool andersonAccelerate(const VectorDt& pose, const VectorDt& prev_pose, VectorDt& out_pose, T avg_err, int iter_num);
private:
    const T restart_thresh;
    const T alpha_lim;
    const int dim;
    std::deque<VectorDt> Gs;
    std::deque<VectorDt> Fs;
};


/*
 * SensorValues.cpp
 *
 *  Created on: 30.08.2017
 *      Author: thies
 */

#include <deal.II/base/exceptions.h>
#include <deal.II/base/mpi.h>

#include <measurements/SensorValues.h>

#include <stddef.h>
#include <tgmath.h>
#include <algorithm>
#include <cmath>
#include <fstream>
#include <iostream>
#include <memory>
#include <random>
#include <string>
#include <vector>

namespace wavepi {
namespace measurements {

template<int dim>
SensorValues<dim> SensorValues<dim>::noise(std::shared_ptr<SensorDistribution<dim>> grid) {
   SensorValues<dim> res(grid);

   // auto time = std::chrono::high_resolution_clock::now();
   // std::default_random_engine generator(time.time_since_epoch().count() % 1000000);
   std::default_random_engine generator(2307);
   std::uniform_real_distribution<double> distribution(-1, 1);

   for (size_t i = 0; i < grid->size(); i++)
      res[i] = distribution(generator);

   return res;
}

template<int dim>
SensorValues<dim> SensorValues<dim>::noise(const SensorValues<dim>& like) {
   return noise(like.grid);
}

template<int dim>
SensorValues<dim> SensorValues<dim>::noise(const SensorValues<dim>& like, double norm) {
   auto res = noise(like.grid);
   res *= norm / res.norm();

   return res;
}

template<int dim>
double SensorValues<dim>::relative_error(const SensorValues<dim>& other) const {
   SensorValues<dim> tmp(*this);
   tmp -= other;

   double denom = this->norm();
   return tmp.norm() / (denom == 0.0 ? 1.0 : denom);
}

template<int dim>
void SensorValues<dim>::write_pvd(std::string path, std::string filename, std::string name) const {
   AssertThrow(grid, ExcNotInitialized());

   grid->write_pvd(elements, path, filename, name);
}

#ifdef WAVEPI_MPI
template<int dim>
void SensorValues<dim>::mpi_irecv(size_t source, std::vector<MPI_Request>& reqs) {
   AssertThrow(reqs.size() == 0, ExcInternalError());

   reqs.emplace_back();
   MPI_Irecv(&elements[0], elements.size(), MPI_DOUBLE, source, 1, MPI_COMM_WORLD, &reqs[0]);
}

template<int dim>
void SensorValues<dim>::mpi_send(size_t destination) {
   MPI_Send(&elements[0], elements.size(), MPI_DOUBLE, destination, 1, MPI_COMM_WORLD);
}

template<int dim>
void SensorValues<dim>::mpi_isend(size_t destination, std::vector<MPI_Request>& reqs) {
   AssertThrow(reqs.size() == 0, ExcInternalError());

   reqs.emplace_back();
   MPI_Isend(&elements[0], elements.size(), MPI_DOUBLE, destination, 1, MPI_COMM_WORLD, &reqs[0]);
}

template<int dim>
void SensorValues<dim>::mpi_all_reduce(SensorValues<dim> source, MPI_Op op) {
   MPI_Allreduce(&source.elements[0], &elements[0], elements.size(), MPI_DOUBLE, op, MPI_COMM_WORLD);
}

template<int dim>
void SensorValues<dim>::mpi_bcast(size_t root) {
   MPI_Bcast(&elements[0], elements.size(), MPI_DOUBLE, root, MPI_COMM_WORLD);
}

#endif

template class SensorValues<1> ;
template class SensorValues<2> ;
template class SensorValues<3> ;

} /* namespace measurements */
} /* namespace wavepi */


//######################################################################
//#   Refiner Module
//#
//#   Copyright (C) 2020 Siemens AG
//#   SPDX-License-Identifier: MIT
//#   Author 2020: This module has been developed by
//#                Roman Kaskman under supervision of Slobodan Ilic
//#######################################################################

#include "refiner.h"
#include <util.h>
#include "rendering_helper.h"
#include "frame_payload.h"
#include "optimizer.h"
#include "correspondence_finder.h"
#include <string>
#include <Eigen/Geometry>
#include "occlusion_handler.hpp"
#include <filesystem>
#include <vis_utils.h>

using namespace Eigen;
using namespace std;

#define CAST_ROUND_INT(x) static_cast<int>(lround(x))

std::vector<Vector4f> extract_edges(const cv::Rect& roi, const cv::Mat& image)
{
	// Create and LSD detector with standard or no refinement.
	cv::Ptr<cv::LineSegmentDetector> ls = cv::createLineSegmentDetector(cv::LSD_REFINE_ADV, 0.9, 0.7);
	vector<cv::Vec4f> lines_std;

	// Detect the lines
	ls->detect(image(roi), lines_std);

	vector<Vector4f> edges(lines_std.size());

	transform(lines_std.begin(), lines_std.end(), edges.begin(), [&roi](const auto& line)
	{
		cv::Vec4f crop_line = line + cv::Vec4f(roi.x, roi.y, roi.x, roi.y);
		return Vector4f(crop_line[0], crop_line[1], crop_line[2], crop_line[3]);
	});

	return edges;
}

cv::Vec4i find_bounding_box_coordinates(const cv::Mat& img)
{
	int min_y = img.rows - 1;
	int min_x = img.cols - 1;
	int max_y = 0;
	int max_x = 0;

	for (int i = 0; i < img.rows; ++i)
	{
		for (int j = 0; j < img.cols; ++j)
		{
			float val = img.at<float>(i, j);

			if (val > 0.0)
			{
				if (i < min_y)
				{
					min_y = i;
				}
				else if (i > max_y)
				{
					max_y = i;
				}

				if (j < min_x)
				{
					min_x = j;
				}
				else if (j > max_x)
				{
					max_x = j;
				}
			}
		}
	}

	return cv::Vec4i(min_y, max_y, min_x, max_x);
}

void add_padding(cv::Vec4i& vec, int padding_pixels) {
	vec[0] = vec[0] - padding_pixels;
	vec[1] = vec[1] + padding_pixels;
	vec[2] = vec[2] - padding_pixels;
	vec[3] = vec[3] + padding_pixels;
}

template <typename T>
T get_median_absolute_deviation(const Matrix<T, Dynamic, 1>& v) {
	T median = get_median(v);
	Matrix<T, Dynamic, 1> res = (v - median).cwiseAbs();
	return get_median(res);
}

void compute_residual_std_dev(const vector<FramePayload>& frame_payloads, const Matrix4d& model_pose_m,
                              const Matrix3d& intrinsics, double& std_dev, bool& has_converged)
{
	vector<double> residuals;

	int converged_num = 0;
	const Isometry3d model_pose = to_isometry(model_pose_m);
	for (const auto& payload : frame_payloads)
	{
		const Correspondence& correspondence = payload.correspondence;
		Matrix4d scene_pose_inv = payload.scene_pose.inverse();

		Isometry3d world_to_cam_transform = to_isometry(scene_pose_inv) * model_pose;

		for (size_t i = 0; i < correspondence.get_number_of_correspondences(); i++)
		{
			const Vector3d world_point = correspondence.world_points[i].cast<double>();
			const Vector2d pixel_point = correspondence.corresponding_points[i].cast<double>();

			Vector3d point_in_cam_coords = world_to_cam_transform * world_point;
			point_in_cam_coords /= point_in_cam_coords[2];
			Vector2d pixel_coords = (intrinsics * point_in_cam_coords).block<2, 1>(0, 0);

			Vector2d rgb_edge_normal = correspondence.get_rgb_normal(i).cast<double>();
			double residual = (pixel_point - pixel_coords).dot(rgb_edge_normal);

			if (std::abs(residual) < 1.5)
			{
				converged_num++;
			}

			residuals.push_back(residual);
		}
	}

	VectorXd eigen_residuals = Eigen::Map<VectorXd, Unaligned>(residuals.data(), residuals.size());
	double mad = get_median_absolute_deviation(eigen_residuals);
	std_dev = 1.482579 * mad;

	auto res_num = static_cast<double>(residuals.size());
	has_converged = (converged_num / res_num) >= 0.9;
}

cv::Rect get_roi_box(const cv::Mat& depth_img, int model_padding_pixels)
{
	cv::Vec4i bbox = find_bounding_box_coordinates(depth_img);
	add_padding(bbox, model_padding_pixels);

	bbox[0] = max(0, bbox[0]);
	bbox[1] = min(depth_img.rows - 1, bbox[1]);

	bbox[2] = max(0, bbox[2]);
	bbox[3] = min(depth_img.cols - 1, bbox[3]);

	int crop_width = bbox[3] - bbox[2];
	int crop_height = bbox[1] - bbox[0];

	return cv::Rect(bbox[2], bbox[0], crop_width, crop_height);
}

Refiner::Refiner(const Configuration& p_configuration)
{
	configuration = p_configuration;
}

vector<Vector4f> get_depth_edges(const cv::Mat &depth_img, const cv::Rect &cropping_box)
{
	cv::Mat norm_depth_img;
	cv::normalize(depth_img, norm_depth_img, 255, 0, cv::NORM_MINMAX);
	norm_depth_img.convertTo(norm_depth_img, CV_8UC1);
	return extract_edges(cropping_box, norm_depth_img);
}

inline void draw_line_ids(cv::Mat &img, const vector<Vector4f> &edges) {
	for (int i = 0; i < edges.size(); i++) {
		const auto &edge = edges[i];
		cv::line(img, cv::Point(CAST_ROUND_INT(edge[0]), CAST_ROUND_INT(edge[1])),
			cv::Point(CAST_ROUND_INT(edge[2]), CAST_ROUND_INT(edge[3])), cv::Scalar(i), 1, 8);
	}
}

Matrix4d Refiner::refine_model_pose(const vector<Matrix4d>& camera_poses,
                                           const vector<cv::Mat>& grayscale_images,
                                           const Matrix4d& model_pose, int model_id)
{
	const auto& first_image = grayscale_images[0];

	int height = first_image.rows;
	int width = first_image.cols;

	CorrespondenceFinder correspondence_finder(configuration);
	RenderingHelper rendering_helper(configuration, model_id, width, height);

	const size_t number_of_frames = grayscale_images.size();
	Matrix4d current_model_pose = model_pose;

	int number_of_iterations = configuration.get_max_iterations();
	const Matrix3d intrinsics = configuration.get_intrinsics().cast<double>();

	for (int iteration = 0; iteration < number_of_iterations; iteration++)
	{
		vector<FramePayload> frame_payloads(number_of_frames);
		for (size_t frame_idx = 0; frame_idx < number_of_frames; ++frame_idx)
		{
			FramePayload payload;

			const Matrix4d& camera_pose = camera_poses[frame_idx];
			payload.scene_pose = camera_pose;

			Matrix4d world_to_camera = camera_pose.inverse() * current_model_pose;

			cv::Mat depth_img;
			cv::Mat rendered_color_img; // not used here
			rendering_helper.render(world_to_camera, depth_img, rendered_color_img);
			cv::Rect cropping_box = get_roi_box(depth_img, configuration.get_model_padding_pixels());

			vector<Vector4f> depth_edges = get_depth_edges(depth_img, cropping_box);

			const cv::Mat& grayscale_img = grayscale_images[frame_idx];
			cv::Mat edge_id_img = cv::Mat(height, width, CV_32SC1, cv::Scalar::all(-1));
			vector<Vector4f> rgb_edges = extract_edges(cropping_box, grayscale_img);

			draw_line_ids(edge_id_img, rgb_edges);
			transform(rgb_edges.begin(), rgb_edges.end(), back_inserter(payload.correspondence.rgb_edge_normals),
			          [](const Vector4f& edge) { return get_edge_normal(edge); });

			Matrix4d to_world_transformation = world_to_camera.inverse();
			correspondence_finder.find_correspondences(depth_img, edge_id_img, to_world_transformation,
			                                           depth_edges, payload.correspondence);

#ifdef _DEBUG
				if (frame_idx % 20 == 0)
				{
					cv::Mat edge_viz = get_edge_visualization(height, width, payload, depth_edges, rgb_edges);
					cv::imshow("Edge matches", edge_viz);
					cv::waitKey(0);
				}

#endif
			frame_payloads[frame_idx] = payload;
		}

		double res_std_dev;
		bool has_converged;
		compute_residual_std_dev(frame_payloads, current_model_pose, intrinsics, res_std_dev, has_converged);

		if (has_converged) break;

		current_model_pose = optimize_model_pose(frame_payloads, current_model_pose, intrinsics, res_std_dev);
	}

	return current_model_pose;
}

map<int, Matrix4d> Refiner::refine_model_poses(const RefinementInput& input)
{
	OcclusionHandler occlusion_handler(configuration, input.rgbd_image_files, input.camera_poses);

	map<int, Matrix4d> refined_model_poses;

	for (const auto& [model_id, model_pose] : input.model_poses)
	{
		vector<size_t> valid_frame_ids = occlusion_handler.get_frame_ids_with_visible_model(model_id, model_pose);
		if (!valid_frame_ids.empty())
		{
			cout << "Running optimization for model #" << model_id << " on " << valid_frame_ids.size() << " frames" << endl;

			vector<Matrix4d> camera_poses(valid_frame_ids.size());
			vector<cv::Mat> grayscale_images(valid_frame_ids.size());

			transform(valid_frame_ids.begin(), valid_frame_ids.end(), camera_poses.begin(),
			          [&input](const auto& frame_id) { return input.camera_poses[frame_id]; });
			transform(valid_frame_ids.begin(), valid_frame_ids.end(), grayscale_images.begin(),
			          [&input](const auto& frame_id)
			          {
				          const string& rgb_file = input.rgbd_image_files[frame_id].first;
				          cv::Mat rgb = read_image(rgb_file);
						  cv::Mat grayscale;
						  cv::cvtColor(rgb, grayscale, CV_RGB2GRAY);
						  return grayscale;
			          });

			refined_model_poses[model_id] = refine_model_pose(camera_poses, grayscale_images, model_pose, model_id);
		}
		else
		{
			refined_model_poses[model_id] = model_pose;
			cerr << "No valid frames for model #" << model_id << ", skipping refinement" << endl;
		}
	}

	return refined_model_poses;
}


/*
 * Copyright 2013-2015 Raphael Bost
 *
 * This file is part of ciphermed.

 *  ciphermed is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  ciphermed is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with ciphermed.  If not, see <http://www.gnu.org/licenses/>. 2
 *
 */

#include <assert.h>
#include <vector>
#include <crypto/paillier.hh>
#include <crypto/gm.hh>
#include <NTL/ZZ.h>
#include <gmpxx.h>
#include <math/util_gmp_rand.h>

#include <ctime>

#include<iostream>

using namespace std;
using namespace NTL;

static void test_paillier()
{
    cout << "Test Paillier ...\n" << flush;

    gmp_randstate_t randstate;
    gmp_randinit_default(randstate);
    gmp_randseed_ui(randstate,time(NULL));

    auto sk = Paillier_priv::keygen(randstate,16,2); //600 ,256
    Paillier_priv pp(sk,randstate);

    auto pk = pp.pubkey();
    mpz_class n = pk[0];
    Paillier p(pk,randstate);

    //mpz_class pt0, pt1,m;
    //mpz_urandomm(pt0.get_mpz_t(),randstate,n.get_mpz_t());
    //mpz_urandomm(pt1.get_mpz_t(),randstate,n.get_mpz_t());
    //mpz_urandomm(m.get_mpz_t(),randstate,n.get_mpz_t());
    mpz_class pt0 = 2, pt1 = 3, m = 5; //instead of the random values
    cout << "pt0: "<< pt0 << endl;
    cout << "pt1: "<< pt1 << endl;
    cout << "m: "<< m << endl;

    mpz_class ct0 = p.encrypt(pt0);         cout << "ct0: "<< ct0 << endl;
    mpz_class ct1 = p.encrypt(pt1);         cout << "ct1: "<< ct1 << endl;
    mpz_class sum = p.add(ct0, ct1);        cout << "sum_enc: "<< sum << endl;
    mpz_class prod = p.constMult(m,ct0);    cout << "prod_enc: "<< prod << endl;
    //    mpz_class diff = p.constMult(-1, ct0);
    mpz_class diff = p.sub(ct0, ct1);       cout << "diff_enc: "<< diff << endl;

    assert(pp.decrypt(ct0) == pt0);
    assert(pp.decrypt(ct1) == pt1);
    assert(pp.decrypt(sum) == (pt0+pt1)%n);
    mpz_class d = pt0 - pt1;
    if (d < 0) {
        d += n;
    }
    assert( pp.decrypt(diff) == d);
    assert(pp.decrypt(prod) == (m*pt0)%n);

    cout << "Test Paillier passed" << endl;

    mpz_class A[3][3] = {{1,2,3},{1,1,1},{4,5,6}};
    //int rows = sizeof A / sizeof A[0];
    //int cols = sizeof A[0] / sizeof(mpz_class);
    //cout << rows << " " << cols <<endl;
    mpz_class A_enc[3][3];
    p.encryptMatrix(A, A_enc);
    mpz_class A_dec[3][3];
    pp.decryptMatrix(A_enc, A_dec);
    cout << "Test Matrix Paillier passed" << endl;
}

static void test_paillier_fast()
{
    cout << "Test Paillier Fast..." << flush;

    gmp_randstate_t randstate;
    gmp_randinit_default(randstate);
    gmp_randseed_ui(randstate,time(NULL));

    auto sk = Paillier_priv_fast::keygen(randstate,600);
    Paillier_priv_fast pp(sk,randstate);

    auto pk = pp.pubkey();
    mpz_class n = pk[0];
    Paillier p(pk,randstate);

    mpz_class pt0, pt1,m;
    mpz_urandomm(pt0.get_mpz_t(),randstate,n.get_mpz_t());
    mpz_urandomm(pt1.get_mpz_t(),randstate,n.get_mpz_t());
    mpz_urandomm(m.get_mpz_t(),randstate,n.get_mpz_t());

    mpz_class ct0 = pp.encrypt(pt0);
    mpz_class ct1 = pp.encrypt(pt1);
    mpz_class sum = p.add(ct0, ct1);
    mpz_class prod = p.constMult(m,ct0);
    //    mpz_class diff = p.constMult(-1, ct0);
    mpz_class diff = p.sub(ct0, ct1);

    assert(pp.decrypt(ct0) == pt0);
    assert(pp.decrypt(ct1) == pt1);
    assert(pp.decrypt(sum) == (pt0+pt1)%n);
    mpz_class d = pt0 - pt1;
    if (d < 0) {
        d += n;
    }
    assert( pp.decrypt(diff) == d);
    assert(pp.decrypt(prod) == (m*pt0)%n);

    cout << " passed" << endl;
}

static void paillier_perf(unsigned int k, unsigned int a_bits, size_t n_iteration)
{
    cout << "Test Paillier performances ..." << endl;

    cout << "k = " << k << "\na_bits = " << a_bits << "\n" << n_iteration << " iterations" << endl;

    gmp_randstate_t randstate;
    gmp_randinit_default(randstate);
    gmp_randseed_ui(randstate,time(NULL));

    auto sk = Paillier_priv::keygen(randstate,k,a_bits);
    Paillier_priv pp(sk,randstate);

    auto pk = pp.pubkey();
    mpz_class n = pk[0];
    Paillier p(pk,randstate);

    mpz_class pt0, pt1,m;
    mpz_urandomm(pt0.get_mpz_t(),randstate,n.get_mpz_t());
    mpz_urandomm(pt1.get_mpz_t(),randstate,n.get_mpz_t());
    mpz_urandomm(m.get_mpz_t(),randstate,n.get_mpz_t());

    mpz_class ct0 = p.encrypt(pt0);
    mpz_class ct1 = p.encrypt(pt1);
    mpz_class sum = p.add(ct0, ct1);
    mpz_class prod = p.constMult(m,ct0);
    //    mpz_class diff = p.constMult(-1, ct0);
    mpz_class diff = p.sub(ct0, ct1);

    assert(pp.decrypt(ct0) == pt0);
    assert(pp.decrypt(ct1) == pt1);
    assert(pp.decrypt(sum) == (pt0+pt1)%n);
    mpz_class d = pt0 - pt1;
    if (d < 0) {
        d += n;
    }
    assert( pp.decrypt(diff) == d);
    assert(pp.decrypt(prod) == (m*pt0)%n);

    struct timespec t0,t1;

    vector<mpz_class> ct(n_iteration);
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t0);
    for (size_t i = 0; i < n_iteration; i++) {
        mpz_class pt;
        mpz_urandomm(pt.get_mpz_t(),randstate,n.get_mpz_t());
        ct[i] = p.encrypt(pt);
    }
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t1);
    uint64_t t = (((uint64_t)t1.tv_sec) - ((uint64_t)t0.tv_sec) )* 1000000000 + (t1.tv_nsec - t0.tv_nsec);
    cerr << "public encryption: "<<  ((double)t/1000000)/n_iteration <<"ms per plaintext" << endl;

    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t0);
    for (size_t i = 0; i < n_iteration; i++) {
        mpz_class pt;
        mpz_urandomm(pt.get_mpz_t(),randstate,n.get_mpz_t());
        pp.encrypt(pt);
    }
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t1);
    t = (((uint64_t)t1.tv_sec) - ((uint64_t)t0.tv_sec) )* 1000000000 + (t1.tv_nsec - t0.tv_nsec);
    cerr << "private encryption: "<<  ((double)t/1000000)/n_iteration <<"ms per plaintext" << endl;

    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t0);
    for (size_t i = 0; i < n_iteration; i++) {
        mpz_class pt;
        mpz_urandomm(pt.get_mpz_t(),randstate,n.get_mpz_t());
        pp.fast_encrypt_precompute(pt);
    }
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t1);
    t = (((uint64_t)t1.tv_sec) - ((uint64_t)t0.tv_sec) )* 1000000000 + (t1.tv_nsec - t0.tv_nsec);
    cerr << "private encryption with precomputation: "<<  ((double)t/1000000)/n_iteration <<"ms per plaintext" << endl;

    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t0);
    for (size_t i = 0; i < n_iteration; i++) {
        pp.decrypt(ct[i]);
    }
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t1);
    t = (((uint64_t)t1.tv_sec) - ((uint64_t)t0.tv_sec) )* 1000000000 + (t1.tv_nsec - t0.tv_nsec);
    cerr << "decryption: "<<  ((double)t/1000000)/n_iteration <<"ms per cyphertext" << endl;

}

static void paillier_fast_perf(unsigned int k, size_t n_iteration)
{
    cout << "Test Paillier Fast performances ..." << endl;

    cout << "k = " << k << "\n" << n_iteration << " iterations" << endl;

    gmp_randstate_t randstate;
    gmp_randinit_default(randstate);
    gmp_randseed_ui(randstate,time(NULL));

    auto sk = Paillier_priv_fast::keygen(randstate,k);
    Paillier_priv_fast pp(sk,randstate);

    auto pk = pp.pubkey();
    mpz_class n = pk[0];
//    Paillier p(pk,randstate);

    mpz_class pt0, pt1,m;
    mpz_urandomm(pt0.get_mpz_t(),randstate,n.get_mpz_t());
    mpz_urandomm(pt1.get_mpz_t(),randstate,n.get_mpz_t());
    mpz_urandomm(m.get_mpz_t(),randstate,n.get_mpz_t());

    struct timespec t0,t1;
    uint64_t t;

    vector<mpz_class> ct(n_iteration);

    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t0);
    for (size_t i = 0; i < n_iteration; i++) {
        mpz_class pt;
        mpz_urandomm(pt.get_mpz_t(),randstate,n.get_mpz_t());
        ct[i] = pp.encrypt(pt);
    }
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t1);
    t = (((uint64_t)t1.tv_sec) - ((uint64_t)t0.tv_sec) )* 1000000000 + (t1.tv_nsec - t0.tv_nsec);
    cerr << "private encryption with generator precomputations: "<<  ((double)t/1000000)/n_iteration <<"ms per plaintext" << endl;

    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t0);
    for (size_t i = 0; i < n_iteration; i++) {
        pp.decrypt(ct[i]);
    }
    clock_gettime(CLOCK_THREAD_CPUTIME_ID,&t1);
    t = (((uint64_t)t1.tv_sec) - ((uint64_t)t0.tv_sec) )* 1000000000 + (t1.tv_nsec - t0.tv_nsec);
    cerr << "decryption: "<<  ((double)t/1000000)/n_iteration <<"ms per cyphertext" << endl;

}

static void test_gm()
{
    cout << "Test GM ..." << flush;

    gmp_randstate_t randstate;
    gmp_randinit_default(randstate);
    gmp_randseed_ui(randstate,time(NULL));

    auto sk = GM_priv::keygen(randstate);
    GM_priv pp(sk,randstate);

    auto pk = pp.pubkey();
    GM p(pk,randstate);

    bool b0 = true; //(bool)RandomBits_long(1);
    bool b1 = false; //(bool)RandomBits_long(1);

    mpz_class ct0 = p.encrypt(b0);
    mpz_class ct1 = p.encrypt(b1);
    mpz_class XOR = p.XOR(ct0, ct1);
    mpz_class rerand = p.reRand(ct0);

    assert(pp.decrypt(pk[1]) == true);
    assert(pp.decrypt(ct0) == b0);
    assert(pp.decrypt(ct1) == b1);
    assert(pp.decrypt(XOR) == (b0 xor b1));
    assert(pp.decrypt(rerand) == b0);

    cout << " passed" << endl;
}

int main(int ac, char **av)
{
    SetSeed(to_ZZ(time(NULL)));
    //test_elgamal();
    test_paillier();
    //test_paillier_fast();
    //test_gm();

    return 0;
}


///////////////////////////////////////////////////////////////////////////////
// error_of.hpp
//
//  Copyright 2005 Eric Niebler. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_ERROR_OF_MEAN_HPP_EAN_27_03_2006
#define BOOST_ACCUMULATORS_STATISTICS_ERROR_OF_MEAN_HPP_EAN_27_03_2006

#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/error_of.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/count.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // error_of_mean_impl
    template<typename Sample, typename Variance>
    struct error_of_mean_impl
      : accumulator_base
    {
        // for boost::result_of
        typedef typename numeric::functional::average<Sample, std::size_t>::result_type result_type;

        error_of_mean_impl(dont_care) {}

        template<typename Args>
        result_type result(Args const &args) const
        {
            using namespace std;
            extractor<Variance> const variance = {};
            return sqrt(numeric::average(variance(args), count(args) - 1));
        }
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::error_of
//
namespace tag
{
    template<>
    struct error_of<mean>
      : depends_on<lazy_variance, count>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::error_of_mean_impl<mpl::_1, lazy_variance> impl;
    };

    template<>
    struct error_of<immediate_mean>
      : depends_on<variance, count>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::error_of_mean_impl<mpl::_1, variance> impl;
    };
}

}} // namespace boost::accumulators

#endif