//
// Created by 范軒瑋 on 2020-04-28.
//

#include "polynomials.h"
#include "geometry.h"
#include "scene.h"
#include "bssrdf.h"
#include "shapes/triangle.h"
#include "interaction.h"
#include "parallel.h"
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Dense>

namespace pbrt {

size_t NumPolynomialCoefficients(size_t deg) {
    return (deg + 1) * (deg + 2) * (deg + 3) / 6;
}

template<size_t polyOrder, bool hardSurfaceConstraint = true>
void basisFunFitBuildA(const Point3f &pos, const Vector3f &inDir, const std::vector<Point3f> &evalP,
                       const Eigen::VectorXi &permX, const Eigen::VectorXi &permY, const Eigen::VectorXi &permZ,
                       const Eigen::VectorXf *weights,
                       Eigen::Matrix<float, Eigen::Dynamic, PolyUtils::nPolyCoeffs(polyOrder,
                                                                                hardSurfaceConstraint)> &A,
                       const Eigen::VectorXf &weightedB, Float scaleFactor) {

    size_t n = evalP.size();
    Eigen::Matrix<float, Eigen::Dynamic, 3 * (polyOrder + 1)> relPosPow(n, 3 * (polyOrder + 1));
    for (size_t i = 0; i < n; ++i) {
        Vector3f rel = (evalP[i] - pos) * scaleFactor;
        for (size_t d = 0; d <= polyOrder; ++d) {
            relPosPow(i, d * 3 + 0) = std::pow(rel.x, d);
            relPosPow(i, d * 3 + 1) = std::pow(rel.y, d);
            relPosPow(i, d * 3 + 2) = std::pow(rel.z, d);
        }
    }

    PBRT_CONSTEXPR int nCoeffs = PolyUtils::nPolyCoeffs(polyOrder, false);
    Eigen::Matrix<float, Eigen::Dynamic, nCoeffs> fullA =
            Eigen::Matrix<float, Eigen::Dynamic, nCoeffs>::Zero(n * 4, nCoeffs);
    size_t termIdx = 0;
    for (size_t d = 0; d <= polyOrder; ++d) {
        for (size_t i = 0; i <= d; ++i) {
            size_t dx = d - i;
            for (size_t j = 0; j <= i; ++j) {
                size_t dy = d - dx - j;
                size_t dz = d - dx - dy;
                Eigen::VectorXf col = relPosPow.col(0 + 3 * dx).array() * relPosPow.col(1 + 3 * dy).array() *
                                      relPosPow.col(2 + 3 * dz).array();
                if (weights) {
                    col = col.array() * (*weights).array();
                }
                fullA.block(0, termIdx, n, 1) = col;
                const int pX = permX[termIdx];
                const int pY = permY[termIdx];
                const int pZ = permZ[termIdx];
                if (pX > 0) {
                    fullA.block(n, pX, n, 1) = (dx + 1) * col;
                }
                if (pY > 0) {
                    fullA.block(2 * n, pY, n, 1) = (dy + 1) * col;
                }
                if (pZ > 0) {
                    fullA.block(3 * n, pZ, n, 1) = (dz + 1) * col;
                }
                ++termIdx;
            }
        }
    }
    A = fullA.block(0, 1, fullA.rows(), fullA.cols() - 1);
}

Eigen::VectorXi DerivPermutationEigen(size_t degree, size_t axis) {
    auto numCoeffs = NumPolynomialCoefficients(degree);
    Eigen::VectorXi permutation = Eigen::VectorXi::Constant(numCoeffs, -1);
    for (size_t d = 0; d <= degree; ++d) {
        for (size_t i = 0; i <= d; ++i) {
            size_t dx = d - i;
            for (size_t j = 0; j <= i; ++j) {
                size_t dy = d - dx - j;
                size_t dz = d - dx - dy;
                Vector3i deg(dx, dy, dz);
                Vector3i derivDeg = deg;
                derivDeg[axis] -= 1;
                if (derivDeg[0] < 0 || derivDeg[1] < 0 || derivDeg[2] < 0) {
                    continue;
                }
                // For a valid derivative: add entry to matrix
                permutation[PolyUtils::PowerToIndex(derivDeg[0], derivDeg[1], derivDeg[2])] = PolyUtils::PowerToIndex(
                        dx, dy, dz);
            }
        }
    }
    return permutation;
}

void ConstraintKDTree::Build(const std::vector<Point3f> &sampledP, const std::vector<Normal3f> &sampledN) {
    int pointsNum = sampledP.size();
    mTree = CTree(pointsNum);
    LOG(INFO) << "Build constraint kd tree: " << pointsNum;
    ParallelFor([&](int64_t i) {
        mTree[i].SetPosition(sampledP[i]);
        ExtraData d;
        d.p = sampledP[i];
        d.n = sampledN[i];
        d.sampledNum = 1;
        d.avgN = Normal3f(-10, -10, -10);
        d.avgP = Point3f(-100, -100, -100);
        mTree[i].SetData(d);
    }, pointsNum, 1024);
    mTree.Build(true);
    CalcAvgValues(mTree[0], 0);
    LOG(INFO) << "Calculated the average value";

    for (size_t i = 0; i < std::min(size_t(32), sampledP.size()); i++) {
        mPoints.push_back(sampledP[i]);
        mNormals.push_back(sampledN[i]);
    }
}

std::tuple<Point3f, Normal3f, size_t> ConstraintKDTree::CalcAvgValues(TreeNode &node, TreeNode::IndexType index) {
    LOG(INFO) << "Calculating average value of index: " << index;
    Point3f rp, lp;
    Normal3f rn, ln;
    size_t rSamples = 0, lSamples = 0;
    if (mTree.HasRightChild(index)) {
        auto rightIdx = node.GetRightIndex(index);
        if (index != rightIdx)
            std::tie(rp, rn, rSamples) = CalcAvgValues(mTree[rightIdx], rightIdx);
    }

    if (!node.IsLeaf()) {
        auto leftIdx = node.GetLeftIndex(index);
        if (index != leftIdx)
            std::tie(lp, ln, lSamples) = CalcAvgValues(mTree[leftIdx], leftIdx);
    }

    size_t allSamples = lSamples + rSamples + 1;
    Point3f avgP = (node.GetData().p + ((Float) rSamples) * rp + ((Float) lSamples) * lp) / ((Float) allSamples);
    Normal3f avgN = (node.GetData().n + ((Float) rSamples) * rn + ((Float) lSamples) * ln) / ((Float) allSamples);

    node.GetData().avgP = avgP;
    node.GetData().avgN = avgN;
    node.GetData().sampledNum = allSamples;
    return std::make_tuple(avgP, avgN, allSamples);
}

std::tuple<std::vector<Point3f>, std::vector<Normal3f>, std::vector<Float>>
ConstraintKDTree::GetConstraints(const Point3f &p, Float kernelEps,
                                 const std::function<Float(Float, Float)> &kernel) const {
    // Extract constraints from KD Tree by traversing from the top
    std::vector<Point3f> positions;
    std::vector<Normal3f> normals;
    std::vector<Float> sampleWeights;
    GetConstraints(p, mTree[0], 0, mTree.GetAABB(), positions, normals, sampleWeights, kernelEps, kernel);

    // Add the super constraints
    // FIXME: Why add samped P here?
    // for (size_t i = 0; i < mPoints.size(); ++i) {
    //     positions.push_back(mPoints[i]);
    //     normals.push_back(mNormals[i]);
    //     sampleWeights.push_back(-1.0f);
    // }
    return std::make_tuple(positions, normals, sampleWeights);
}

void ConstraintKDTree::GetConstraints(const Point3f &p, SimpleKDNode<Point3f, pbrt::ConstraintKDTree::ExtraData> node,
                                      TreeNode::IndexType index, const Bounds3f &aabb, std::vector<Point3f> &points,
                                      std::vector<Normal3f> &normals, std::vector<Float> &sampleWeights,
                                      pbrt::Float kernelEps,
                                      const std::function<Float(Float, Float)> &kernelFunc) const {
    Float dist2Threshold = 9.0 * kernelEps;
    Float distSq =pbrt::DistanceSquared(p, aabb);
    if (distSq > dist2Threshold) {
        return;
    }
    if (distSq < dist2Threshold) {
        points.push_back(node.GetData().p);
        normals.push_back(node.GetData().n);
        sampleWeights.push_back(1.0f);
    }

    if (node.IsLeaf())
        return;

    // Compute bounding box of child nodes
    uint16_t ax = node.GetAxis();
    Point3f leftMin = aabb.pMin;
    Point3f leftMax = aabb.pMax;
    leftMax[ax] = node.GetPos()[ax];
    Point3f rightMin = aabb.pMin;
    Point3f rightMax = aabb.pMax;
    rightMin[ax] = node.GetPos()[ax];
    Bounds3f leftBounds(leftMin, leftMax);
    Bounds3f rightBounds(rightMin, rightMax);
    if (mTree.HasRightChild(index)) {
        auto rightIdx = node.GetRightIndex(index);
        if (rightIdx != index)
            GetConstraints(p, mTree[rightIdx], rightIdx, rightBounds, points, normals, sampleWeights, kernelEps,
                           kernelFunc);
    }
    // Node always has left child by construction
    auto leftIdx = node.GetLeftIndex(index);
    if(leftIdx != index)
        GetConstraints(p, mTree[leftIdx], leftIdx, leftBounds, points, normals, sampleWeights, kernelEps, kernelFunc);
}

Float PolyUtils::GetKernelEps(const MediumParameters &mediumParas, int channel,
                              pbrt::Float kernelMultiplier) {
    // The kernel epsilon size is related to the medium's properties
    Float sigmaT = mediumParas.sigmaT[channel];
    Float albedo = mediumParas.albedo[channel];
    Float g = mediumParas.g;
    Float sigmaS = albedo * sigmaT;
    Float sigmaa = sigmaT - sigmaS;
    Float sigmaSp = (1 - g) * sigmaS;
    Float sigmaTp = sigmaSp + sigmaa;
    Float alphaP = sigmaSp / sigmaTp;
    Float effAlphaP = pbrt::EffectiveAlbedo(alphaP);
    Float val = 0.25f * g + 0.25 * alphaP + 1.0 * effAlphaP;
    return kernelMultiplier * 4.0f * val * val / (sigmaTp * sigmaTp);
}

std::tuple<PolyUtils::Polynomial, std::vector<Point3f>, std::vector<Normal3f>>
PolyUtils::FitPolynomial(const PolyFitRecord &polyFitRecord, const ConstraintKDTree *kdTree) {
    if (polyFitRecord.config.hardSurfaceConstraint) {
        if (polyFitRecord.config.order == 2) {
            return FitPolynomialImpl<2, true>(polyFitRecord, kdTree);
        } else {
            return FitPolynomialImpl<3, true>(polyFitRecord, kdTree);
        }
    } else {
        LOG(ERROR) << "Polynomial without hard surface constraint is unsupported.";
        exit(1);
    }
}

template<size_t polyOrder, bool hardSurfaceConstraint>
std::tuple<PolyUtils::Polynomial, std::vector<Point3f>, std::vector<Normal3f>>
PolyUtils::FitPolynomialImpl(const PolyFitRecord &pfRec, const ConstraintKDTree *kdTree) {
    Float kernelEps = pfRec.kernelEps;
    std::function<Float(Float, Float)> kernelFunc = PolyUtils::GaussianKernel;
    std::vector<Point3f> posConstraints;
    std::vector<Normal3f> normConstraints;
    std::vector<Float> sampleWeights;

    // Problems may come from here.
    std::tie(posConstraints, normConstraints, sampleWeights) = kdTree->GetConstraints(pfRec.p, kernelEps, kernelFunc);

    size_t n = posConstraints.size();
    float invSqrtN = 1.0f / std::sqrt(n);

    Eigen::VectorXf weights(n);
    Vector3f s, t;
    Normal3f pfd = Normal3f(pfRec.d);
    OnbDuff(pfd, s, t);
    PolyUtils::Frame local(s, t, pfd);

    bool useLightspace = pfRec.config.useLightspace;

    for (size_t i = 0; i < n; i++) {
        Float d2 = DistanceSquared(pfRec.p, posConstraints[i]), w;
        if (sampleWeights[i] < 0) {
            w = pfRec.config.globalConstraintWeight * std::sqrt(1.0f / 32.0f);
        } else {
            w = std::sqrt(kernelFunc(d2, kernelEps) * sampleWeights[i]) * invSqrtN;
        }

        weights[i] = w;
        if (useLightspace) {
            auto localPos = local.toLocal(posConstraints[i] - pfRec.p);
            posConstraints[i] = Point3f(localPos.x, localPos.y, localPos.z);
        }
    }

    Eigen::VectorXf weightedB(4 * n);
    for (size_t i = 0; i < n; i++) {
        Normal3f normal = normConstraints[i];
        if (useLightspace) {
            normal = Normal3f(local.toLocal(Vector3f(normal)));
        }
        weightedB[i] = 0.0f;
        weightedB[i + 1 * n] = normal.x * weights[i];
        weightedB[i + 2 * n] = normal.y * weights[i];
        weightedB[i + 3 * n] = normal.z * weights[i];
    }

    Eigen::VectorXi pX = DerivPermutationEigen(polyOrder, 0);
    Eigen::VectorXi pY = DerivPermutationEigen(polyOrder, 1);
    Eigen::VectorXi pZ = DerivPermutationEigen(polyOrder, 2);

    PBRT_CONSTEXPR size_t nCoeffs = nPolyCoeffs(polyOrder, hardSurfaceConstraint);

    Eigen::Matrix<float, Eigen::Dynamic, nCoeffs> A(4 * n, nCoeffs);
    Eigen::Matrix<float, nCoeffs, nCoeffs> AtA(nCoeffs, nCoeffs);
    // This scale factor seems to lead to a well behaved fit in many different settings
    float fitScaleFactor = PolyUtils::GetFitScaleFactor(kernelEps);
    Vector3f usedRefDir = useLightspace ? local.toLocal((Vector3f) pfRec.n) : (Vector3f) pfRec.n;

    if (useLightspace) {
        basisFunFitBuildA<polyOrder, hardSurfaceConstraint>(Point3f(), usedRefDir, posConstraints,
                                                            pX, pY, pZ, &weights, A, weightedB, fitScaleFactor);
    } else {
        basisFunFitBuildA<polyOrder, hardSurfaceConstraint>(pfRec.p, usedRefDir, posConstraints, pX, pY, pZ,
                                                            &weights, A, weightedB, fitScaleFactor);
    }
    Eigen::Matrix<float, nCoeffs, 1> Atb = A.transpose() * weightedB;

    Eigen::VectorXf coeffs;
    if (pfRec.config.useSvd) {
        Eigen::MatrixXf ADyn = A;
        Eigen::BDCSVD<Eigen::MatrixXf> svd = ADyn.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
        const Eigen::VectorXf &sVal = svd.singularValues();
        float eps = 0.01f;
        coeffs = svd.matrixV() * ((sVal.array() > eps).select(sVal.array().inverse(), 0)).matrix().asDiagonal() *
                 svd.matrixU().transpose() * weightedB;
    } else {
        Eigen::MatrixXf reg = Eigen::MatrixXf::Identity(A.cols(), A.cols()) * pfRec.config.regularization;
        reg(0, 0) = 0.0f;
        reg(1, 1) = 0.0f;
        reg(2, 2) = 0.0f;
        AtA = A.transpose() * A + reg;
        coeffs = AtA.ldlt().solve(Atb);
    }

    std::vector<Float> coeffsVec(NumPolynomialCoefficients(polyOrder));
    size_t cfSize = coeffsVec.size();
    if (hardSurfaceConstraint) {
        coeffsVec[0] = 0.0f;
        for (size_t i = 1; i < coeffs.size(); ++i)
            coeffsVec[i] = coeffs[i - 1];
    } else {
        for (size_t i = 0; i < coeffs.size(); ++i)
            coeffsVec[i] = coeffs[i];
    }

    PolyUtils::Polynomial poly;
    poly.coeffs = coeffsVec;
    poly.refPos = pfRec.p;
    poly.refDir = pfRec.d;
    poly.useLocalDir = pfRec.config.useLightspace;
    poly.scaleFactor = PolyUtils::GetFitScaleFactor(kernelEps);
    poly.order = polyOrder;
    return std::make_tuple(poly, posConstraints, normConstraints);
}

const static int PERMX2[10] = {1, 4, 5, 6, -1, -1, -1, -1, -1, -1};
const static int PERMY2[10] = {2, 5, 7, 8, -1, -1, -1, -1, -1, -1};
const static int PERMZ2[10] = {3, 6, 8, 9, -1, -1, -1, -1, -1, -1};
const static int PERMX3[20] = {1, 4, 5, 6, 10, 11, 12, 13, 14, 15, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1};
const static int PERMY3[20] = {2, 5, 7, 8, 11, 13, 14, 16, 17, 18, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1};
const static int PERMZ3[20] = {3, 6, 8, 9, 12, 14, 15, 17, 18, 19, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1};

const int *DerivPermutation(size_t degree, size_t axis) {
    switch (axis) {
        case 0:
            return degree == 2 ? PERMX2 : PERMX3;
        case 1:
            return degree == 2 ? PERMY2 : PERMY3;
        case 2:
            return degree == 2 ? PERMZ2 : PERMZ3;
        default:
            return nullptr;
    }
}

template<typename T>
std::pair<float, Vector3f> EvalPolyGrad(const Point3f &pos,
                                        const Point3f &evalP, size_t degree,
                                        const int *permX, const int *permY, const int *permZ,
                                        Float scaleFactor, bool useLocalDir, const Vector3f &refDir,
                                        const T &coeffs) {
    Vector3f relPos;
    if (useLocalDir) {
        Vector3f s, t;
        PolyUtils::OnbDuff(Normal3f(refDir), s, t);
        PolyUtils::Frame local(s, t, Normal3f(refDir));
        relPos = local.toLocal(Vector3f(evalP - pos)) * scaleFactor;
    } else {
        relPos = Vector3f(evalP - pos) * scaleFactor;
    }

    size_t termIdx = 0;
    Vector3f deriv(0.0f, 0.0f, 0.0f);
    float value = 0.0f;

    Float xPowers[4] = {1.0, relPos.x, relPos.x * relPos.x, relPos.x * relPos.x * relPos.x};
    Float yPowers[4] = {1.0, relPos.y, relPos.y * relPos.y, relPos.y * relPos.y * relPos.y};
    Float zPowers[4] = {1.0, relPos.z, relPos.z * relPos.z, relPos.z * relPos.z * relPos.z};
    for (size_t d = 0; d <= degree; ++d) {
        for (size_t i = 0; i <= d; ++i) {
            size_t dx = d - i;
            for (size_t j = 0; j <= i; ++j) {
                size_t dy = d - dx - j;
                size_t dz = d - dx - dy;
                float t = xPowers[dx] * yPowers[dy] * zPowers[dz];
                value += coeffs[termIdx] * t;

                int pX = permX[termIdx];
                int pY = permY[termIdx];
                int pZ = permZ[termIdx];
                if (pX > 0)
                    deriv.x += ((float)dx + 1.0f) * t * coeffs[pX];
                if (pY > 0)
                    deriv.y += ((float)dy + 1.0f) * t * coeffs[pY];
                if (pZ > 0)
                    deriv.z += ((float)dz + 1.0f) * t * coeffs[pZ];
                ++termIdx;
            }
        }
    }
    return std::make_pair(value, deriv);
}

void PolyUtils::ProjectPointsToSurface(const Scene *scene, const Point3f &refPoint, const Vector3f &refDir,
                                       ScatterSamplingRecord &rec, const Eigen::VectorXf &polyCoefficients,
                                       size_t polyOrder, bool useLocalDir, Float scaleFactor, Float kernelEps,
                                       SurfaceInteraction *res) {
    if (!rec.isValid)
        return;

    Vector3f dir = EvaluateGradient(refPoint, polyCoefficients, rec, polyOrder, scaleFactor, useLocalDir, refDir);
    Float dists[2] = {2 * kernelEps, std::numeric_limits<Float>::infinity()};

    // FIXME: Adjust the dir.
    dir = Normalize(dir);

    for (int i = 0; i < 2; i++) {
        Float maxDist = dists[i];
        Ray r1(rec.p, dir); // FIXME: Add maxDist?
        SurfaceInteraction its, its2;
        Point3f projectedP;
        Normal3f normal;
        Float pointTime = -1.0f;
        bool itsFound = false;
        if (scene->Intersect(r1, &its)) {
            projectedP = its.p;
            normal = its.shading.n; // FIXME: Note the difference between shading.n and n
            pointTime = its.time;
            itsFound = true;
            *res = its;
        }
        Float maxT = itsFound ? its.time : maxDist;
        Ray r2(rec.p, -dir);
        if (scene->Intersect(r2, &its2)) {
            if (pointTime < 0 || pointTime > its2.time) {
                projectedP = its2.p;
                normal = its2.shading.n;
            }
            itsFound = true;
            *res = its2;
        }
        rec.isValid = itsFound;
        if (itsFound) {
            rec.p = projectedP;
            rec.n = normal;
            return;
        }
    }
}

Vector3f
PolyUtils::EvaluateGradient(const Point3f &pos, const Eigen::VectorXf &coeffs, const ScatterSamplingRecord &rec,
                            size_t degree, Float scaleFactor,
                            bool useLocalDir, const Vector3f &refDir) {
    const int *permX = DerivPermutation(degree, 0);
    const int *permY = DerivPermutation(degree, 1);
    const int *permZ = DerivPermutation(degree, 2);
    Float polyVal;
    Vector3f gradient;
    std::tie(polyVal, gradient) = EvalPolyGrad(pos, rec.p, degree, permX, permY, permZ, scaleFactor, useLocalDir,
                                               refDir, coeffs);
    if (useLocalDir) {
        Vector3f s, t;
        PolyUtils::OnbDuff(Normal3f(refDir), s, t);
        PolyUtils::Frame local(s, t, Normal3f(refDir));
        gradient = local.toWorld(gradient);
    }
    // FIXME: May be bug here.
    return gradient;
}

Normal3f PolyUtils::AdjustRayDirForPolynomialTracing(Vector3f &inDir, const SurfaceInteraction &isect, int polyOrder,
                                                     Float polyScaleFactor, int channel) {
    const int *pX = DerivPermutation(polyOrder, 0);
    const int *pY = DerivPermutation(polyOrder, 1);
    const int *pZ = DerivPermutation(polyOrder, 2);

    float polyValue;
    Vector3f polyNormal;
    std::tie(polyValue, polyNormal) = EvalPolyGrad(isect.p, isect.p, polyOrder, pX, pY, pZ,
                                                   polyScaleFactor, false,
                                                   inDir, isect.polyStorage->coeffs[channel]);
    Vector3f rotationAxis = pbrt::Cross(isect.shading.n, polyNormal);
    if (rotationAxis.Length() < 1e-8f) {
        return (Normal3f) polyNormal;
    }
    Normal3f normalizedTarget = isect.shading.n;
    float angle = acos(std::max(std::min(Dot(polyNormal, normalizedTarget), 1.0f), -1.0f));
    Transform transf = pbrt::Rotate(Degrees(angle), rotationAxis);
    inDir = transf(inDir);
    return Normal3f(polyNormal);
}

} // namespace pbrt.


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

#include <quaternion_operation/quaternion_operation.h>

#include <boost/assert.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/algorithms/disjoint.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <iostream>
#include <traffic_simulator/math/collision.hpp>
#include <vector>

namespace traffic_simulator
{
namespace math
{
bool checkCollision2D(
  geometry_msgs::msg::Pose pose0, openscenario_msgs::msg::BoundingBox bbox0,
  geometry_msgs::msg::Pose pose1, openscenario_msgs::msg::BoundingBox bbox1)
{
  double z_diff_pose =
    std::fabs((pose0.position.z + bbox0.center.z) - (pose1.position.z + bbox1.center.z));
  if (z_diff_pose > (std::fabs(bbox0.dimensions.z + bbox1.dimensions.z) * 0.5)) {
    return false;
  }
  namespace bg = boost::geometry;
  typedef bg::model::d2::point_xy<double> bg_point;
  const bg::model::polygon<bg_point> poly0 = get2DPolygon(pose0, bbox0);
  const bg::model::polygon<bg_point> poly1 = get2DPolygon(pose1, bbox1);
  if (bg::intersects(poly0, poly1)) {
    return true;
  }
  if (bg::intersects(poly1, poly0)) {
    return true;
  }
  if (bg::disjoint(poly0, poly1)) {
    return false;
  }
  return true;
}
}  // namespace math
}  // namespace traffic_simulator


/*=========================================================================

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
/*=========================================================================
  The Software is copyright (c) Commonwealth Scientific and Industrial Research Organisation (CSIRO)
  ABN 41 687 119 230.
  All rights reserved.

  Licensed under the CSIRO BSD 3-Clause License
  You may not use this file except in compliance with the License.
  You may obtain a copy of the License in the file LICENSE.md or at

  https://stash.csiro.au/projects/SMILI/repos/smili/browse/license.txt

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.
=========================================================================*/
#include "vtkDiffusionTensorGlyphFilter.h"

#include <boost/math/special_functions/spherical_harmonic.hpp>

#include "vtkObjectFactory.h"
#include "vtkCell.h"
#include "vtkCellData.h"
#include "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkPointData.h"
#include "vtkPolyData.h"
#include "vtkUnsignedCharArray.h"
#include "vtkAppendPolyData.h"

vtkStandardNewMacro(vtkDiffusionTensorGlyphFilter);

vtkDiffusionTensorGlyphFilter::vtkDiffusionTensorGlyphFilter() : vtkProgrammableGlyphFilter()
{
  m_Resolution = 16;
}

vtkDiffusionTensorGlyphFilter::~vtkDiffusionTensorGlyphFilter()
{

}

double vtkDiffusionTensorGlyphFilter::computeAmplitude(std::vector<double> SH, double x, double y, double z, int lmax)
{
    double az = atan2(y, x);
    double el = acos(z);
    double val = 0.0;
    for (int l = 0; l <= lmax; l+=2)
    {
        // val += SH[vtkDiffusionTensorGlyphFilter::index(l,0)] * boost::math::legendre_p<double>(l, 0, z);
        val += SH[vtkDiffusionTensorGlyphFilter::index(l,0)] * boost::math::spherical_harmonic_r<double>(l, 0, el,az);
    }

    for (int m = 1; m <= lmax; m++)
    {
//        float caz = cos(m*az);
//        float saz = sin(m*az);
        for (int l = 2*((m+1)/2); l <= lmax; l+=2)
        {
        // float buf = boost::math::legendre_p<double>(l, 0, z);
        // val += SH[index(l,m)]*buf*caz;
        // val += SH[index(l,-m)]*buf*saz;
            val += SH[vtkDiffusionTensorGlyphFilter::index(l,m)]*boost::math::spherical_harmonic_r(l,m,el,az);
            val += SH[vtkDiffusionTensorGlyphFilter::index(l,-m)]*boost::math::spherical_harmonic_i(l,m,el,az);
        }
    }

    return val;
}

//Overide the vtkProgrammableGlyphFilter due to colouring not being unsigned char etc.
/*int vtkDiffusionTensorGlyphFilter::RequestData(
  vtkInformation *vtkNotUsed(request),
  vtkInformationVector **inputVector,
  vtkInformationVector *outputVector)
{
  // get the info objects
  vtkInformation *inInfo = inputVector[0]->GetInformationObject(0);
  vtkInformation *sourceInfo = inputVector[1]->GetInformationObject(0);
  vtkInformation *outInfo = outputVector->GetInformationObject(0);

  // get the input and output
  vtkDataSet *input = vtkDataSet::SafeDownCast(
    inInfo->Get(vtkDataObject::DATA_OBJECT()));
  vtkPolyData *source = vtkPolyData::SafeDownCast(
    sourceInfo->Get(vtkDataObject::DATA_OBJECT()));
  vtkPolyData *output = vtkPolyData::SafeDownCast(
    outInfo->Get(vtkDataObject::DATA_OBJECT()));

  vtkPointData *inputPD = input->GetPointData();
  vtkCellData *inputCD = input->GetCellData();
  vtkPointData *outputPD = output->GetPointData();
  vtkCellData *outputCD = output->GetCellData();
  vtkPoints *newPts, *sourcePts;
  vtkUnsignedCharArray *ptScalars=NULL, *cellScalars=NULL; //CHANGED
  vtkDataArray *inPtScalars = NULL, *inCellScalars = NULL;
  vtkIdType numPts = input->GetNumberOfPoints();
  vtkPointData *sourcePD;
  vtkCellData *sourceCD;
  vtkIdType numSourcePts, numSourceCells, ptOffset=0, cellId, ptId, id, idx;
  int i, npts;
  vtkIdList *pts;
  vtkIdList *cellPts;
  vtkCell *cell;

  // Initialize
  vtkDebugMacro(<<"Generating programmable glyphs!");
  std::cout << "Generating Diffusion programmable glyphs!" << std::endl;

  if ( numPts < 1 )
    {
    vtkErrorMacro(<<"No input points to glyph");
    }

  pts=vtkIdList::New();
  pts->Allocate(VTK_CELL_SIZE);
  sourcePD = source->GetPointData();
  sourceCD = source->GetCellData();
  numSourcePts = source->GetNumberOfPoints();
  numSourceCells = source->GetNumberOfCells();

  outputPD->CopyScalarsOff(); //'cause we control the coloring process
  outputCD->CopyScalarsOff();

  output->Allocate(numSourceCells*numPts,numSourceCells*numPts);
  outputPD->CopyAllocate(sourcePD, numSourcePts*numPts, numSourcePts*numPts); //causes float array allocation
  outputCD->CopyAllocate(sourceCD, numSourceCells*numPts, numSourceCells*numPts);
  newPts = vtkPoints::New();
  newPts->Allocate(numSourcePts*numPts);

  // figure out how to color the data and setup
  if ( this->ColorMode == VTK_COLOR_BY_INPUT )
    {
    if ( (inPtScalars = inputPD->GetScalars()) )
      {
      ptScalars = vtkUnsignedCharArray::New();
      ptScalars->SetNumberOfComponents(3); //CHANGED
      ptScalars->SetName("Colours");
      ptScalars->SetNumberOfTuples(numSourcePts*numPts);
      }
    if ( (inCellScalars = inputCD->GetScalars()) )
      {
      cellScalars = vtkUnsignedCharArray::New();
      cellScalars->SetNumberOfComponents(3); //CHANGED
      cellScalars->SetNumberOfTuples(numSourcePts*numPts);
      }
    }

  else
    {
    if ( sourcePD->GetScalars() )
      {
      ptScalars = vtkUnsignedCharArray::New();
      ptScalars->SetNumberOfComponents(3); //CHANGED
      ptScalars->SetName("Colours");
      ptScalars->SetNumberOfTuples(numSourcePts*numPts);
      }
    if ( sourceCD->GetScalars() )
      {
      cellScalars = vtkUnsignedCharArray::New();
      cellScalars->SetNumberOfComponents(3); //CHANGED
      cellScalars->SetNumberOfTuples(numSourcePts*numPts);
      }
    }

  // Loop over all points, invoking glyph method and Update(),
  // then append output of source to output of this filter.
  //
//  this->Updating = 1; // to prevent infinite recursion
  this->PointData = input->GetPointData();
  for (this->PointId=0; this->PointId < numPts; this->PointId++)
    {
    if ( ! (this->PointId % 10000) )
      {
      this->UpdateProgress ((double)this->PointId/numPts);
      if (this->GetAbortExecute())
        {
        break;
        }
      }

    input->GetPoint(this->PointId, this->Point);

    if ( this->GlyphMethod )
      {
      (*this->GlyphMethod)(this->GlyphMethodArg);

      // The GlyphMethod may have set the source connection to NULL
      if (this->GetNumberOfInputConnections(1) == 0)
        {
        source = NULL;
        }
      else
        {
        // Update the source connection in case the GlyphMethod changed
        // its parameters.
        this->GetInputAlgorithm(1, 0)->Update();
        // The GlyphMethod may also have changed the source.
        sourceInfo = inputVector[1]->GetInformationObject(0);
        source = vtkPolyData::SafeDownCast(
          sourceInfo->Get(vtkDataObject::DATA_OBJECT()));
        }
      }
    if (source)
      {
      sourcePts = source->GetPoints();
      numSourcePts = source->GetNumberOfPoints();
      numSourceCells = source->GetNumberOfCells();
      sourcePD = source->GetPointData();
      sourceCD = source->GetCellData();

      if ( this->ColorMode == VTK_COLOR_BY_SOURCE )
        {
        inPtScalars = sourcePD->GetScalars();
        inCellScalars = sourceCD->GetScalars();
        }

      // Copy all data from source to output.
      for (ptId=0; ptId < numSourcePts; ptId++)
        {
        id = newPts->InsertNextPoint(sourcePts->GetPoint(ptId));
        outputPD->CopyData(sourcePD, ptId, id);
        }

      for (cellId=0; cellId < numSourceCells; cellId++)
        {
        cell = source->GetCell(cellId);
        cellPts = cell->GetPointIds();
        npts = cellPts->GetNumberOfIds();
        for (pts->Reset(), i=0; i < npts; i++)
          {
          pts->InsertId(i,cellPts->GetId(i) + ptOffset);
          }
        id = output->InsertNextCell(cell->GetCellType(),pts);
        outputCD->CopyData(sourceCD, cellId, id);
        }

      // If we're coloring the output with scalars, do that now
      if ( ptScalars )
        {
        for (ptId=0; ptId < numSourcePts; ptId++)
          {
          idx = (this->ColorMode == VTK_COLOR_BY_INPUT ? this->PointId : ptId);
          unsigned char colourOfPoint[3] = {0, 0, 0};
          colourOfPoint[0] = static_cast<unsigned char>( inPtScalars->GetComponent(idx, 0) );
          colourOfPoint[1] = static_cast<unsigned char>( inPtScalars->GetComponent(idx, 1) );
          colourOfPoint[2] = static_cast<unsigned char>( inPtScalars->GetComponent(idx, 2) );
          ptScalars->InsertNextTupleValue(colourOfPoint); //CHANGED
          }
        }
      else if ( cellScalars )
        {
        for (cellId=0; cellId < numSourceCells; cellId++)
          {
          idx = (this->ColorMode == VTK_COLOR_BY_INPUT ? this->PointId : cellId);
          unsigned char colourOfCell[3] = {0, 0, 0};
          colourOfCell[0] = static_cast<unsigned char>( inCellScalars->GetComponent(idx, 0) );
          colourOfCell[1] = static_cast<unsigned char>( inCellScalars->GetComponent(idx, 1) );
          colourOfCell[2] = static_cast<unsigned char>( inCellScalars->GetComponent(idx, 2) );
          cellScalars->InsertNextTupleValue(colourOfCell); //CHANGED
          }
        }

      ptOffset += numSourcePts;

      }//if a source is available
    } //for all input points

//  this->Updating = 0;

  pts->Delete();

  output->SetPoints(newPts);
  newPts->Delete();

  if ( ptScalars )
    {
    idx = outputPD->AddArray(ptScalars);
    outputPD->SetActiveAttribute(idx, vtkDataSetAttributes::SCALARS);
    ptScalars->Delete();
    }

  if ( cellScalars )
    {
    idx = outputCD->AddArray(cellScalars);
    outputCD->SetActiveAttribute(idx, vtkDataSetAttributes::SCALARS);
    cellScalars->Delete();
    }

  output->Squeeze();

  return 1;
}*/
int vtkDiffusionTensorGlyphFilter::RequestData(
  vtkInformation *vtkNotUsed(request),
  vtkInformationVector **inputVector,
  vtkInformationVector *outputVector)
{
  // get the info objects
  vtkInformation *inInfo = inputVector[0]->GetInformationObject(0);
  vtkInformation *sourceInfo = inputVector[1]->GetInformationObject(0);
  vtkInformation *outInfo = outputVector->GetInformationObject(0);

  // get the input and output
  vtkDataSet *input = vtkDataSet::SafeDownCast(
    inInfo->Get(vtkDataObject::DATA_OBJECT()));
  vtkPolyData *source = vtkPolyData::SafeDownCast(
    sourceInfo->Get(vtkDataObject::DATA_OBJECT()));
  vtkPolyData *output = vtkPolyData::SafeDownCast(
    outInfo->Get(vtkDataObject::DATA_OBJECT()));

  vtkPointData *inputPD = input->GetPointData();
  vtkCellData *inputCD = input->GetCellData();
  vtkIdType numPts = input->GetNumberOfPoints();
  vtkPointData *sourcePD;
  vtkCellData *sourceCD;
  vtkIdType numSourcePts, numSourceCells, ptOffset=0, cellId, ptId, id, idx;

  vtkSmartPointer<vtkAppendPolyData> appendedOutput = vtkSmartPointer<vtkAppendPolyData>::New();
  #if VTK_MAJOR_VERSION <= 5
    appendedOutput->AddInput(output);
  #else
    appendedOutput->AddInputData(output);
  #endif

  // Initialize
  vtkDebugMacro(<<"Generating programmable glyphs!");

  if ( numPts < 1 )
    {
    vtkErrorMacro(<<"No input points to glyph");
    }

  sourcePD = source->GetPointData();
  sourceCD = source->GetCellData();
  numSourcePts = source->GetNumberOfPoints();
  numSourceCells = source->GetNumberOfCells();

  // Loop over all points, invoking glyph method and Update(),
  // then append output of source to output of this filter.
  //
//  this->Updating = 1; // to prevent infinite recursion
  this->PointData = input->GetPointData();
  for (this->PointId=0; this->PointId < numPts; this->PointId++)
    {
    if ( ! (this->PointId % 10000) )
      {
      this->UpdateProgress (static_cast<long double>(this->PointId)/numPts);
      if (this->GetAbortExecute())
        {
        break;
        }
      }

    input->GetPoint(this->PointId, this->Point);

    if ( this->GlyphMethod )
      {
      (*this->GlyphMethod)(this->GlyphMethodArg);

    #if VTK_MAJOR_VERSION > 5
      // The GlyphMethod may have set the source connection to NULL
      if (this->GetNumberOfInputConnections(1) == 0)
        {
        source = NULL;
        }
      else
        {
        // Update the source connection in case the GlyphMethod changed
        // its parameters.
        this->GetInputAlgorithm(1, 0)->Update();
        // The GlyphMethod may also have changed the source.
        sourceInfo = inputVector[1]->GetInformationObject(0);
        source = vtkPolyData::SafeDownCast(
          sourceInfo->Get(vtkDataObject::DATA_OBJECT()));
        }
    #endif
      }
    if (source)
      {
      numSourcePts = source->GetNumberOfPoints();
      numSourceCells = source->GetNumberOfCells();
      sourcePD = source->GetPointData();
      sourceCD = source->GetCellData();

      #if VTK_MAJOR_VERSION <= 5
        appendedOutput->AddInput(source);
      #else
        appendedOutput->AddInputData(source);
      #endif

      ptOffset += numSourcePts;
      }//if a source is available
    } //for all input points

//  this->Updating = 0;
  appendedOutput->Update();
  this->UpdateProgress(100);
  output->DeepCopy(appendedOutput->GetOutput());
  output->Squeeze();

  return 1;
}

void vtkDiffusionTensorGlyphFilter::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os,indent);

  os << indent << "Color Mode: " << this->GetColorModeAsString() << endl;
  os << indent << "Point Id: " << this->PointId << "\n";
  os << indent << "Point: " << this->Point[0]
                    << ", " << this->Point[1]
                    << ", " << this->Point[2] << "\n";
  if (this->PointData)
    {
    os << indent << "PointData: " << this->PointData << "\n";
    }
  else
    {
    os << indent << "PointData: (not defined)\n";
    }

  if ( this->GlyphMethod )
    {
    os << indent << "Glyph Method defined\n";
    }
  else
    {
    os << indent << "No Glyph Method\n";
    }
}


#ifndef BART_SRC_MATERIAL_MATERIAL_I_HPP_
#define BART_SRC_MATERIAL_MATERIAL_I_HPP_

#include <unordered_map>
#include <vector>

#include <deal.II/lac/full_matrix.h>

//! Material data
namespace bart::data::material {

//! Defines the interface for Material classes providing material properties
/*!
 * The material properties are used almost exclusively to get cross-sections, but other types of data can be stored
 * if needed.
*/
class MaterialI {
 public:
  //! Dealii full matrix type
  using DealiiMatrix = dealii::FullMatrix<double>;
  //! Maps material ID (int) to another type
  template <typename MappedType>
  using MaterialIDMappedTo = std::unordered_map<int, MappedType>;
  virtual ~MaterialI() = default;
  /*!
    returns an unordered_map from material ID to a
    boolean that is true if the material was labeled fissile
  */
  virtual auto GetFissileIDMap() const -> MaterialIDMappedTo<bool> = 0;
  //! Returns diffusion coefficient or 0 if not provided
  virtual auto GetDiffusionCoef() const -> MaterialIDMappedTo<std::vector<double>> = 0;
  //! Returns all \f$\sigma_\mathrm{t}\f$ for all groups.
  virtual auto GetSigT() const -> MaterialIDMappedTo<std::vector<double>>  = 0;
  //! Returns all \f$1/\sigma_\mathrm{t}\f$ for all groups.
  virtual auto GetInvSigT() const -> MaterialIDMappedTo<std::vector<double>> = 0;
  //! Returns all fixed source value \f$Q\f$'s for all groups.
  virtual auto GetQ() const -> MaterialIDMappedTo<std::vector<double>> = 0;
  //! Returns all \f$Q/(4\pi)\f$'s for all groups.
  virtual auto GetQPerSter() const -> MaterialIDMappedTo<std::vector<double>> = 0;
  //! Returns all \f$\nu\sigma_\mathrm{f}\f$'s.
  virtual auto GetNuSigF() const -> MaterialIDMappedTo<std::vector<double>> = 0;
  //! Returns all scattering transfer matrices.
  virtual auto GetSigS() const -> MaterialIDMappedTo<DealiiMatrix> = 0;
  //! Returns all scattering transfer matrices scaled by \f$4\pi\f$.
  virtual auto GetSigSPerSter() const -> MaterialIDMappedTo<DealiiMatrix> = 0;
  //! Returns fission transfer matrix \f$\chi\nu\sigma_\mathrm{f}\f$ for all fissile materials.
  /*! Returns the fission transfer matrix for all fissile materials. Entries
      are given by:
      \f[
      \mathbf{F} = \begin{bmatrix}
      \nu_0\sigma_{f,0}\chi_0 & \nu_0\sigma_{f,0}\chi_1 & \cdots & \nu_0\sigma_{f,0}\chi_G \\
      \nu_1\sigma_{f,1}\chi_0 & \nu_1\sigma_{f,1}\chi_1 & \cdots & \nu_1\sigma_{f,1}\chi_G \\
      \vdots & \vdots & \ddots & \vdots \\
      \nu_G\sigma_{f,G}\chi_0 & \nu_G\sigma_{f,G}\chi_1 & \cdots & \nu_G\sigma_{f,G}\chi_G
      \end{bmatrix}
      \f]
  */
  virtual auto GetChiNuSigF() const -> MaterialIDMappedTo<DealiiMatrix> = 0;
  //! Returns \f$\chi\nu\sigma_\mathrm{f}/(4\pi)\f$ for all fissile materials.
  virtual auto GetChiNuSigFPerSter() const -> MaterialIDMappedTo<DealiiMatrix> = 0;
};

} // namespace bart::data::material

#endif // BART_SRC_MATERIAL_MATERIAL_I_HPP_


/*
 *  testFillFactorJacobians.cpp
 *  Trogdor6
 *
 *  Created by Paul C Hansen on 8/13/11.
 *  Copyright 2011 Stanford University. All rights reserved.
 *
 *  This file is covered by the MIT license.  See LICENSE.txt.
 */

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE testFillFactorJacobians

#include <boost/test/unit_test.hpp>

#include <iostream>
#include <cstdlib>
#include <ctime>
#include "Voxelize.h"
#include "SimpleMesh/Search.h"

using namespace std;
using namespace RLE;
using namespace SimpleMesh;

static vector<ControlVertex> sBlockVertices(Rect3d r);

static vector<Vector3d> sSubtract(const vector<ControlVertex> & cv0,
    const vector<ControlVertex> & cv1, double dh = 1.0)
{
    vector<Vector3d> out(cv0.size());
    assert(cv0.size() == cv1.size());

    for (int nn = 0; nn < cv0.size(); nn++)
        out[nn] = (cv0[nn].point() - cv1[nn].point()) / dh;

    return out;
}

// used in an RLE::transform
static double sDot(Vector3d lhs, Vector3d rhs)
{
//    if (isnan(dot(lhs, rhs)))
//        int blarg = 54;
    return dot(lhs, rhs);
}

static void printJacobians(const map<unsigned int, DynamicRLE3<Vector3d> > & m);

BOOST_AUTO_TEST_CASE( StandardBlock )
{
    Rect3d bounds(-3,-2,-1,3,2,1);
    Rect3i voxels(-1,-1,-1,1,1,1);
    Voxelize vox(bounds, voxels);

    Rect3d rect1(-2, -4.0/3.0, -2.0/3.0, 2, 4.0/3, 2.0/3);

    std::vector<SimpleMesh::ControlVertex> controlVertices1, controlVertices2;
    std::vector<SimpleMesh::Triangle> meshTris1, meshTris2;
    SimpleMesh::Test::oneBoxMesh(controlVertices1, meshTris1, rect1);

    DynamicRLE3<double> fillFactors0 = vox.fillFactors(meshTris1);
    map<unsigned int, DynamicRLE3<Vector3d> > DF = vox.fillFactorJacobians(meshTris1);

//    cerr << "Fill factors #0:\n" << fillFactors0 << "\n";

    double epsilon = 1e-6;
    for (int translateXYZ = 0; translateXYZ < 3; translateXYZ++)
    {
        Rect3d rect2 = rect1 + epsilon*Vector3d::unit(translateXYZ);
        SimpleMesh::Test::oneBoxMesh(controlVertices2, meshTris2, rect2);

        DynamicRLE3<double> fillFactors1 = vox.fillFactors(meshTris2);
        DynamicRLE3<double> deltaFF = (fillFactors1 - fillFactors0);
        vector<Vector3d> deltaCV = SimpleMesh::Test::subtractPositions(controlVertices2, controlVertices1);

        // Test the jacobian now.
        DynamicRLE3<double> ffPredicted(fillFactors0);

        for (int vv = 0; vv < deltaCV.size(); vv++)
        {
            DynamicRLE3<double> dotProd;
            RLE::scalarTransform(DF[vv], deltaCV[vv], dotProd, sDot);
            ffPredicted = ffPredicted + dotProd;

//            cerr << "v" << vv << ": dot =\n" << dotProd << "\n";
        }

//        cerr << "Prediction:\n" << ffPredicted << "\n";

//        cerr << "Derivative measured:\n" << (fillFactors1 - fillFactors0)/epsilon;
//        cerr << "\nDerivative expected:\n" << (ffPredicted - fillFactors0)/epsilon
//            << "\n";

        DynamicRLE3<double> dfPredicted = (ffPredicted - fillFactors0);

        for (int xx = voxels.p1[0]; xx <= voxels.p2[0]; xx++)
        for (int yy = voxels.p1[1]; yy <= voxels.p2[1]; yy++)
        for (int zz = voxels.p1[2]; zz <= voxels.p2[2]; zz++)
        {
            if (dfPredicted.markAt(xx,yy,zz,0.0) != 0.0)
            {
                BOOST_CHECK_CLOSE(dfPredicted.markAt(xx,yy,zz,0.0), deltaFF.markAt(xx,yy,zz,0.0), 1e-4);
            }
            else
            {
                BOOST_CHECK_SMALL(deltaFF.markAt(xx,yy,zz,0.0), 1e-6);
            }
        }
    }
}

BOOST_AUTO_TEST_CASE( HalfFilled )
{
    Rect3d bounds(0,0,0,1,1,1);
//    Rect3i voxels(0,0,0,0,0,0); // one voxel
//    Rect3i voxels(0,0,0,1,0,0); // split X
//    Rect3i voxels(0,0,0,0,1,0); // split Y
//    Rect3i voxels(0,0,-1,0,0,1); // split Z
    Rect3i voxels(-1,-1,-1,1,1,1);
//    Rect3i voxels(0,0,0,1,1,1);
    Voxelize vox(bounds, voxels);

    Rect3d rect0(0.1, 0.1, 0.1, 0.9, 0.9, 0.9); // inset XYZ
//    Rect3d rect0(0.1, 0.1, 0.1, 0.9, 0.9, 0.9);

    std::vector<SimpleMesh::ControlVertex> controlVertices1, controlVertices2;
    std::vector<SimpleMesh::Triangle> meshTris1, meshTris2;
    SimpleMesh::Test::oneBoxMesh(controlVertices1, meshTris1, rect0);
//
//    TrackedPolyhedron block0 = sBlock(rect0);

    DynamicRLE3<double> fillFactors0 = vox.fillFactors(meshTris1);
    map<unsigned int, DynamicRLE3<Vector3d> > DF = vox.fillFactorJacobians(meshTris1);

    double epsilon = 1e-3;
    for (int translateXYZ = 0; translateXYZ < 3; translateXYZ++)
    {
//        cerr << "Translate " << char('x'+translateXYZ) << "\n";
        Rect3d rect1 = rect0 + epsilon*Vector3d::unit(translateXYZ);

//        TrackedPolyhedron block1 = sBlock(rect1);
        SimpleMesh::Test::oneBoxMesh(controlVertices2, meshTris2, rect1);
        DynamicRLE3<double> fillFactors1 = vox.fillFactors(meshTris2);
        vox.fillFactorJacobians(meshTris2);

        //cerr << "Fill factors #1:\n" << fillFactors1 << "\n";

        DynamicRLE3<double> deltaFF = (fillFactors1 - fillFactors0);

        vector<Vector3d> deltaCV = SimpleMesh::Test::subtractPositions(controlVertices2, controlVertices1);

        // Test the jacobian now.
        DynamicRLE3<double> ffPredicted(fillFactors0);

        for (int vv = 0; vv < deltaCV.size(); vv++)
        {
            DynamicRLE3<double> dotProd;
            RLE::scalarTransform(DF[vv], deltaCV[vv], dotProd, sDot);
            ffPredicted = ffPredicted + dotProd;
//            cerr << "v" << vv << ": dot =\n" << dotProd << "\n";
        }

        DynamicRLE3<double> dfPredicted = (ffPredicted - fillFactors0);

        for (int xx = voxels.p1[0]; xx <= voxels.p2[0]; xx++)
        for (int yy = voxels.p1[1]; yy <= voxels.p2[1]; yy++)
        for (int zz = voxels.p1[2]; zz <= voxels.p2[2]; zz++)
        {
            if (dfPredicted.markAt(xx,yy,zz,0.0) != 0.0)
            {
                BOOST_CHECK_CLOSE(dfPredicted.markAt(xx,yy,zz,0.0), deltaFF.markAt(xx,yy,zz,0.0), 1e-4);
            }
            else
            {
                BOOST_CHECK_SMALL(deltaFF.markAt(xx,yy,zz,0.0), 1e-6);
            }
        }
    }

}

static void printJacobians(const map<unsigned int, DynamicRLE3<Vector3d> > & m)
{
    map<unsigned int, DynamicRLE3<Vector3d> >::const_iterator itr;

    for (itr = m.begin(); itr != m.end(); itr++)
    {
        cout << "CV" << itr->first << ": \n";
        cout << itr->second << "\n";
    }
}


#pragma once

#include <fstream>
#include <iostream>
#include <boost/static_assert.hpp>
#include <regex>

// own includes ------------------------------------------------------------
#include "enum/enum.hpp"
#include "spectral_basis.hpp"
#include "spectral_basis_factory_base.hpp"
#include "spectral_elem.hpp"
#include "spectral_elem_accessor.hpp"
#include "spectral_function.hpp"

namespace boltzmann {

namespace local_ {
template <typename BASIS>
struct CMP
{
  /**
   * @brief lexicographical ordering for basis elements
   *
   *
   * @return
   */
  template <typename E>
  bool operator()(const E& e1, const E& e2) const
  {
    auto id1 = e1.get_id();
    auto id2 = e2.get_id();

    if (id1 < id2)
      return true;
    else
      return false;
  }
};
}  // end namespace local_

// ----------------------------------------------------------------------
class SpectralBasisFactoryComplex : public SpectralBasisFactoryBase<XiRC, LaguerreRR>
{
 public:
  /// definition of element ordering, TODO: define this at a global place
  /**
   *
   *
   * @param basis
   * @param K
   * @param L
   * @param beta
   * @param sorted sort basis functions by `l`, the angular index
   */
  static void create(basis_type& basis, int K, int L, double beta, bool sorted = false);
  /**
   *
   *
   * @param basis
   * @param K
   * @param L
   * @param beta
   * @param sorted sort basis functions by `l`, the angular index
   */
  static void create_test(basis_type& basis, int K, int L, double beta, bool sorted = false);

  static void write_basis_descriptor(const basis_type& basis,
                                     std::string fname = "spectral_basis.desc");

  /**
   * @brief read basis_descriptor file
   *
   * @param basis
   * @param descriptor_file
   */
  static void create(basis_type& basis, std::string descriptor_file);
};

// ----------------------------------------------------------------------
void
SpectralBasisFactoryComplex::create(basis_type& basis, int K, int L, double beta, bool sorted)
{
  for (int l = -L; l < L; ++l) {
    for (int k = 0; k < K; ++k) {
      if (k % 2 == std::abs(l) % 2) {
        fa_type xir(l);
        fr_type phi(1. / beta, k);

        basis.add_elem(xir, phi);
      }
    }
  }

  /// sort basis functions by l-index
  // if (sorted) {
  //   basis.sort(local_::CMP<basis_type>());
  // }

  basis.finalize();
}

// ----------------------------------------------------------------------
void
SpectralBasisFactoryComplex::create_test(basis_type& basis, int K, int L, double beta, bool sorted)
{
  for (int l = -L; l < L; ++l) {
    for (int k = 0; k < K; ++k) {
      if (k % 2 == std::abs(l) % 2) {
        fa_type xir(l);
        double fw = 1 / beta;
        /// momentum conservation
        if (k == 1 && l == 1) fw = 0;
        /// mass and energy conservation
        if ((k == 0 || k == 2) && l == 0) fw = 0;
        fr_type phi(fw, k);
        basis.add_elem(xir, phi);
      }
    }
  }

  // /// sort basis functions by l-index
  // if(sorted) {
  //   basis.sort(local_::CMP<basis_type>());
  // }
  basis.finalize();
}

// ----------------------------------------------------------------------
void
SpectralBasisFactoryComplex::write_basis_descriptor(const basis_type& basis, std::string fname)
{
  typename elem_t::Acc::template get<fa_type> xir_getter;
  typename elem_t::Acc::template get<fr_type> rr_getter;

  std::ofstream fout(fname);
  for (auto it = basis.begin(); it != basis.end(); ++it) {
    fout << xir_getter(*it).get_id() << "\t" << rr_getter(*it).get_id() << std::endl;
  }
  fout.close();
}

// ----------------------------------------------------------------------
void
SpectralBasisFactoryComplex::create(basis_type& basis, std::string descriptor_file)
{
  std::ifstream ifile;
  ifile.open(descriptor_file);
  std::string line;

  std::regex basisf_regex(
      "exp_ii_([-]?[0-9]+)[[:space:]]*[(]beta_([0-9.]+),[[:space:]]*k_([0-9]+)[)]");
  // new descriptor
  std::regex basisf_regex_new(
      "exp_ii_([-]?[0-9.]+)[[:space:]]*[(]fw_([0-9.]+),[[:space:]]*k_([0-9]+)[)]");
  std::smatch basisf_match;

  while (std::getline(ifile, line)) {
    std::regex_search(line, basisf_match, basisf_regex);
    if (basisf_match.size() > 0) {
      auto l_match = basisf_match[1];
      auto beta_match = basisf_match[2];
      auto k_match = basisf_match[3];
      int l = atoi(l_match.str().c_str());
      int k = atoi(k_match.str().c_str());
      double beta = atof(beta_match.str().c_str());

      fr_type phi(1. / beta, k);

      fa_type xir(l);
      basis.add_elem(xir, phi);
    }
    // try to match new descriptor
    std::regex_search(line, basisf_match, basisf_regex_new);
    if (basisf_match.size() > 0) {
      // angular frequency
      auto l_match = basisf_match[1];
      int l = atoi(l_match.str().c_str());
      // exponential weight
      auto fw_match = basisf_match[2];
      double fw = atof(fw_match.str().c_str());
      // laguerre poly index
      auto k_match = basisf_match[3];
      int k = atoi(k_match.str().c_str());

      fr_type phi(fw, k);
      fa_type xir(l);
      basis.add_elem(xir, phi);
    }
  }

  basis.finalize();
  ifile.close();
}

}  // end namespace boltzmann


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

/* Test_IO.cpp - Testing the I/O of the important classes of the library
 * (context, keys, ciphertexts).
 */
#include <fstream>
#include <unistd.h>

#include <NTL/ZZX.h>
#include <NTL/vector.h>

#include "FHE.h"
#include "timing.h"
#include "EncryptedArray.h"

#define N_TESTS 3
static long ms[N_TESTS][10] = {
  //nSlots  m   phi(m) ord(2)
  //  {   2,    7,    6,    3,   0,0,0,0,0,0},
  {   6,   31,   30,    5,   0,0,0,0,0,0},
  {   6,  127,  126,    7,  127,  1,  108,  24,   6,-3}, // gens=108(6), 24(!3)
  {  60, 1023,  600,   10,   11, 93,  838, 584,  10, 6}, // gens=838(10),584(6)
  //  {  378,  5461,  5292, 14}, // gens=3(126),509(3)
  //  {  630,  8191,  8190, 13}, // gens=39(630)
  //  {  600, 13981, 12000, 20}, // gens=10(30),23(10),3(!2)
  //  {  682, 15709, 15004, 22} // gens=5(682)
};

void checkCiphertext(const Ctxt& ctxt, const ZZX& ptxt, const FHESecKey& sk);

// Testing the I/O of the important classes of the library
// (context, keys, ciphertexts).
int main(int argc, char *argv[])
{
  ArgMapping amap;

  long r=1;
  long p=2;
  long c = 2;
  long w = 64;
  long L = 5;
  amap.arg("p", p, "plaintext base");
  amap.arg("r", r,  "lifting");
  amap.arg("c", c, "number of columns in the key-switching matrices");
  amap.parse(argc, argv);

  long ptxtSpace = power_long(p,r);

  FHEcontext* contexts[N_TESTS];
  FHESecKey*  sKeys[N_TESTS];
  Ctxt*       ctxts[N_TESTS];
  EncryptedArray* eas[N_TESTS];
  vector<ZZX> ptxts[N_TESTS];

  // first loop: generate stuff and write it to cout

  // open file for writing
  {fstream keyFile("iotest.txt", fstream::out|fstream::trunc);
   assert(keyFile.is_open());
  for (long i=0; i<N_TESTS; i++) {
    long m = ms[i][1];

    cout << "Testing IO: m="<<m<<", p^r="<<p<<"^"<<r<<endl;

    Vec<long> mvec(INIT_SIZE,2);
    mvec[0] = ms[i][4];  mvec[1] = ms[i][5];
    vector<long> gens(2);
    gens[0] = ms[i][6];  gens[1] = ms[i][7];
    vector<long> ords(2);
    ords[0] = ms[i][8];  ords[1] = ms[i][9];

    if (gens[0]>0)
      contexts[i] = new FHEcontext(m, p, r, gens, ords);
    else
      contexts[i] = new FHEcontext(m, p, r);
    contexts[i]->zMStar.printout();

    buildModChain(*contexts[i], L, c);  // Set the modulus chain
    if (i==N_TESTS-1) contexts[i]->makeBootstrappable(mvec);

    // Output the FHEcontext to file
    writeContextBase(keyFile, *contexts[i]);
    writeContextBase(cout, *contexts[i]);
    keyFile << *contexts[i] << endl;

    sKeys[i] = new FHESecKey(*contexts[i]);
    const FHEPubKey& publicKey = *sKeys[i];
    sKeys[i]->GenSecKey(w,ptxtSpace); // A Hamming-weight-w secret key
    addSome1DMatrices(*sKeys[i]);// compute key-switching matrices that we need
    eas[i] = new EncryptedArray(*contexts[i]);

    long nslots = eas[i]->size();

    // Output the secret key to file, twice. Below we will have two copies
    // of most things.
    keyFile << *sKeys[i] << endl;;
    keyFile << *sKeys[i] << endl;;

    vector<ZZX> b;
    long p2r = eas[i]->getContext().alMod.getPPowR();
    ZZX poly = RandPoly(0,to_ZZ(p2r)); // choose a random constant polynomial
    eas[i]->decode(ptxts[i], poly);

    ctxts[i] = new Ctxt(publicKey);
    eas[i]->encrypt(*ctxts[i], publicKey, ptxts[i]);
    eas[i]->decrypt(*ctxts[i], *sKeys[i], b);
    assert(ptxts[i].size() == b.size());
    for (long j = 0; j < nslots; j++) assert (ptxts[i][j] == b[j]);

    // output the plaintext
    keyFile << "[ ";
    for (long j = 0; j < nslots; j++) keyFile << ptxts[i][j] << " ";
    keyFile << "]\n";

    eas[i]->encode(poly,ptxts[i]);
    keyFile << poly << endl;

    // Output the ciphertext to file
    keyFile << *ctxts[i] << endl;
    keyFile << *ctxts[i] << endl;
    cerr << "okay " << i << endl<< endl;
  }
  keyFile.close();}
  cerr << "so far, so good\n\n";

  // second loop: read from input and repeat the computation

  // open file for read
  {fstream keyFile("iotest.txt", fstream::in);
  for (long i=0; i<N_TESTS; i++) {

    // Read context from file
    unsigned long m1, p1, r1;
    vector<long> gens, ords;
    readContextBase(keyFile, m1, p1, r1, gens, ords);
    FHEcontext tmpContext(m1, p1, r1, gens, ords);
    keyFile >> tmpContext;
    assert (*contexts[i] == tmpContext);
    cerr << i << ": context matches input\n";

    // We define some things below wrt *contexts[i], not tmpContext.
    // This is because the various operator== methods check equality of
    // references, not equality of the referenced FHEcontext objects.
    FHEcontext& context = *contexts[i];
    FHESecKey secretKey(context);
    FHESecKey secretKey2(tmpContext);
    const FHEPubKey& publicKey = secretKey;
    const FHEPubKey& publicKey2 = secretKey2;

    keyFile >> secretKey;
    keyFile >> secretKey2;
    assert(secretKey == *sKeys[i]);
    cerr << "   secret key matches input\n";

    EncryptedArray ea(context);
    EncryptedArray ea2(tmpContext);

    long nslots = ea.size();

    // Read the plaintext from file
    vector<ZZX> a;
    a.resize(nslots);
    assert(nslots == (long)ptxts[i].size());
    seekPastChar(keyFile, '['); // defined in NumbTh.cpp
    for (long j = 0; j < nslots; j++) {
      keyFile >> a[j];
      assert(a[j] == ptxts[i][j]);
    }
    seekPastChar(keyFile, ']');
    cerr << "   ptxt matches input\n";

    // Read the encoded plaintext from file
    ZZX poly1, poly2;
    keyFile >> poly1;
    eas[i]->encode(poly2,a);
    assert(poly1 == poly2);
    cerr << "   eas[i].encode(a)==poly1 okay\n";

    ea.encode(poly2,a);
    assert(poly1 == poly2);
    cerr << "   ea.encode(a)==poly1 okay\n";

    ea2.encode(poly2,a);
    assert(poly1 == poly2);
    cerr << "   ea2.encode(a)==poly1 okay\n";

    eas[i]->decode(a,poly1);
    assert(nslots == (long)a.size());
    for (long j = 0; j < nslots; j++) assert(a[j] == ptxts[i][j]);
    cerr << "   eas[i].decode(poly1)==ptxts[i] okay\n";

    ea.decode(a,poly1);
    assert(nslots == (long)a.size());
    for (long j = 0; j < nslots; j++) assert(a[j] == ptxts[i][j]);
    cerr << "   ea.decode(poly1)==ptxts[i] okay\n";

    ea2.decode(a,poly1);
    assert(nslots == (long)a.size());
    for (long j = 0; j < nslots; j++) assert(a[j] == ptxts[i][j]);
    cerr << "   ea2.decode(poly1)==ptxts[i] okay\n";

    // Read ciperhtext from file
    Ctxt ctxt(publicKey);
    Ctxt ctxt2(publicKey2);
    keyFile >> ctxt;
    keyFile >> ctxt2;
    assert(ctxts[i]->equalsTo(ctxt,/*comparePkeys=*/false));
    cerr << "   ctxt matches input\n";

    sKeys[i]->Decrypt(poly2,*ctxts[i]);
    assert(poly1 == poly2);
    cerr << "   sKeys[i]->decrypt(*ctxts[i]) == poly1 okay\n";

    secretKey.Decrypt(poly2,*ctxts[i]);
    assert(poly1 == poly2);
    cerr << "   secretKey.decrypt(*ctxts[i]) == poly1 okay\n";

    secretKey.Decrypt(poly2,ctxt);
    assert(poly1 == poly2);
    cerr << "   secretKey.decrypt(ctxt) == poly1 okay\n";

    secretKey2.Decrypt(poly2,ctxt2);
    assert(poly1 == poly2);
    cerr << "   secretKey2.decrypt(ctxt2) == poly1 okay\n";

    eas[i]->decrypt(ctxt, *sKeys[i], a);
    assert(nslots == (long)a.size());
    for (long j = 0; j < nslots; j++) assert(a[j] == ptxts[i][j]);
    cerr << "   eas[i].decrypt(ctxt, *sKeys[i])==ptxts[i] okay\n";

    ea.decrypt(ctxt, secretKey, a);
    assert(nslots == (long)a.size());
    for (long j = 0; j < nslots; j++) assert(a[j] == ptxts[i][j]);
    cerr << "   ea.decrypt(ctxt, secretKey)==ptxts[i] okay\n";

    ea2.decrypt(ctxt2, secretKey2, a);
    assert(nslots == (long)a.size());
    for (long j = 0; j < nslots; j++) assert(a[j] == ptxts[i][j]);
    cerr << "   ea2.decrypt(ctxt2, secretKey2)==ptxts[i] okay\n";

    cerr << "test "<<i<<" okay\n\n";
  }}
  unlink("iotest.txt"); // clean up before exiting
}

#if 0
/************************ OLD CODE ************************/
#include <cstdlib>
#include <string>
#include <sstream>
#include <fstream>
#include <iostream>

#include "FHE.h"
#include "EncryptedArray.h"
// #include "parameters.h"  //this has the function get_m_c

int main()
{
  string * tmp_ptr;

  long LLL, DDD, KKK ;

  long m;
  long ccc;
  FHEcontext * context_ptr;
  FHESecKey  * fhekey_ptr;
  EncryptedArray * ea_ptr;

  LLL = 682; DDD=12; KKK=80;

  /*
      pair<long, long> m_c  = get_m_c(LLL, DDD, KKK);
      m = m_c.first;
      ccc = m_c.second;
  */
  m = 15709;
  ccc = 3;

  context_ptr = new FHEcontext(m, 2, 1);
  buildModChain(*context_ptr, DDD, ccc);
  fhekey_ptr = new FHESecKey(*context_ptr);
  fhekey_ptr->clear();
  fhekey_ptr->GenSecKey(64,2);
  addSome1DMatrices(*fhekey_ptr);
  const  FHEPubKey & pub_key =  *fhekey_ptr;

  ZZX G;
  G = ZZX(1,1);
  ea_ptr = new EncryptedArray(*context_ptr, G);

  // Test I/O, write context and public key, then try to read them back
  cout << "KEY\n";
  cout <<"L= " <<LLL<< " D= " << DDD<< " K= " << KKK << endl<< flush;

  {
  stringstream s1;
  writeContextBase(s1, *context_ptr);
  s1 << *context_ptr;

  string s2 = s1.str();
  cout << s2 << endl;  // output context also to external cout

  // Read back context from input stream (s3)
  unsigned long m1, p1, r1;
  stringstream s3(s2);
  readContextBase(s3, m1, p1, r1);
  FHEcontext c1(m1, p1, r1);
  s3 >> c1;
  assert(c1 == *context_ptr);
  }
  {
  stringstream s1;
  s1 << pub_key;

  string s2 = s1.str();
  cout << s2 <<"\nENDKEY" << endl; // output public key also to external cout

  // Read back cpublic key from input stream (s3)
  stringstream s3(s2);
  FHEPubKey pk1(*context_ptr);
  s3 >> pk1;
  assert(pk1 == pub_key);
  }
}
#endif


#include "minkowski.h"

#include <fstream>
#include <iostream>
#include <boost/polygon/polygon.hpp>
#include <string>
#include <iostream>
#include <sstream>
#include <limits>

#undef min
#undef max

typedef boost::polygon::point_data<int> point;
typedef boost::polygon::polygon_set_data<int> polygon_set;
typedef boost::polygon::polygon_with_holes_data<int> polygon;
typedef std::pair<point, point> edge;
using namespace boost::polygon::operators;
using namespace std;;

void convolve_two_segments(std::vector<point>& figure, const edge& a, const edge& b) {
  using namespace boost::polygon;
  figure.clear();
  figure.push_back(point(a.first));
  figure.push_back(point(a.first));
  figure.push_back(point(a.second));
  figure.push_back(point(a.second));
  convolve(figure[0], b.second);
  convolve(figure[1], b.first);
  convolve(figure[2], b.first);
  convolve(figure[3], b.second);
}

template <typename itrT1, typename itrT2>
void convolve_two_point_sequences(polygon_set& result, itrT1 ab, itrT1 ae, itrT2 bb, itrT2 be) {
  using namespace boost::polygon;
  if(ab == ae || bb == be)
    return;
  point first_a = *ab;
  point prev_a = *ab;
  std::vector<point> vec;
  polygon poly;
  ++ab;
  for( ; ab != ae; ++ab) {
    point first_b = *bb;
    point prev_b = *bb;
    itrT2 tmpb = bb;
    ++tmpb;
    for( ; tmpb != be; ++tmpb) {
      convolve_two_segments(vec, std::make_pair(prev_b, *tmpb), std::make_pair(prev_a, *ab));
      set_points(poly, vec.begin(), vec.end());
      result.insert(poly);
      prev_b = *tmpb;
    }
    prev_a = *ab;
  }
}

template <typename itrT>
void convolve_point_sequence_with_polygons(polygon_set& result, itrT b, itrT e, const std::vector<polygon>& polygons) {
  using namespace boost::polygon;
  for(std::size_t i = 0; i < polygons.size(); ++i) {
    convolve_two_point_sequences(result, b, e, begin_points(polygons[i]), end_points(polygons[i]));
    for(polygon_with_holes_traits<polygon>::iterator_holes_type itrh = begin_holes(polygons[i]);
        itrh != end_holes(polygons[i]); ++itrh) {
      convolve_two_point_sequences(result, b, e, begin_points(*itrh), end_points(*itrh));
    }
  }
}

void convolve_two_polygon_sets(polygon_set& result, const polygon_set& a, const polygon_set& b) {
  using namespace boost::polygon;
  result.clear();
  std::vector<polygon> a_polygons;
  std::vector<polygon> b_polygons;
  a.get(a_polygons);
  b.get(b_polygons);
  for(std::size_t ai = 0; ai < a_polygons.size(); ++ai) {
    convolve_point_sequence_with_polygons(result, begin_points(a_polygons[ai]),
                                          end_points(a_polygons[ai]), b_polygons);
    for(polygon_with_holes_traits<polygon>::iterator_holes_type itrh = begin_holes(a_polygons[ai]);
        itrh != end_holes(a_polygons[ai]); ++itrh) {
      convolve_point_sequence_with_polygons(result, begin_points(*itrh),
                                            end_points(*itrh), b_polygons);
    }
    for(std::size_t bi = 0; bi < b_polygons.size(); ++bi) {
      polygon tmp_poly = a_polygons[ai];
      result.insert(convolve(tmp_poly, *(begin_points(b_polygons[bi]))));
      tmp_poly = b_polygons[bi];
      result.insert(convolve(tmp_poly, *(begin_points(a_polygons[ai]))));
    }
  }
}

double inputscale;

using namespace boost::polygon;

vector<double> Avec;
vector<double> Bvec;
vector<vector<double>> chlds;
vector<vector<double>> retpnts;
vector<vector<double>> retholes;

void getResults(double* data, double* holesData)
{
	int index=0;
	for(int i=0;i<retpnts.size();i++){
		for(int j=0;j<retpnts[i].size();j++){
			data[index]=retpnts[i][j];
			index++;
		}
	}

	index=0;
	for(int i=0;i<retholes.size();i++){
		for(int j=0;j<retholes[i].size();j++){
			holesData[index]=retholes[i][j];
			index++;
		}
	}
}

void getSizes1(int* sizes)
{
	sizes[0]=retpnts.size();
	sizes[1]=retholes.size();
}

void getSizes2(int* sizes1,int* sizes2)
{
	for(int i=0;i<retpnts.size();i++){
		sizes1[i]=retpnts[i].size();
	}
	for(int i=0;i<retholes.size();i++){
		sizes2[i]=retholes[i].size();
	}
}

void setData(int cntA, double* pntsA, int holesCnt, int* holesSizes, double* holesPoints, int cntB, double* pntsB)
{
	Avec.clear();
	Bvec.clear();
	chlds.clear();

	retpnts.clear();
	retholes.clear();

	for(int i=0;i<cntA;i++){
		Avec.push_back(pntsA[i]);
	}
	for(int i=0;i<cntB;i++){
		Bvec.push_back(pntsB[i]);
	}

	int index=0;
	for(int i=0;i<holesCnt;i++){
		chlds.push_back(vector<double>());
		for(int j=0;j<holesSizes[i];j++){
			chlds.back().push_back(holesPoints[index]);
			index++;
		}
	}
}

void calculateNFP()
{

	polygon_set a, b, c;
	std::vector<polygon> polys;
	std::vector<point> pts;

  // get maximum bounds for scaling factor
  unsigned int len = Avec.size()/2;
  double Amaxx = 0;
  double Aminx = 0;
  double Amaxy = 0;
  double Aminy = 0;
  for (unsigned int i = 0; i < len; i++) {
  	double x1=Avec[i*2];
	double y1=Avec[i*2+1];
  	Amaxx = (std::max)(Amaxx, (double)x1);
  	Aminx = (std::min)(Aminx, (double)x1);
  	Amaxy = (std::max)(Amaxy, (double)y1);
  	Aminy = (std::min)(Aminy, (double)y1);
  }

  len = Bvec.size()/2;
  double Bmaxx = 0;
  double Bminx = 0;
  double Bmaxy = 0;
  double Bminy = 0;
  for (unsigned int i = 0; i < len*2; i+=2) {
  	double x1=Bvec[i];
	double y1=Bvec[i+1];
  	Bmaxx = (std::max)(Bmaxx, (double)x1);
  	Bminx = (std::min)(Bminx, (double)x1);
  	Bmaxy = (std::max)(Bmaxy, (double)y1);
  	Bminy = (std::min)(Bminy, (double)y1);
  }

  double Cmaxx = Amaxx + Bmaxx;
  double Cminx = Aminx + Bminx;
  double Cmaxy = Amaxy + Bmaxy;
  double Cminy = Aminy + Bminy;

  double maxxAbs = (std::max)(Cmaxx, std::fabs(Cminx));
  double maxyAbs = (std::max)(Cmaxy, std::fabs(Cminy));

  double maxda = (std::max)(maxxAbs, maxyAbs);
  int maxi = std::numeric_limits<int>::max();

  if(maxda < 1){
  	maxda = 1;
  }

  // why 0.1? dunno. it doesn't screw up with 0.1
  inputscale = (0.1f * (double)(maxi)) / maxda;

  //double scale = 1000;
  len = Avec.size()/2;

  for (unsigned int i = 0; i < len; i++) {
 	double x1=Avec[i*2];
	double y1=Avec[i*2+1];
    int x = (int)(inputscale * (double)x1);
    int y = (int)(inputscale * (double)y1);

    pts.push_back(point(x, y));
  }

  polygon poly;
  boost::polygon::set_points(poly, pts.begin(), pts.end());
  a+=poly;

  // subtract holes from a here...

  len = chlds.size();

  for(unsigned int i=0; i<len; i++){
	  vector<double> hole=chlds[i];
    pts.clear();
    unsigned int hlen = hole.size()/2;
    for(unsigned int j=0; j<hlen; j++){
    	double x1=hole[j*2];
		double y1=hole[j*2+1];
    	int x = (int)(inputscale * (double)x1);
    	int y = (int)(inputscale * (double)y1);
    	pts.push_back(point(x, y));
    }
    boost::polygon::set_points(poly, pts.begin(), pts.end());
    a -= poly;
  }

  //and then load points B
  pts.clear();
  len = Bvec.size()/2;

  //javascript nfps are referenced with respect to the first point
  double xshift = 0;
  double yshift = 0;

  for (unsigned int i = 0; i < len; i++) {
	double x1=Bvec[i*2];
	double y1=Bvec[i*2+1];

    int x = -(int)(inputscale * (double)x1);
    int y = -(int)(inputscale * (double)y1);
    pts.push_back(point(x, y));

    if(i==0){
    	xshift = (double)x1;
    	yshift = (double)y1;
    }
  }

  boost::polygon::set_points(poly, pts.begin(), pts.end());
  b+=poly;

  polys.clear();

  convolve_two_polygon_sets(c, a, b);
  c.get(polys);

  for(unsigned int i = 0; i < polys.size(); ++i ){

  	vector<double> pointlist = vector<double>();

  	int j = 0;

  	for(polygon_traits<polygon>::iterator_type itr = polys[i].begin(); itr != polys[i].end(); ++itr) {

  	 //  std::cout << (double)(*itr).get(boost::polygon::HORIZONTAL) / inputscale << std::endl;
	 double x1=((double)(*itr).get(boost::polygon::HORIZONTAL)) / inputscale + xshift;
	 double y1=((double)(*itr).get(boost::polygon::VERTICAL)) / inputscale + yshift;

	   pointlist.push_back(x1);
	   pointlist.push_back(y1);

       j++;
    }
    retpnts.push_back(pointlist);
    // holes

    int k = 0;
    for(polygon_with_holes_traits<polygon>::iterator_holes_type itrh = begin_holes(polys[i]); itrh != end_holes(polys[i]); ++itrh){
		vector<double> child=vector<double>();

    	int z = 0;
    	for(polygon_traits<polygon>::iterator_type itr2 = (*itrh).begin(); itr2 != (*itrh).end(); ++itr2) {

			double x1=((double)(*itr2).get(boost::polygon::HORIZONTAL)) / inputscale + xshift;
			double y1=((double)(*itr2).get(boost::polygon::VERTICAL)) / inputscale + yshift;

    		child.push_back(x1);
			child.push_back(y1);

    		z++;
    	}
		retholes.push_back(child);

    	k++;
    }
  }
}


// USAGE: ./sdf-demo BASIC_OBJ_FILE (with only triangular faces supported1
// Try the OBJ files in sample-obj included in the project.
#include <sdf/sdf.hpp>
#include <meshview/meshview.hpp>
#include <meshview/meshview_imgui.hpp>

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

#include <chrono>
// Timing macro
#define _BEGIN_PROFILE auto start = std::chrono::high_resolution_clock::now()
#define _PROFILE(x)                                                            \
    do {                                                                       \
        double _delta = std::chrono::duration<double, std::milli>(             \
                            std::chrono::high_resolution_clock::now() - start) \
                            .count();                                          \
        printf("%s: %f ms = %f fps\n", #x, _delta, 1e3f / _delta);             \
        start = std::chrono::high_resolution_clock::now();                     \
    } while (false)

using namespace sdf;

namespace {

Eigen::Matrix<float, 3, 3, Eigen::RowMajor> rodrigues(
    Eigen::Ref<const Eigen::Vector3f> axis_angle) {
    Eigen::Matrix<float, 3, 3, Eigen::RowMajor> rotation;
    const float norm = axis_angle.norm();
    if (norm < 1e-5f) {
        rotation.setIdentity();
    } else {
        rotation.noalias() =
            Eigen::AngleAxisf(norm, axis_angle / norm).toRotationMatrix();
    }
    return rotation;
}
}  // namespace

int main(int argc, char** argv) {
    if (argc < 2) {
        std::cerr << "Expect 1 argument: path to sample-obj/x.obj (or other "
                     "simple OBJ file)\n";
        return 0;
    }
    // Create meshview viewer
    meshview::Viewer viewer;
    viewer.wireframe = true;

    // Load obj
    meshview::Mesh& obj_mesh = viewer.add_mesh(argv[1]);
    if (obj_mesh.verts_pos().rows() == 0) {
        std::cerr << "Failed to load " << argv[1] << "\n";
        return 1;
    }
    Points mesh_verts_initial = obj_mesh.verts_pos();

    // Create SDF instance from loaded mesh (robust mode)
    sdf::SDF sdf(obj_mesh.verts_pos(), obj_mesh.faces);
    std::cout << sdf.verts().rows() << " verts\n";

    // Cross section visualization parameters
    float csection_z = 0.0f;
    Eigen::Vector3f csection_axisangle, model_axisangle;
    csection_axisangle.setZero();
    model_axisangle.setZero();

    // Generate flat point cloud, for visualizing a cross-section of the SDF
    const int FLAT_CLOUD_DIM = 400;
    const float FLAT_CLOUD_RADIUS =
        (sdf.aabb().tail<3>() - sdf.aabb().head<3>()).maxCoeff();
    const float FLAT_CLOUD_STEP = FLAT_CLOUD_RADIUS * 2 / FLAT_CLOUD_DIM;

    Points _pts_flat(FLAT_CLOUD_DIM * FLAT_CLOUD_DIM, 3);
    for (int i = 0; i < FLAT_CLOUD_DIM; ++i) {
        float y = -FLAT_CLOUD_RADIUS + FLAT_CLOUD_STEP * i;
        for (int j = 0; j < FLAT_CLOUD_DIM; ++j) {
            float x = -FLAT_CLOUD_RADIUS + FLAT_CLOUD_STEP * j;
            _pts_flat.row(i * FLAT_CLOUD_DIM + j) << x, y, 0.f;
        }
    }

    // Add planar cross section point cloud
    auto& flat_cloud = viewer.add_point_cloud(_pts_flat, 0.f, 1.f, 0.f);

    // Make a backup of verts to allow transformations
    Points csection_verts_initial = flat_cloud.verts_pos();
    auto csection_verts = flat_cloud.verts_pos();

    const float MAX_DISTANCE_FUNC = 0.09f;
    bool updated = false;

    // Color by containment only
    bool contains_only = false;

    std::vector<meshview::Mesh*> spheres;
    // This part visualizes the surface sampling
    // sdf::Points rand_pts = sdf.sample_surface(50);
    // for (int i = 0; i < rand_pts.rows(); ++i) {
    //     spheres.push_back(&viewer
    //                            .add_sphere(Eigen::Vector3f(0.f, 0.f, 0.f),
    //                                        0.02f,
    //                                        Eigen::Vector3f(1.f, 0.0f, 0.f))
    //                            .set_translation(rand_pts.row(i).transpose()));
    // }

    // Update the cross section point cloud
    auto update = [&](bool model_rot_updated = false) {
        const float norm = csection_axisangle.norm();
        if (model_rot_updated) {
            // Rotate mesh
            obj_mesh.verts_pos().noalias() =
                mesh_verts_initial * rodrigues(model_axisangle).transpose();
            sdf.update();

            // This part updates the surface sampling
            // rand_pts = sdf.sample_surface(spheres.size());
            // for (int i = 0; i < rand_pts.rows(); ++i) {
            //     spheres[i]->set_translation(rand_pts.row(i).transpose());
            // }
        }

        csection_verts.noalias() = csection_verts_initial;
        csection_verts.array().rightCols<1>() += csection_z;
        csection_verts *= rodrigues(csection_axisangle).transpose();
        _BEGIN_PROFILE;
        Eigen::VectorXf verts_sdf = sdf(csection_verts);
        _PROFILE(compute SDF);
        for (size_t i = 0; i < csection_verts.rows(); ++i) {
            float t = contains_only ? 1.0f
                                    : (1.f - std::min(std::abs(verts_sdf[i]),
                                                      MAX_DISTANCE_FUNC) *
                                                 (1.f / MAX_DISTANCE_FUNC));

            auto rgb = flat_cloud.verts_rgb().row(i);
            rgb[0] = (verts_sdf[i] < 0) ? 0.0f : 1.0f;
            rgb[1] = t;
            rgb[2] = t * 0.5;
        }

        // Update the mesh on-the-fly (send mesh to GPU)
        updated = true;
    };
    update();

    viewer.on_key = [&](int key, meshview::input::Action action,
                        int mods) -> bool {
        if (action != meshview::input::Action::release) {
            if (key == 'J' || key == 'K') {
                csection_z += (key == 'J') ? 0.01 : -0.01;
                update();
                flat_cloud.update();
            }

            if (action == meshview::input::Action::press) {
                if (key == 'M') {
                    obj_mesh.enabled = !obj_mesh.enabled;
                }
            }
        }
        return true;
    };

    viewer.on_open = []() { ImGui::GetIO().IniFilename = nullptr; };
    viewer.on_gui = [&]() {
        updated = false;
        // * GUI code
        ImGui::SetNextWindowPos(ImVec2(10, 10), ImGuiCond_Once);
        ImGui::SetNextWindowSize(ImVec2(500, 360), ImGuiCond_Once);
        ImGui::Begin("Model and Cross Section", NULL);
        ImGui::TextUnformatted("Reset: ");
        ImGui::SameLine();
        if (ImGui::Button("Axis-angle##ResetCrossSectionAA")) {
            csection_axisangle.setZero();
            update();
        }
        ImGui::SameLine();
        if (ImGui::Button("Z##ResetCrossSectionZ")) {
            csection_z = 0.f;
            update();
        }

        if (ImGui::SliderFloat("cross sec z##slideflatz", &csection_z, -1.0f,
                               1.0f)) {
            update();
        }
        ImGui::TextUnformatted("Tip: press j,k to adjust cross section z");

        if (ImGui::SliderFloat3("cross sec rot##slideflatrot",
                                csection_axisangle.data(), -3.14f, 3.14f)) {
            update();
        }

        if (ImGui::SliderFloat3("model rot##slidemodelrot",
                                model_axisangle.data(), -3.14f, 3.14f)) {
            update(true);
        }

        ImGui::Checkbox("Show mesh", &obj_mesh.enabled);
        ImGui::Checkbox("Wireframe mesh", &viewer.wireframe);
        if (ImGui::Checkbox("Containment only", &contains_only)) {
            update();
        }

        ImGui::End();  // Model and Cross Section
        // Return true if updated to indicate mesh data has been changed
        // the viewer will update the GPU buffers automatically
        return updated;
    };
    viewer.show();
    return 0;
}