#include <geometrycentral/direction_fields.h>

#include "geometrycentral/linear_solvers.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include <Eigen/SparseQR>

using std::cout;
using std::endl;

namespace geometrycentral {

// Anonymous namespace for helper functions
namespace {

VertexData<Complex> computeSmoothestVertexDirectionField_noBoundary(Geometry<Euclidean>* geometry, int nSym,
                                                                    bool alignCurvature) {
  HalfedgeMesh* mesh = geometry->getMesh();
  size_t N = mesh->nVertices();

  GeometryCache<Euclidean>& gc = geometry->cache;
  gc.requireVertexTransportCoefs();
  gc.requireEdgeCotanWeights();
  gc.requireVertexIndices();
  gc.requireVertexDualAreas();

  // Energy matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> energyMatrix(
      N, N); // have to use ColMajor because LU solver below demands it

  // Supposedly reserving space in the matrix makes construction real zippy
  // below
  Eigen::VectorXi nEntries(N);
  for (VertexPtr v : mesh->vertices()) {
    nEntries[gc.vertexIndices[v]] = v.degree() + 1;
  }
  energyMatrix.reserve(nEntries);

  // Mass matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> massMatrix(N, N);
  massMatrix.reserve(1);

  // === Build matrices

  // Build the mass matrix
  for (VertexPtr v : mesh->vertices()) {
    size_t i = gc.vertexIndices[v];
    massMatrix.insert(i, i) = gc.vertexDualAreas[v];
  }

  // Build the energy matrix
  for (VertexPtr v : mesh->vertices()) {
    size_t i = gc.vertexIndices[v];

    std::complex<double> weightISum = 0;
    for (HalfedgePtr he : v.incomingHalfedges()) {
      size_t j = gc.vertexIndices[he.vertex()];
      std::complex<double> rBar = std::pow(gc.vertexTransportCoefs[he], nSym);
      double weight = gc.edgeCotanWeights[he.edge()];
      energyMatrix.insert(i, j) = -weight * rBar;
      weightISum += weight;
    }

    energyMatrix.insert(i, i) = weightISum;
  }

  // Shift to avoid singularity
  Eigen::SparseMatrix<Complex> eye(N, N);
  eye.setIdentity();
  energyMatrix += 1e-4 * eye;

  // Store the solution here
  Eigen::VectorXcd solution;

  // If requested, align to principal curvatures
  if (alignCurvature) {

    gc.requirePrincipalDirections();

    Eigen::VectorXcd dirVec(N);
    if (nSym == 2) {
      for (VertexPtr v : mesh->vertices()) {
        dirVec[gc.vertexIndices[v]] = gc.principalDirections[v];
      }
    } else if (nSym == 4) {
      for (VertexPtr v : mesh->vertices()) {
        dirVec[gc.vertexIndices[v]] = std::pow(gc.principalDirections[v], 2);
      }
    }

    // Normalize the alignment field
    double scale = std::sqrt(std::abs((dirVec.adjoint() * massMatrix * dirVec)[0]));
    dirVec /= scale;

    double lambdaT = 0.0; // this is something of a magical constant, see
                          // "Globally Optimal Direction Fields", eqn 16

    // Eigen::VectorXcd RHS = massMatrix * dirVec;
    Eigen::VectorXcd RHS = dirVec;
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix - lambdaT * massMatrix;
    solution = solveSquare(LHS, RHS);
  }
  // Otherwise find the smallest eigenvector
  else {
    std::cout << "Solving smoothest field eigenvalue problem..." << std::endl;
    solution = smallestEigenvectorPositiveDefinite(energyMatrix, massMatrix);
  }

  // Copy the result to a VertexData vector
  VertexData<Complex> toReturn(mesh);
  for (VertexPtr v : mesh->vertices()) {
    toReturn[v] = solution[gc.vertexIndices[v]] / std::abs(solution[gc.vertexIndices[v]]);
  }

  return toReturn;
}

VertexData<Complex> computeSmoothestVertexDirectionField_boundary(Geometry<Euclidean>* geometry, int nSym,
                                                                  bool alignCurvature) {
  HalfedgeMesh* mesh = geometry->getMesh();
  size_t nInterior = mesh->nInteriorVertices();

  GeometryCache<Euclidean>& gc = geometry->cache;
  gc.requireVertexTransportCoefs();
  gc.requireEdgeCotanWeights();
  gc.requireVertexBases();
  gc.requireInteriorVertexIndices();
  gc.requireVertexDualAreas();

  // Compute the boundary values
  VertexData<std::complex<double>> boundaryValues(mesh);
  for (VertexPtr v : mesh->vertices()) {
    if (v.isBoundary()) {
      Vector3 b = geometry->boundaryNormal(v);
      Complex bC(dot(gc.vertexBases[v][0], b), dot(gc.vertexBases[v][1], b)); // TODO can do better
      bC = unit(bC);
      boundaryValues[v] = std::pow(bC, nSym);
    } else {
      boundaryValues[v] = 0;
    }
  }

  VertexData<size_t> vertInd = mesh->getInteriorVertexIndices();

  // Energy matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> energyMatrix(nInterior, nInterior);

  Eigen::VectorXi nEntries(nInterior);
  for (VertexPtr v : mesh->vertices()) {
    if (v.isBoundary()) {
      continue;
    }
    nEntries[gc.interiorVertexIndices[v]] = v.degree() + 1;
  }
  energyMatrix.reserve(nEntries);

  // Mass matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> massMatrix(nInterior, nInterior);
  massMatrix.reserve(1);

  // RHS
  Eigen::VectorXcd b(nInterior);

  // === Build matrices

  // Build the mass matrix and zero b
  for (VertexPtr v : mesh->vertices()) {
    if (v.isBoundary()) {
      continue;
    }
    size_t i = gc.interiorVertexIndices[v];
    b(i) = 0.0;
    massMatrix.insert(i, i) = gc.vertexDualAreas[v];
  }

  // Build the energy matrix
  for (VertexPtr v : mesh->vertices()) {
    if (v.isBoundary()) {
      continue;
    }
    size_t i = gc.interiorVertexIndices[v];

    std::complex<double> weightISum = 0;
    for (HalfedgePtr he : v.incomingHalfedges()) {
      std::complex<double> rBar = std::pow(gc.vertexTransportCoefs[he], nSym);
      double w = gc.edgeCotanWeights[he.edge()];

      // Interior-boundary term
      if (he.vertex().isBoundary()) {
        std::complex<double> bVal = boundaryValues[he.vertex()];
        b(i) += w * rBar * bVal;
      } else { // Interior-interior term
        size_t j = gc.interiorVertexIndices[he.vertex()];
        energyMatrix.insert(i, j) = -w * rBar;
      }
      weightISum += w;
    }

    energyMatrix.insert(i, i) = weightISum;
  }

  // Shift to avoid singularities
  Eigen::SparseMatrix<Complex> eye(nInterior, nInterior);
  eye.setIdentity();
  energyMatrix += 1e-4 * eye;

  // Compute the actual solution
  std::cout << "Solving linear problem..." << std::endl;

  // Store the solution here
  Eigen::VectorXcd solution;

  // If requested, align to principal curvatures
  if (alignCurvature) {

    gc.requirePrincipalDirections();

    Eigen::VectorXcd dirVec(nInterior);
    for (VertexPtr v : mesh->vertices()) {
      if (v.isBoundary()) {
        continue;
      }

      Complex directionVal = gc.principalDirections[v];
      if (nSym == 4) {
        directionVal = std::pow(directionVal, 2);
      }

      // Normalize the curvature vectors. By doing so, we lose the property of adjusting the strength of the alignment
      // based on the strength of the curvature, but resolve any scaling issues between the magnitude of the normals and
      // the magnitude of the desired field.  Be careful when interpreting this as opposed to the usual direction field
      // optimization.
      dirVec[gc.interiorVertexIndices[v]] = directionVal / std::abs(directionVal);
    }

    double t = 0.01; // this is something of a magical constant, see "Globally
                     // Optimal Direction Fields", eqn 9

    Eigen::VectorXcd RHS = massMatrix * (t * dirVec + b);
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix;
    solution = solveSquare(LHS, RHS);
  }
  // Otherwise find the general closest solution
  else {
    std::cout << "Solving smoothest field dirichlet problem..." << std::endl;
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix;
    Eigen::VectorXcd RHS = massMatrix * b;
    solution = solveSquare(LHS, RHS);
  }

  // Copy the result to a VertexData vector for both the boudary and interior
  VertexData<Complex> toReturn(mesh);
  for (VertexPtr v : mesh->vertices()) {
    if (v.isBoundary()) {
      toReturn[v] = boundaryValues[v];
    } else {
      toReturn[v] = unit(solution[gc.interiorVertexIndices[v]]);
    }
  }

  return toReturn;
}
}; // namespace

VertexData<Complex> computeSmoothestVertexDirectionField(Geometry<Euclidean>* geometry, int nSym, bool alignCurvature) {
  std::cout << "Computing globally optimal direction field" << std::endl;

  if (alignCurvature && !(nSym == 2 || nSym == 4)) {
    throw std::logic_error("ERROR: It only makes sense to align with curvature when nSym = 2 or "
                           "4");
  }

  // Dispatch to either the boundary of no boundary variant depending on the
  // mesh type
  bool hasBoundary = false;
  for (VertexPtr v : geometry->getMesh()->vertices()) {
    hasBoundary |= v.isBoundary();
  }

  if (hasBoundary) {
    std::cout << "Mesh has boundary, computing dirichlet boundary condition solution" << std::endl;
    return computeSmoothestVertexDirectionField_boundary(geometry, nSym, alignCurvature);
  } else {
    std::cout << "Mesh has no boundary, computing unit-norm solution" << std::endl;
    return computeSmoothestVertexDirectionField_noBoundary(geometry, nSym, alignCurvature);
  }
}

// Helpers for computing face-based direction fields
namespace {

FaceData<Complex> computeSmoothestFaceDirectionField_noBoundary(Geometry<Euclidean>* geometry, int nSym,
                                                                bool alignCurvature) {

  HalfedgeMesh* mesh = geometry->getMesh();
  unsigned int N = mesh->nFaces();

  GeometryCache<Euclidean>& gc = geometry->cache;
  gc.requireFaceTransportCoefs();
  gc.requireFaceNormals();
  gc.requireFaceAreas();
  gc.requireDihedralAngles();
  gc.requireFaceIndices();

  // === Allocate matrices
  // Energy matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> energyMatrix(N, N);
  energyMatrix.reserve(Eigen::VectorXi::Constant(N, 4));

  // Mass matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> massMatrix(N, N);
  massMatrix.reserve(Eigen::VectorXi::Constant(N, 1));

  // === Build matrices

  // Build the mass matrix
  for (FacePtr f : mesh->faces()) {
    size_t i = gc.faceIndices[f];
    massMatrix.insert(i, i) = gc.faceAreas[f];
  }

  // Build the energy matrix
  for (FacePtr f : mesh->faces()) {
    size_t i = gc.faceIndices[f];

    std::complex<double> weightISum = 0;
    for (HalfedgePtr he : f.adjacentHalfedges()) {

      if (!he.twin().isReal()) {
        continue;
      }

      FacePtr neighFace = he.twin().face();
      unsigned int j = gc.faceIndices[neighFace];

      // LC connection between the faces
      Complex rBar = std::pow(gc.faceTransportCoefs[he.twin()], nSym);

      double weight = 1; // FIXME TODO figure out weights
      energyMatrix.insert(i, j) = -weight * rBar;
      weightISum += weight;
    }

    energyMatrix.insert(i, i) = weightISum;
  }

  // Shift to avoid singularity
  Eigen::SparseMatrix<Complex> eye(N, N);
  eye.setIdentity();
  energyMatrix += 1e-4 * eye;

  // Store the solution here
  Eigen::VectorXcd solution;

  // If requested, align to principal curvatures
  if (alignCurvature) {

    Eigen::VectorXcd dirVec(N);
    for (FacePtr f : mesh->faces()) {

      // Compute something like the principal directions
      double weightSum = 0;
      Complex sum = 0;

      for (HalfedgePtr he : f.adjacentHalfedges()) {

        double dihedralAngle = std::abs(gc.dihedralAngles[he.edge()]);
        double weight = norm(geometry->vector(he));
        weightSum += weight;
        double angleCoord = angleInPlane(geometry->vector(f.halfedge()), geometry->vector(he), gc.faceNormals[f]);
        Complex coord = std::exp(angleCoord * IM_I *
                                 (double)nSym); // nsym should be 2 or 4, checked in the funciton which calls this

        sum += coord * weight * dihedralAngle;
      }

      sum /= weightSum;

      dirVec[gc.faceIndices[f]] = sum;
    }

    // Normalize the alignment field
    double scale = std::sqrt(std::abs((dirVec.adjoint() * massMatrix * dirVec)[0]));
    dirVec /= scale;

    double lambdaT = 0.0; // this is something of a magical constant, see "Globally Optimal Direction Fields", eqn 16

    // Eigen::VectorXcd RHS = massMatrix * dirVec;
    Eigen::VectorXcd RHS = dirVec;
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix - lambdaT * massMatrix;
    solution = solveSquare(LHS, RHS);

  }
  // Otherwise find the smallest eigenvector
  else {
    std::cout << "Solving smoothest field eigenvalue problem..." << std::endl;
    solution = smallestEigenvectorPositiveDefinite(energyMatrix, massMatrix);
  }

  // Copy the result to a FaceData object
  FaceData<Complex> field(mesh);
  for (FacePtr f : mesh->faces()) {
    field[f] = solution[gc.faceIndices[f]] / std::abs(solution[gc.faceIndices[f]]);
  }

  return field;
}

FaceData<Complex> computeSmoothestFaceDirectionField_boundary(Geometry<Euclidean>* geometry, int nSym,
                                                              bool alignCurvature) {

  HalfedgeMesh* mesh = geometry->getMesh();

  GeometryCache<Euclidean>& gc = geometry->cache;
  gc.requireFaceTransportCoefs();
  gc.requireFaceNormals();
  gc.requireFaceAreas();
  gc.requireDihedralAngles();

  // Index interior faces
  size_t nInteriorFace = 0;
  FaceData<size_t> interiorFaceInd(mesh, -77);
  FaceData<char> isInterior(mesh);
  for (FacePtr f : mesh->faces()) {
    bool isBoundary = false;
    for (EdgePtr e : f.adjacentEdges()) {
      isBoundary |= e.isBoundary();
    }
    isInterior[f] = !isBoundary;
    if (!isBoundary) {
      interiorFaceInd[f] = nInteriorFace++;
    }
  }

  // Compute boundary values
  FaceData<Complex> boundaryValues(mesh);
  for (FacePtr f : mesh->faces()) {
    if (isInterior[f]) {
      boundaryValues[f] = 0;
    } else {
      Vector3 bVec = Vector3::zero();
      for (HalfedgePtr he : f.adjacentHalfedges()) {
        if (he.edge().isBoundary()) {
          bVec += geometry->vector(he).rotate_around(gc.faceNormals[f], -PI / 2.0);
        }
      }
      Complex bC(dot(gc.faceBases[f][0], bVec), dot(gc.faceBases[f][1], bVec));
      bC = unit(bC);
      boundaryValues[f] = std::pow(bC, nSym);
    }
  }

  // === Allocate matrices
  // Energy matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> energyMatrix(nInteriorFace, nInteriorFace);
  energyMatrix.reserve(Eigen::VectorXi::Constant(nInteriorFace, 4));

  // Mass matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> massMatrix(nInteriorFace, nInteriorFace);
  massMatrix.reserve(Eigen::VectorXi::Constant(nInteriorFace, 1));

  // RHS
  Eigen::VectorXcd b(nInteriorFace);

  // === Build matrices

  // Build the mass matrix
  for (FacePtr f : mesh->faces()) {
    if (isInterior[f]) {
      size_t i = interiorFaceInd[f];
      massMatrix.insert(i, i) = gc.faceAreas[f];
    }
  }

  // Build the energy matrix
  for (FacePtr f : mesh->faces()) {
    if (isInterior[f]) {
      size_t i = interiorFaceInd[f];

      std::complex<double> weightISum = 0;
      for (HalfedgePtr he : f.adjacentHalfedges()) {

        FacePtr neighFace = he.twin().face();
        double weight = 1; // FIXME TODO figure out weights
        Complex rBar = std::pow(gc.faceTransportCoefs[he.twin()], nSym);

        if (isInterior[neighFace]) {
          size_t j = interiorFaceInd[neighFace];
          energyMatrix.insert(i, j) = -weight * rBar;
        } else {
          std::complex<double> bVal = boundaryValues[neighFace];
          b(i) += weight * rBar * bVal;
        }

        weightISum += weight;
      }

      energyMatrix.insert(i, i) = weightISum;
    }
  }

  // Shift to avoid singularity
  Eigen::SparseMatrix<Complex> eye(nInteriorFace, nInteriorFace);
  eye.setIdentity();
  energyMatrix += 1e-4 * eye;

  // Store the solution here
  Eigen::VectorXcd solution;

  // If requested, align to principal curvatures
  if (alignCurvature) {

    Eigen::VectorXcd dirVec(nInteriorFace);
    for (FacePtr f : mesh->faces()) {
      if (isInterior[f]) {

        // Compute something like the principal directions
        double weightSum = 0;
        Complex sum = 0;

        for (HalfedgePtr he : f.adjacentHalfedges()) {

          double dihedralAngle = std::abs(gc.dihedralAngles[he.edge()]);
          double weight = norm(geometry->vector(he));
          weightSum += weight;
          double angleCoord = angleInPlane(geometry->vector(f.halfedge()), geometry->vector(he), gc.faceNormals[f]);
          Complex coord = std::exp(angleCoord * IM_I *
                                   (double)nSym); // nsym should be 2 or 4, checked in the funciton which calls this

          sum += coord * weight * dihedralAngle;
        }

        sum /= weightSum;

        // Normalize the curvature vectors. By doing so, we lose the property of adjusting the strength of the alignment
        // based on the strength of the curvature, but resolve any scaling issues between the magnitude of the normals
        // and the magnitude of the desired field.  Be careful when interpreting this as opposed to the usual direction
        // field optimization.
        dirVec[interiorFaceInd[f]] = unit(sum);
      }
    }

    double t = 0.1;  // this is something of a magical constant, see "Globally
                     // Optimal Direction Fields", eqn 9
                     // NOTE: This value is different from the one used for vertex fields; seems to work better?

    std::cout << "Solving smoothest field dirichlet problem with curvature term..." << std::endl;
    Eigen::VectorXcd RHS = massMatrix * (t * dirVec + b);
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix;
    solution = solveSquare(LHS, RHS);

  }
  // Otherwise find the general closest solution
  else {
    std::cout << "Solving smoothest field dirichlet problem..." << std::endl;
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix;
    Eigen::VectorXcd RHS = massMatrix * b;
    solution = solveSquare(LHS, RHS);
  }

  // Copy the result to a FaceData object
  FaceData<Complex> field(mesh);
  for (FacePtr f : mesh->faces()) {
    if (isInterior[f]) {
      field[f] = unit(solution[interiorFaceInd[f]]);
    } else {
      field[f] = unit(boundaryValues[f]);
    }
  }

  return field;
}

} // namespace

FaceData<Complex> computeSmoothestFaceDirectionField(Geometry<Euclidean>* geometry, int nSym, bool alignCurvature) {

  std::cout << "Computing globally optimal direction field in faces" << std::endl;

  if (alignCurvature && !(nSym == 2 || nSym == 4)) {
    throw std::logic_error("ERROR: It only makes sense to align with curvature when nSym = 2 or "
                           "4");
  }

  // Dispatch to either the boundary of no boundary variant depending on the mesh type
  bool hasBoundary = false;
  for (VertexPtr v : geometry->getMesh()->vertices()) {
    hasBoundary |= v.isBoundary();
  }

  if (hasBoundary) {
    std::cout << "Mesh has boundary, computing dirichlet boundary condition solution" << std::endl;
    return computeSmoothestFaceDirectionField_boundary(geometry, nSym, alignCurvature);
  } else {
    std::cout << "Mesh has no boundary, computing unit-norm solution" << std::endl;
    return computeSmoothestFaceDirectionField_noBoundary(geometry, nSym, alignCurvature);
  }
}

FaceData<int> computeFaceIndex(Geometry<Euclidean>* geometry, VertexData<Complex> directionField, int nSym) {
  HalfedgeMesh* mesh = geometry->getMesh();

  GeometryCache<Euclidean>& gc = geometry->cache;
  gc.requireFaceTransportCoefs();

  // Store the result here
  FaceData<int> indices(mesh);

  // TODO haven't tested that this correctly reports the index when it is larger
  // than +-1

  for (FacePtr f : mesh->faces()) {
    // Trace the direction field around the face and see how many times it
    // spins!
    double totalRot = 0;

    for (HalfedgePtr he : f.adjacentHalfedges()) {
      // Compute the rotation along the halfedge implied by the field
      Complex x0 = directionField[he.vertex()];
      Complex x1 = directionField[he.twin().vertex()];
      Complex transport = std::pow(gc.vertexTransportCoefs[he], nSym);

      // Find the difference in angle
      double theta0 = std::arg(transport * x0);
      double theta1 = std::arg(x1);
      double deltaTheta = regularizeAngle(theta1 - theta0 + PI) - PI; // regularize to [-PI,PI]

      totalRot += deltaTheta; // accumulate
    }

    // Compute the net rotation and corresponding index
    int index = static_cast<int>(std::round(totalRot / (2 * PI))); // should be very close to a multiple of 2PI
    indices[f] = index;
  }

  return indices;
}

VertexData<int> computeVertexIndex(Geometry<Euclidean>* geometry, FaceData<Complex> directionField, int nSym) {

  HalfedgeMesh* mesh = geometry->getMesh();
  GeometryCache<Euclidean>& gc = geometry->cache;
  gc.requireFaceTransportCoefs();

  // Store the result here
  VertexData<int> indices(mesh);

  // TODO haven't tested that this correctly reports the index when it is larger
  // than +-1

  for (VertexPtr v : mesh->vertices()) {

    // Trace the direction field around the face and see how many times it
    // spins!
    double totalRot = 0;

    for (HalfedgePtr he : v.incomingHalfedges()) {
      // Compute the rotation along the halfedge implied by the field
      Complex x0 = directionField[he.face()];
      Complex x1 = directionField[he.twin().face()];
      Complex transport = std::pow(gc.faceTransportCoefs[he], nSym);

      // Find the difference in angle
      double theta0 = std::arg(transport * x0);
      double theta1 = std::arg(x1);
      double deltaTheta = std::arg(x1 / (transport * x0));

      totalRot += deltaTheta;
    }

    double angleDefect = geometry->angleDefect(v);
    totalRot += angleDefect * nSym;

    // Compute the net rotation and corresponding index
    int index = static_cast<int>(std::round(totalRot / (2 * PI))); // should be very close to a multiple of 2PI
    indices[v] = index;
  }

  return indices;
}

} // namespace geometrycentral


#include <Eigen/Eigen>
#include <Eigen/Eigenvalues>

#include "py4dgeo/compute.hpp"
#include "py4dgeo/kdtree.hpp"
#include "py4dgeo/openmp.hpp"
#include "py4dgeo/py4dgeo.hpp"

#include <algorithm>
#include <complex>
#include <vector>

namespace py4dgeo {

void
compute_multiscale_directions(const Epoch& epoch,
                              EigenPointCloudConstRef corepoints,
                              const std::vector<double>& scales,
                              EigenNormalSetConstRef orientation,
                              EigenNormalSetRef result)
{
  // Instantiate a container for the first thrown exception in
  // the following parallel region.
  CallbackExceptionVault vault;
#ifdef PY4DGEO_WITH_OPENMP
#pragma omp parallel for schedule(dynamic, 1)
#endif
  for (IndexType i = 0; i < corepoints.rows(); ++i) {
    vault.run([&]() {
      double highest_planarity = 0.0;
      for (auto scale : scales) {
        // Find the working set on this scale
        KDTree::RadiusSearchResult points;
        auto qp = corepoints.row(i).eval();
        epoch.kdtree.radius_search(&(qp(0, 0)), scale, points);
        auto subset = epoch.cloud(points, Eigen::all).cast<double>();

        // Calculate covariance matrix
        auto centered = subset.rowwise() - subset.colwise().mean();
        auto cov = (centered.adjoint() * centered) / double(subset.rows() - 1);
        auto coveval = cov.eval();

        // Calculate Eigen vectors
        Eigen::SelfAdjointEigenSolver<decltype(coveval)> solver(coveval);
        const auto& evalues = solver.eigenvalues();

        // Calculate planarity
        double planarity = (evalues[1] - evalues[0]) / evalues[2];
        if (planarity > highest_planarity) {
          highest_planarity = planarity;

          double prod =
            (solver.eigenvectors().col(0).dot(orientation.row(0).transpose()));
          double sign = (prod < 0.0) ? -1.0 : 1.0;
          result.row(i) = sign * solver.eigenvectors().col(0);
        }
      }
    });
  }

  // Potentially rethrow an exception that occurred in above parallel region
  vault.rethrow();
}

}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// With contributions from Cornelius Steinhardt

#ifndef MTL_MATRIX_SVD_INCLUDE
#define MTL_MATRIX_SVD_INCLUDE

#include <cmath>
#include <limits>
#include <algorithm>
#include <boost/numeric/mtl/matrix/strict_upper.hpp>
#include <boost/numeric/mtl/operation/diagonal.hpp>
#include <boost/numeric/mtl/operation/one_norm.hpp>
#include <boost/numeric/mtl/operation/sub_matrix.hpp>
#include <boost/numeric/mtl/operation/trans.hpp>
#include <boost/numeric/mtl/operation/two_norm.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace mat {

/// Returns A=S*V*D' for matrix A as references
template <typename Matrix>
inline void svd(const Matrix& A, Matrix& S, Matrix& V, Matrix& D, double tol= 10e-10)
{
	vampir_trace<3037> tracer;
    typedef typename Collection<Matrix>::value_type   value_type;
    typedef typename Collection<Matrix>::size_type    size_type;
    size_type        ncols= num_cols(A), nrows= num_rows(A), loops, col= ncols, row= nrows;
    value_type       ref, zero= math::zero(ref), one= math::one(ref);
    double 	     err(std::numeric_limits<double>::max()), e, f;

    if (nrows != ncols) // important for right dimension
	std::swap(row, col);

     //init
    Matrix Q(row,row),  R(row,col),  VT(row,col), E(row,col),
 	   QT(col,col), RT(col,row);

    loops= 100 * std::max(nrows,ncols);
    S= one; D= one; E= zero;
    for (size_type i= 0; err > tol && i < loops; ++i) {
	boost::tie(QT, RT)= qr(V);
 	S*= QT;
	VT= trans(RT);
	boost::tie(Q, R)= qr(VT);
	D*= Q;
	E= triu(R,1);
	V= trans(R);

	//ready for exit when upper(R)=0
	f= two_norm(diagonal(R));
	e= one_norm(E);
	if ( f== zero ) f= 1;
	err= e/f;
    } //end for

    {
	V= 0;
	mtl::mat::inserter<Matrix>  ins_V(V);
	mtl::mat::inserter<Matrix,  mtl::operations::update_times<value_type> > ins_S(S);

	for (size_type i= 0, end= std::min(nrows, ncols); i < end; i++) {
	    ins_V[i][i] << std::abs(R[i][i]);
	    if (R[i][i] < zero)
		for (size_type j= 0; j < nrows; j++)
		    ins_S[j][i] << -1;  //carefull changing: multiplication with minus one
	}
    }
}

/// Returns A=S*V*D' for matrix A as triplet
template <typename Matrix>
boost::tuple<Matrix, Matrix, Matrix >
inline svd(const Matrix& A, double tol= 10e-10)
{
	vampir_trace<3038> tracer;
    typedef typename Collection<Matrix>::size_type    size_type;
    size_type    ncols= num_cols(A), nrows= num_rows(A), col= ncols, row= nrows;
    if (nrows != ncols) // important for right dimension
	std::swap(row, col);

    Matrix       ST(col,col), V(A), D(row,row);
    svd(A, ST, V, D, tol);
    return boost::make_tuple(ST, V, D);
}

}} // namespace mtl::matrix

#endif // MTL_MATRIX_SVD_INCLUDE


#include <cmath>
#include <array>
#include <stdexcept>
#ifdef HAS_EIGEN3
#include <Eigen/Eigenvalues>
#endif

#include <Kernels/precision.hpp>
#include <Physics/InitialField.h>
#include <Model/Setup.h>
#include <Solver/Interoperability.h>

extern seissol::Interoperability e_interoperability;

seissol::physics::Planarwave::Planarwave(real phase)
  : m_setVar(27),
    m_kVec{3.14159265358979323846, 3.14159265358979323846, 3.14159265358979323846},
    m_phase(phase)
{
#ifdef HAS_EIGEN3
  const double rho = 1.0;
  const double mu = 1.0;
  const double lambda = 2.0;
  const double Qp = 20.0;
  const double Qs = 10.0;

  seissol::model::Material material;
  e_interoperability.fitAttenuation(rho, mu, lambda, Qp, Qs, material);

  std::complex<real> planeWaveOperator[NUMBER_OF_QUANTITIES*NUMBER_OF_QUANTITIES];
  model::getPlaneWaveOperator(material, m_kVec.data(), planeWaveOperator);

  using Matrix = Eigen::Matrix<std::complex<real>, NUMBER_OF_QUANTITIES, NUMBER_OF_QUANTITIES, Eigen::ColMajor>;
  using Vector = Eigen::Matrix<std::complex<real>, NUMBER_OF_QUANTITIES, 1, Eigen::ColMajor>;
  Matrix A(planeWaveOperator);
  Eigen::ComplexEigenSolver<Matrix> ces;
  ces.compute(A);

  auto eigenvalues = ces.eigenvalues();
  for (size_t i = 0; i < NUMBER_OF_QUANTITIES; ++i) {
    m_lambdaA[i] = eigenvalues(i,0);
  }

  Vector ic;
  for (size_t j = 0; j < 9; ++j) {
    ic(j) = 1.0;
  }
  for (size_t j = 9; j < NUMBER_OF_QUANTITIES; ++j) {
    ic(j) = 0.0;
  }

  auto eigenvectors = ces.eigenvectors();
  Vector amp = eigenvectors.colPivHouseholderQr().solve(ic);
  for (int j = 0; j < m_setVar; ++j) {
    m_varField.push_back(j);
    m_ampField.push_back(amp(j));
  }

  auto R = yateto::DenseTensorView<2,std::complex<real>>(m_eigenvectors, {NUMBER_OF_QUANTITIES, NUMBER_OF_QUANTITIES});
  for (size_t j = 0; j < NUMBER_OF_QUANTITIES; ++j) {
    for (size_t i = 0; i < NUMBER_OF_QUANTITIES; ++i) {
      R(i,j) = eigenvectors(i,j);
    }
  }
#else
  throw std::runtime_error("Eigen3 required for anelastic planarwave.");
#endif
}

void seissol::physics::Planarwave::evaluate(  double time,
                                              std::vector<std::array<double, 3>> const& points,
                                              yateto::DenseTensorView<2,real,unsigned>& dofsQP ) const
{
  dofsQP.setZero();

  auto R = yateto::DenseTensorView<2,std::complex<real>>(
             const_cast<std::complex<real>*>(m_eigenvectors),
             {NUMBER_OF_QUANTITIES, NUMBER_OF_QUANTITIES}
           );
  for (int v = 0; v < m_setVar; ++v) {
    const auto omega =  m_lambdaA[m_varField[v]];
    for (unsigned j = 0; j < dofsQP.shape(1); ++j) {
      for (size_t i = 0; i < points.size(); ++i) {
        dofsQP(i,j) += (R(j,m_varField[v]) * m_ampField[v] *
                        std::exp(std::complex<real>(0.0, 1.0) * (
                          omega * time - m_kVec[0]*points[i][0] - m_kVec[1]*points[i][1] - m_kVec[2]*points[i][2] + m_phase
                        ))).real();
      }
    }
  }
}


#include <utility>
#include <random>
#include <vector>
#include <random>
#include <algorithm>
#include <iostream>
#include <fstream>

#include <Eigen/Dense>

typedef std::pair<double, double> CoordinatePair;
typedef struct {
  double value;
  CoordinatePair location;
} Target;

const std::string ln = "\r\n";
const std::string del = " ";

const char* DATA_FILE_ARG = "data";
const char* NUM_NODES_ARG = "nodes";
const char* SENSING_RANGE_ARG = "sense_range";
const char* COMMUNICATION_RANGE_ARG = "comm_range";
const char* FIELD_X_ARG = "x";
const char* FIELD_Y_ARG = "y";
const char* FIELD_CELLS_ARG = "cells";

const double C_W_CONSTANT = 0.01;

const int SEED = 5;
const double NO_READING = -9999.9999;

const double ERROR_THRESHOLD = 0.0001;

class Configurations{
 public:
  std::string data_file_name;
  int number_of_sensor_nodes;
  double sensing_range;
  double communication_range;
  double field_x_size;
  double field_y_size;
  int field_cells;

  Configurations() {
    data_file_name = "data_files_should_have_names";
    number_of_sensor_nodes = 10;
    sensing_range = 1.6;
    communication_range = 1.5;
    field_x_size = 4.0;
    field_y_size = 4.0;
    field_cells = 25;
  };
};

Configurations ProcessCommandLineArguments(int pArgc, char** pArguments);

CoordinatePair GenerateNewCoordinatePair(
  std::default_random_engine& random_generator,
  const Configurations& configurations);

double GenerateNoisyReading(
  Target target,
  double constant,
  CoordinatePair sensor_node_location,
  CoordinatePair average_sensor_location,
  double sensing_range,
  std::default_random_engine generator);

double ComputeDistance(
  CoordinatePair node_location,
  CoordinatePair reckoning_point);

double ComputeNoiseCovariance(
  const CoordinatePair& node_coordinates,
  const CoordinatePair& reckoning_point,
  double p_weight_constant,
  double p_node_sensing_range);

int CountNeighbors(
  double communication_range,
  CoordinatePair source,
  std::vector<CoordinatePair> all_nodes);

int CountNeighborsWhoSenseTarget(
  double communication_range,
  CoordinatePair source,
  std::vector<CoordinatePair> all_nodes,
  Eigen::VectorXd estimates);

double ComputeAverageEstimate(
  Eigen::VectorXd estimates,
  Eigen::MatrixXd weights,
  std::function<double(Eigen::VectorXd, Eigen::MatrixXd)> averaging_method);

double ComputeMaxDegreeWeight(
  int source_node_index,
  double source_node_measurement,
  int neighbor_index,
  double neighbor_measurement,
  double communication_range,
  const std::vector<CoordinatePair>& sensor_nodes,
  const Eigen::MatrixXd weights);

double ComputeMetropolisWeight(
  int source_node_index,
  double source_node_measurement,
  int neighbor_index,
  double neighbor_measurement,
  double communication_range,
  const std::vector<CoordinatePair>& sensor_nodes,
  const Eigen::MatrixXd weights);

bool EmptyEstimatePresent(Eigen::VectorXd estimates);

bool SomeNodeNotConverged(
  Eigen::VectorXd estimates,
  double average_estimate,
  double error_threshold,
  bool method);

void DumpResultsToFile(
  Configurations configurations,
  std::string filename,
  std::vector<CoordinatePair> nodes,
  std::vector<Eigen::VectorXd> estimates);

std::vector<Eigen::VectorXd> MaxDegreeAnalysis(
  Configurations configurations,
  Target target,
  std::vector<CoordinatePair> sensor_nodes,
  CoordinatePair average_node_location,
  std::default_random_engine random_generator,
  Eigen::VectorXd initial_estimates);

std::vector<Eigen::VectorXd> MetropolisAnalysis(
  Configurations configurations,
  Target target,
  std::vector<CoordinatePair> sensor_nodes,
  CoordinatePair average_node_location,
  std::default_random_engine random_generator,
  Eigen::VectorXd initial_estimates);

std::vector<Eigen::VectorXd> WeightDesign1Analysis(
  Configurations configurations,
  Target target,
  std::vector<CoordinatePair> sensor_nodes,
  CoordinatePair average_node_location,
  std::default_random_engine random_generator,
  Eigen::VectorXd initial_estimates);

std::vector<Eigen::VectorXd> WeightDesign2Analysis(
  Configurations configurations,
  Target target,
  std::vector<CoordinatePair> sensor_nodes,
  CoordinatePair average_node_location,
  std::default_random_engine random_generator,
  Eigen::VectorXd initial_estimates);

double ComputeWeightDesign1Weight(
  int source_node_index,
  double source_node_measurement,
  int neighbor_index,
  double neighbor_measurement,
  double communication_range,
  double sensing_range,
  CoordinatePair target_location,
  const std::vector<CoordinatePair>& sensor_nodes,
  const Eigen::MatrixXd weights);

double ComputeWeightDesign2Weight(
  int source_node_index,
  double source_node_measurement,
  int neighbor_index,
  double neighbor_measurement,
  double communication_range,
  double sensing_range,
  CoordinatePair target_location,
  const std::vector<CoordinatePair>& sensor_nodes,
  const Eigen::MatrixXd weights,
  Eigen::VectorXd estimates);

std::vector<Target> ReadInData(Configurations configurations, std::string filename);

void DumpFieldDataToFile(
  Configurations configurations,
  std::string filename,
  std::vector<CoordinatePair> nodes,
  std::vector<double> estimates);

int
main(int arg_count, char** arg_values) {
  Configurations configurations = ProcessCommandLineArguments(
    arg_count,
    arg_values
  );

  // setup (RNGs 'n' stuff)
  std::default_random_engine random_generator(1);

  // generate targets
  std::vector<Target> targets;
  targets.push_back({50.0, {0.0, 0.0}});

  // generate node coordinates
  std::vector<CoordinatePair> sensor_nodes;
  CoordinatePair average_node_location = {0.0, 0.0};
  for (int i = 0; i < configurations.number_of_sensor_nodes; ++i) {
    sensor_nodes.push_back(
      GenerateNewCoordinatePair(random_generator, configurations)
    );

    std::cout << "x: " << sensor_nodes[i].first << " y: " << sensor_nodes[i].second << std::endl;

    average_node_location.first += sensor_nodes[i].first;
      average_node_location.second += sensor_nodes[i].second;
  }
  average_node_location.first /= (double) configurations.number_of_sensor_nodes;
  average_node_location.second /=
    (double) configurations.number_of_sensor_nodes;

  std::cout << "x: " << average_node_location.first << " y: " << average_node_location.second << std::endl;

  // for each target
  for (Target target : targets) {

    Eigen::VectorXd estimates(configurations.number_of_sensor_nodes);
    for (int i = 0; i < configurations.number_of_sensor_nodes; ++i) {
      estimates(i) = GenerateNoisyReading(
        target,
        0.01,
        sensor_nodes[i],
        average_node_location,
        configurations.sensing_range,
        random_generator
      );

      std::cout << "estimate: " << estimates(i) << std::endl;
    }

    std::vector<Eigen::VectorXd> results = MaxDegreeAnalysis(
      configurations,
      target,
      sensor_nodes,
      average_node_location,
      random_generator,
      estimates
    );
    DumpResultsToFile(configurations, "MaxDegreeResults.txt", sensor_nodes, results);

    results = MetropolisAnalysis(
      configurations,
      target,
      sensor_nodes,
      average_node_location,
      random_generator,
      estimates
    );
    DumpResultsToFile(configurations, "MetropolisResults.txt", sensor_nodes, results);

    results = WeightDesign1Analysis(
      configurations,
      target,
      sensor_nodes,
      average_node_location,
      random_generator,
      estimates
    );
    DumpResultsToFile(configurations, "WeightDesign1Results.txt", sensor_nodes, results);

    results = WeightDesign2Analysis(
      configurations,
      target,
      sensor_nodes,
      average_node_location,
      random_generator,
      estimates
    );
    DumpResultsToFile(configurations, "WeightDesign2Results.txt", sensor_nodes, results);
  }

  // part 2/////////////////////////////////////////////////////////////////////////////////
  configurations.field_x_size = 12.0;
  configurations.field_y_size = 12.0;
  configurations.field_cells = 25;
  configurations.number_of_sensor_nodes = 30;
  configurations.sensing_range = 5.0;
  configurations.communication_range = 4.5;

  sensor_nodes.clear();
  average_node_location = {0.0, 0.0};
  for (int i = 0; i < configurations.number_of_sensor_nodes; ++i) {
    sensor_nodes.push_back(
      {0.0, 0.0}
    );

    std::cout << "x: " << sensor_nodes[i].first << " y: " << sensor_nodes[i].second << std::endl;

    average_node_location.first += sensor_nodes[i].first;
      average_node_location.second += sensor_nodes[i].second;
  }
  average_node_location.first /= (double) configurations.number_of_sensor_nodes;
  average_node_location.second /=
    (double) configurations.number_of_sensor_nodes;

  std::cout << "x: " << average_node_location.first << " y: " << average_node_location.second << std::endl;

  std::vector<double> final_cell_estimates_1;
  std::vector<double> final_cell_estimates_2;

  std::vector<Target> new_targets = ReadInData(configurations, "field1.txt");

  // for each target
  for (Target target : new_targets) {

    Eigen::VectorXd estimates(configurations.number_of_sensor_nodes);
    for (int i = 0; i < configurations.number_of_sensor_nodes; ++i) {
      estimates(i) = GenerateNoisyReading(
        target,
        0.01,
        sensor_nodes[i],
        average_node_location,
        configurations.sensing_range,
        random_generator
      );

      std::cout << "estimate: " << estimates(i) << std::endl;
    }

    std::vector<Eigen::VectorXd> wd1_results = WeightDesign1Analysis(
      configurations,
      target,
      sensor_nodes,
      average_node_location,
      random_generator,
      estimates
    );
    std::vector<Eigen::VectorXd> wd2_results = WeightDesign2Analysis(
      configurations,
      target,
      sensor_nodes,
      average_node_location,
      random_generator,
      estimates
    );

    final_cell_estimates_1.push_back(wd1_results.back()(0));
    final_cell_estimates_2.push_back(wd2_results.back()(0));
  }

  DumpFieldDataToFile(configurations, "WeightDesign1_field.txt", sensor_nodes, final_cell_estimates_1);
  DumpFieldDataToFile(configurations, "WeightDesign2_field.txt", sensor_nodes, final_cell_estimates_2);

  return 0;
}

Configurations
ProcessCommandLineArguments(int pArgc, char** pArguments) {
  Configurations configurations;

  for (int i = 1; i < pArgc; ++i) {
    char* arg_value_pair = pArguments[i];
    std::string argument = strtok(arg_value_pair, "-=");
    char* value = strtok(NULL, "=");

    if (argument == DATA_FILE_ARG) {
      configurations.data_file_name = value;
      printf(
        "Data File: %s\n",
        configurations.data_file_name.c_str()
      );
    } else if (argument == NUM_NODES_ARG) {
      configurations.number_of_sensor_nodes = atoi(value);
      printf(
        "Number of senor nodes: %i\n",
        configurations.number_of_sensor_nodes
      );
    } else if (argument == SENSING_RANGE_ARG) {
      configurations.sensing_range = strtod(value,NULL);
      printf(
        "Node sensing range: %f\n",
        configurations.sensing_range
      );
    } else if (argument == COMMUNICATION_RANGE_ARG) {
      configurations.communication_range = strtod(value, NULL);
      printf(
        "Node communication range: %f\n",
        configurations.communication_range
      );
    } else if (argument == FIELD_X_ARG) {
      configurations.field_x_size = strtod(value, NULL);
      printf(
        "Field X size: %f\n",
        configurations.field_x_size
      );
    } else if (argument == FIELD_Y_ARG) {
      configurations.field_y_size = strtod(value, NULL);
      printf(
        "Field Y size: %f\n",
        configurations.field_y_size
      );
    } else if (argument == FIELD_CELLS_ARG) {
      configurations.field_cells = atoi(value);
      printf(
        "Cells per side of the field: %i\n",
        configurations.field_cells
      );
    }else {
      printf(
        "%s is an unrecognized argument. Program terminating.\n",
        argument.c_str()
      );
      throw std::exception();
    }
  }

  return configurations;
}

CoordinatePair
GenerateNewCoordinatePair(
  std::default_random_engine& random_generator,
  const Configurations& configurations) {

  CoordinatePair new_coordinates;

  new_coordinates.first =
    random_generator() % (int) (configurations.field_x_size * 5.0);
  new_coordinates.first /= 10.0;
  if (random_generator() % 2 == 0) {
    new_coordinates.first *= -1.0;
  }

  new_coordinates.second =
    random_generator() % (int) (configurations.field_y_size * 5.0);
  new_coordinates.second /= 10.0;
  if (random_generator() % 2 == 0) {
    new_coordinates.second *= -1.0;
  }

  return new_coordinates;
}

double GenerateNoisyReading(
  Target target,
  double constant,
  CoordinatePair sensor_node_location,
  CoordinatePair average_sensor_location,
  double sensing_range,
  std::default_random_engine generator) {

  double distance = ComputeDistance(sensor_node_location, target.location);

  if (distance <= sensing_range) {
    std::normal_distribution<double> noise_distribution(
      target.value,
      ComputeNoiseCovariance(
        sensor_node_location,
        average_sensor_location,
        constant,
        sensing_range
      )
    );

    return noise_distribution(generator);
  } else {
    return NO_READING;
  }
}

double ComputeDistance(
  CoordinatePair node_location,
  CoordinatePair reckoning_point) {

  // Euclidean Distance = sqrt((x_0 - x_1)^2 + (y_0 - y_1)^2)

  double x_diff = node_location.first - reckoning_point.first;
  double y_diff = node_location.second - reckoning_point.second;

  return sqrt((x_diff * x_diff) + (y_diff * y_diff));
}

double ComputeNoiseCovariance(
  const CoordinatePair& node_coordinates,
  const CoordinatePair& reckoning_point,
  double constant,
  double sensing_range) {

  double distance = ComputeDistance(
    node_coordinates,
    reckoning_point
  );

  double numerator = (distance * distance) + constant;

  return numerator / (sensing_range * sensing_range);
}

double ComputeAverageEstimate(
  Eigen::VectorXd estimates,
  Eigen::MatrixXd weights,
  std::function<double(Eigen::VectorXd, Eigen::MatrixXd)> averaging_method) {

  double sum = 0.0;
  int n = 0;
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) != NO_READING) {
      sum += estimates(i);
      n++;
    }
  }

  return sum / (double) n;
}

double ComputeMaxDegreeWeight(
  int source_node_index,
  double source_node_measurement,
  int neighbor_index,
  double neighbor_measurement,
  double communication_range,
  const std::vector<CoordinatePair>& sensor_nodes,
  const Eigen::MatrixXd weights) {

  if (source_node_index != neighbor_index) {
    if (source_node_measurement == NO_READING) {
      return 0.0;
    }

    int num_neighbors = 0;
    for (int i = 0; i < sensor_nodes.size(); ++i) {
      if (i != source_node_index) {
        double separation = ComputeDistance(
          sensor_nodes[source_node_index],
          sensor_nodes[neighbor_index]
        );

        if (separation <= communication_range) {
          num_neighbors++;
        }
      }
    }

    if (neighbor_measurement != NO_READING) {
      double separation = ComputeDistance(
        sensor_nodes[source_node_index],
        sensor_nodes[neighbor_index]
      );

      if (separation <= communication_range) {
        return 1.0 / (double) sensor_nodes.size();
      }
    }

    return 0.0;
  } else {
    if (source_node_measurement == NO_READING) {
      return 1.0;
    } else {
      double neighbor_weights = 0.0;

      for (int j = 0; j < sensor_nodes.size(); ++j) {
        if (j != source_node_index) {
          neighbor_weights += weights(source_node_index, j);
        }
      }

      return 1.0 - neighbor_weights;
    }
  }
}

bool EmptyEstimatePresent(Eigen::VectorXd estimates) {
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) == NO_READING) {
      return true;
    }
  }

  return false;
}

bool SomeNodeNotConverged(
  Eigen::VectorXd estimates,
  double average_estimate,
  double error_threshold,
  bool method) {

  if (method) {
    for (int i = 0; i < estimates.size(); ++i) {
      if (estimates(i) != NO_READING &&
          fabs(estimates(i) - average_estimate) >= error_threshold) {
        return true;
      }
    }
  } else {
    double estimate = 0.0;
    for (int i = 0; i < estimates.size(); ++i) {
      if (estimate == 0.0 && estimates(i) != NO_READING) {
        estimate = estimates(i);
        break;
      }
    }

    for (int i = 0; i < estimates.size(); ++i) {
      if (estimates(i) != NO_READING &&
          (estimates(i) < (estimate - error_threshold) || (estimate + error_threshold) < estimates(i))) {
        return true;
      }
    }
  }

  return false;
}

void DumpResultsToFile(
  Configurations configurations,
  std::string filename,
  std::vector<CoordinatePair> nodes,
  std::vector<Eigen::VectorXd> estimates) {

  std::ofstream fout;
  fout.open(filename.c_str());

  for (CoordinatePair source : nodes) {
    int num_neighbors = 0;
    for (CoordinatePair neighbor : nodes) {
      if (configurations.communication_range <
          ComputeDistance(source, neighbor)) {
        num_neighbors++;
      }
    }

    fout << source.first << del << source.second << del << num_neighbors
         << ln;
  }

  for (Eigen::VectorXd snapshot : estimates) {
    fout << snapshot.transpose() << ln;
  }

  fout.close();
}

std::vector<Eigen::VectorXd> MaxDegreeAnalysis(
  Configurations configurations,
  Target target,
  std::vector<CoordinatePair> sensor_nodes,
  CoordinatePair average_node_location,
  std::default_random_engine random_generator,
  Eigen::VectorXd initial_estimates) {

  // take initial measurements
  Eigen::VectorXd estimates = initial_estimates;

  // determine weight matrix
  Eigen::MatrixXd weights(
    configurations.number_of_sensor_nodes,
    configurations.number_of_sensor_nodes
  );
  weights.setZero();
  for (int i = 0; i < configurations.number_of_sensor_nodes; ++i) {
    for (int j = 0; j < configurations.number_of_sensor_nodes; ++j) {
      if (i != j) {
        weights(i, j) = ComputeMaxDegreeWeight(
          i,
          estimates(i),
          j,
          estimates(j),
          configurations.communication_range,
          sensor_nodes,
          weights
        );
      }
    }

    weights(i, i) = ComputeMaxDegreeWeight(i, estimates(i), i, estimates(i), configurations.communication_range, sensor_nodes, weights);
  }

  std::cout << "Max-Degree weights" << std::endl << weights << std::endl;

  std::function<double(Eigen::VectorXd, Eigen::MatrixXd)> averaging_method;
  double average_estimate = 0;
  average_estimate = ComputeAverageEstimate(
    estimates,
    weights,
    averaging_method
  );

  std::cout << "average: " << average_estimate << std::endl;

  // iterate til consensus
  std::vector<Eigen::VectorXd> estimate_history;
  estimate_history.push_back(estimates);
  int l = 0;
  while (SomeNodeNotConverged(estimates, average_estimate, ERROR_THRESHOLD, true)) {
    estimates = weights * estimates;
    l++;
    estimate_history.push_back(estimates);
  }

  // update nodes that didn't see the target
  double consensus;
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) != NO_READING) {
      consensus = estimates(i);
      break;
    }
  }
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) == NO_READING) {
      estimates(i) = consensus;
    }
  }
  estimate_history.push_back(estimates);

  return estimate_history;
}

std::vector<Eigen::VectorXd> MetropolisAnalysis(
  Configurations configurations,
  Target target,
  std::vector<CoordinatePair> sensor_nodes,
  CoordinatePair average_node_location,
  std::default_random_engine random_generator,
  Eigen::VectorXd initial_estimates) {

  // take initial measurements
  Eigen::VectorXd estimates = initial_estimates;

  // determine weight matrix
  Eigen::MatrixXd weights(
    configurations.number_of_sensor_nodes,
    configurations.number_of_sensor_nodes
  );
  weights.setZero();
  for (int i = 0; i < configurations.number_of_sensor_nodes; ++i) {
    for (int j = 0; j < configurations.number_of_sensor_nodes; ++j) {
      if (i != j) {
        weights(i, j) = ComputeMetropolisWeight(
          i,
          estimates(i),
          j,
          estimates(j),
          configurations.communication_range,
          sensor_nodes,
          weights
        );
      }
    }

    weights(i, i) = ComputeMetropolisWeight(i, estimates(i), i, estimates(i), configurations.communication_range, sensor_nodes, weights);
  }

  std::cout << "Metropolis Weights: " << std::endl << weights << std::endl;

  double average_estimate = 0.0;
  int num_in_average = 0;
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) != NO_READING) {
      average_estimate += estimates(i);
      num_in_average++;
    }
  }
  average_estimate /= (double) num_in_average;

  std::cout << "average: " << average_estimate << std::endl;

  // iterate til consensus
  std::vector<Eigen::VectorXd> estimate_history;
  estimate_history.push_back(estimates);
  int l = 0;
  while (SomeNodeNotConverged(estimates, average_estimate, ERROR_THRESHOLD, true)) {
    estimates = weights * estimates;
    l++;
    estimate_history.push_back(estimates);
  }

  // update nodes that didn't see the target
  double consensus;
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) != NO_READING) {
      consensus = estimates(i);
      break;
    }
  }
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) == NO_READING) {
      estimates(i) = consensus;
    }
  }
  estimate_history.push_back(estimates);

  return estimate_history;
}

int CountNeighbors(
  double communication_range,
  CoordinatePair source,
  std::vector<CoordinatePair> all_nodes) {

  int num_neighbors = 0;

  for (CoordinatePair node : all_nodes) {
    if (node != source &&
        ComputeDistance(source, node) <= communication_range) {

      num_neighbors++;
    }
  }

  return num_neighbors;
}

double ComputeMetropolisWeight(
  int source_node_index,
  double source_node_measurement,
  int neighbor_index,
  double neighbor_measurement,
  double communication_range,
  const std::vector<CoordinatePair>& sensor_nodes,
  const Eigen::MatrixXd weights) {

  if (source_node_index != neighbor_index) {
    if (source_node_measurement != NO_READING &&
        neighbor_measurement != NO_READING &&
        ComputeDistance(sensor_nodes[source_node_index], sensor_nodes[neighbor_index]) <= communication_range) {

      return 1.0 / std::max(
        CountNeighbors(
          communication_range,
          sensor_nodes[source_node_index],
          sensor_nodes
        ),
        CountNeighbors(
          communication_range,
          sensor_nodes[neighbor_index],
          sensor_nodes
        )
      );
    }

    return 0.0;
  } else {
    if (source_node_measurement == NO_READING) {
      return 1.0;
    } else {
      double neighbor_weights = 0.0;

      for (int j = 0; j < sensor_nodes.size(); ++j) {
        if (j != source_node_index) {
          neighbor_weights += weights(source_node_index, j);
        }
      }

      return 1.0 - neighbor_weights;
    }
  }
}

std::vector<Eigen::VectorXd> WeightDesign1Analysis(
  Configurations configurations,
  Target target,
  std::vector<CoordinatePair> sensor_nodes,
  CoordinatePair average_node_location,
  std::default_random_engine random_generator,
  Eigen::VectorXd initial_estimates) {

  // take initial measurements
  Eigen::VectorXd estimates = initial_estimates;

  // determine weight matrix
  Eigen::MatrixXd weights(
    configurations.number_of_sensor_nodes,
    configurations.number_of_sensor_nodes
  );
  weights.setZero();
  for (int i = 0; i < configurations.number_of_sensor_nodes; ++i) {
    for (int j = 0; j < configurations.number_of_sensor_nodes; ++j) {
      if (i != j) {
        weights(i, j) = ComputeWeightDesign1Weight(
          i,
          estimates(i),
          j,
          estimates(j),
          configurations.communication_range,
          configurations.sensing_range,
          target.location,
          sensor_nodes,
          weights
        );
      }
    }

    weights(i, i) = ComputeWeightDesign1Weight(i, estimates(i), i, estimates(i),
      configurations.communication_range,
      configurations.sensing_range,
      target.location,
      sensor_nodes,
      weights
    );
  }

  std::cout << "Weight Design 1 Weights:" << std::endl << weights << std::endl;

  double average_estimate = 0.0;
  double total_weight = 0.0;
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) != NO_READING) {
      average_estimate += estimates(i) * weights(i, i);
      total_weight += weights(i, i);
    }
  }
  average_estimate /= total_weight;

  std::cout << "weighted average: " << average_estimate << std::endl;

  // iterate til consensus
  std::vector<Eigen::VectorXd> estimate_history;
  estimate_history.push_back(estimates);
  int l = 0;
  while (SomeNodeNotConverged(estimates, average_estimate, ERROR_THRESHOLD, false)) {
    estimates = weights * estimates;
    l++;
    estimate_history.push_back(estimates);

// char y;
// std::cin >> y;
// std::cout << estimates.transpose() << std::endl;
  }

  // update nodes that didn't see the target
  double consensus;
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) != NO_READING) {
      consensus = estimates(i);
      break;
    }
  }
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) == NO_READING) {
      estimates(i) = consensus;
    }
  }
  estimate_history.push_back(estimates);

  return estimate_history;
}

double ComputeWeightDesign1Weight(
  int source_node_index,
  double source_node_measurement,
  int neighbor_index,
  double neighbor_measurement,
  double communication_range,
  double sensing_range,
  CoordinatePair target_location,
  const std::vector<CoordinatePair>& sensor_nodes,
  const Eigen::MatrixXd weights) {

  if (source_node_index != neighbor_index) {
    if (source_node_measurement != NO_READING &&
        neighbor_measurement != NO_READING &&
        ComputeDistance(sensor_nodes[source_node_index], sensor_nodes[neighbor_index]) <= communication_range) {

      return C_W_CONSTANT /
        (ComputeNoiseCovariance(
          sensor_nodes[source_node_index],
          target_location,
          C_W_CONSTANT,
          sensing_range
        ) +
        ComputeNoiseCovariance(
          sensor_nodes[source_node_index],
          target_location,
          C_W_CONSTANT,
          sensing_range
        ))
      ;
    }

    return 0.0;
  } else {
    if (source_node_measurement == NO_READING) {
      return 1.0;
    } else {
      double neighbor_weights = 0.0;

      for (int j = 0; j < sensor_nodes.size(); ++j) {
        if (j != source_node_index) {
          neighbor_weights += weights(source_node_index, j);
        }
      }

      return 1.0 - neighbor_weights;
    }
  }
}

std::vector<Eigen::VectorXd> WeightDesign2Analysis(
  Configurations configurations,
  Target target,
  std::vector<CoordinatePair> sensor_nodes,
  CoordinatePair average_node_location,
  std::default_random_engine random_generator,
  Eigen::VectorXd initial_estimates) {

  // take initial measurements
  Eigen::VectorXd estimates = initial_estimates;

  // determine weight matrix
  Eigen::MatrixXd weights(
    configurations.number_of_sensor_nodes,
    configurations.number_of_sensor_nodes
  );
  weights.setZero();
  for (int i = 0; i < configurations.number_of_sensor_nodes; ++i) {
    weights(i, i) = ComputeWeightDesign2Weight(i, estimates(i), i, estimates(i),
      configurations.communication_range,
      configurations.sensing_range,
      target.location,
      sensor_nodes,
      weights,
      estimates
    );

    for (int j = 0; j < configurations.number_of_sensor_nodes; ++j) {
      if (i != j) {
        weights(i, j) = ComputeWeightDesign2Weight(
          i,
          estimates(i),
          j,
          estimates(j),
          configurations.communication_range,
          configurations.sensing_range,
          target.location,
          sensor_nodes,
          weights,
          estimates
        );
      }
    }
  }

  std::cout << "Weight Design 2 Weights:" << std::endl << weights << std::endl;

  double average_estimate = 0.0;
  double total_weight = 0.0;
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) != NO_READING) {
      average_estimate += estimates(i) * weights(i, i);
      total_weight += weights(i, i);
    }
  }
  average_estimate /= total_weight;

  std::cout << "weighted average: " << average_estimate << std::endl;

  // iterate til consensus
  std::vector<Eigen::VectorXd> estimate_history;
  estimate_history.push_back(estimates);
  int l = 0;
  while (SomeNodeNotConverged(estimates, average_estimate, ERROR_THRESHOLD, false)) {
    estimates = weights * estimates;
    l++;
    estimate_history.push_back(estimates);

// char y;
// std::cin >> y;
// std::cout << estimates.transpose() << std::endl;
  }

  // update nodes that didn't see the target
  double consensus;
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) != NO_READING) {
      consensus = estimates(i);
      break;
    }
  }
  for (int i = 0; i < estimates.size(); ++i) {
    if (estimates(i) == NO_READING) {
      estimates(i) = consensus;
    }
  }
  estimate_history.push_back(estimates);

  return estimate_history;
}

double ComputeWeightDesign2Weight(
  int source_node_index,
  double source_node_measurement,
  int neighbor_index,
  double neighbor_measurement,
  double communication_range,
  double sensing_range,
  CoordinatePair target_location,
  const std::vector<CoordinatePair>& sensor_nodes,
  const Eigen::MatrixXd weights,
  Eigen::VectorXd estimates) {

  if (source_node_index != neighbor_index) {
    if (source_node_measurement != NO_READING &&
        neighbor_measurement != NO_READING &&
        ComputeDistance(sensor_nodes[source_node_index], sensor_nodes[neighbor_index]) <= communication_range) {

        return (1.0 - weights(source_node_index, source_node_index)) /
          (double) CountNeighborsWhoSenseTarget(communication_range, sensor_nodes[source_node_index], sensor_nodes, estimates);
    }

    return 0.0;
  } else {
    if (source_node_measurement == NO_READING) {
      return 1.0;
    } else {
      return C_W_CONSTANT / ComputeNoiseCovariance(sensor_nodes[source_node_index], target_location, C_W_CONSTANT, sensing_range);
    }
  }
}

int CountNeighborsWhoSenseTarget(
  double communication_range,
  CoordinatePair source,
  std::vector<CoordinatePair> all_nodes,
  Eigen::VectorXd estimates) {

  int num_neighbors = 0;

  for (int i = 0; i < all_nodes.size(); ++i) {
    CoordinatePair node = all_nodes[i];

    if (node != source &&
        ComputeDistance(source, node) <= communication_range &&
        estimates(i) != NO_READING) {

      num_neighbors++;
    }
  }

  return num_neighbors;
}

std::vector<Target> ReadInData(Configurations configurations, std::string filename) {

  std::ifstream fin;
  fin.open(filename.c_str());

  std::vector<Target> targets;

  char dummy;

  double x;
  double y;

  double increment = 0.5;
  double startval = -6.0;

  y = startval;
  for (int i = 0; i < configurations.field_cells; ++i) {
    x = startval;
    for (int j = 0; j < configurations.field_cells; ++j) {
      Target target;
      fin >> target.value >> dummy;
      target.value *= 1;
      target.location = {x, y};
      targets.push_back(target);

std::cout << target.value << ", ";

      x += increment;
    }

std::cout << std::endl;

    y += increment;
  }

char c;
std::cin >> c;

  fin.close();

  return targets;
}

void DumpFieldDataToFile(
  Configurations configurations,
  std::string filename,
  std::vector<CoordinatePair> nodes,
  std::vector<double> estimates) {

  std::ofstream fout;
  fout.open(filename.c_str());

  for (CoordinatePair source : nodes) {
    int num_neighbors = 0;
    for (CoordinatePair neighbor : nodes) {
      if (configurations.communication_range <
          ComputeDistance(source, neighbor)) {
        num_neighbors++;
      }
    }

    fout << source.first << ", " << source.second << ", " << num_neighbors
         << ln;
  }

  for (int i = 0; i < configurations.field_cells; ++i) {
    for (int j = 0; j < configurations.field_cells; ++j) {
      fout << estimates[(i * configurations.field_cells) + j] << ", ";
    }
    fout << ln;
  }

  fout.close();
}


#include "KD.h"
#include "Ray.h"
#include "Material.h"
#include "Triangle.h"

#include <algorithm>

#include <Eigen/Dense>

std::vector<KdNode> kd_tree;

bool AABB::intersects(const Ray& ray) const {
    auto p = ray.p;
    auto d = ray.d;

    auto d_inv = Eigen::Vector3f(1.0f/d[0], 1.0f/d[1], 1.0f/d[2]);

    float mins[3] = {min.x, min.y, min.z};
    float maxs[3] = {max.x, max.y, max.z};

    float t1 = (mins[0] - p[0]) * d_inv[0];
    float t2 = (maxs[0] - p[0]) * d_inv[0];

    float tmin = std::min(t1, t2);
    float tmax = std::max(t1, t2);

    for (int i = 1; i < 3; ++i) {
        t1 = (mins[i] - p[i]) * d_inv[i];
        t2 = (maxs[i] - p[i]) * d_inv[i];

        tmin = std::max(tmin, std::min(std::min(t1, t2), tmax));
        tmax = std::min(tmax, std::max(std::max(t1, t2), tmin));
    }

    return tmax > std::max(tmin, 0.0f);
}

bool kd_load(const char* fileName, std::vector<KdNode> &kdTree) {
    FILE* fp = fopen(fileName, "r+");
    if (!fp) {
        fprintf(stderr, "Could not load %s\n", fileName);
        return false;
    }
    char temp[256];
    int nodeId = -1;
    while (true) {
        *temp = '\0';
        nodeId++;
        int ignore = fscanf(fp, "%s{ ", temp);
        if (!ignore) {
            fprintf(stderr, "Something catastrophic went wrong\n");
            exit(EXIT_FAILURE);
        }
        if (strcmp(temp, "inner{") == 0) {
            KdNode kd;
            kd.nodeId = nodeId;
            kd.isLeaf = false;
            int ignore = fscanf(fp, "%f %f %f %f %f %f ; %d %d %d %f }",
                                &kd.boundingBox.min.x,
                                &kd.boundingBox.min.y,
                                &kd.boundingBox.min.z,
                                &kd.boundingBox.max.x,
                                &kd.boundingBox.max.y,
                                &kd.boundingBox.max.z,
                                &kd.leftChildId,
                                &kd.rightChildId,
                                &kd.splitAxis,
                                &kd.splitPosition);
            if (!ignore) {
                fprintf(stderr, "Something catastrophic went wrong\n");
                exit(EXIT_FAILURE);
            }
            kdTree.push_back(kd);
        } else if (strcmp(temp, "leaf{") == 0) {
            KdNode kd;
            kd.nodeId = nodeId;
            kd.isLeaf = true;
            int ignore = fscanf(fp, "%f %f %f %f %f %f ;",
                                &kd.boundingBox.min.x,
                                &kd.boundingBox.min.y,
                                &kd.boundingBox.min.z,
                                &kd.boundingBox.max.x,
                                &kd.boundingBox.max.y,
                                &kd.boundingBox.max.z);
            if (!ignore) {
                fprintf(stderr, "Something catastrophic went wrong\n");
                exit(EXIT_FAILURE);
            }
            char token[256];
            while (true) {
                int ignore = fscanf(fp, " %s", token);
                if (!ignore) {
                    fprintf(stderr, "Something catastrophic went wrong\n");
                    exit(EXIT_FAILURE);
                }
                if (strcmp(token, "}") == 0)
                    break;
                int triIndex = atoi(token);
                kd.triIndex.push_back(triIndex);
            }
            kdTree.push_back(kd);
        } else {
            break;
        }
    }
    return true;
}

SurfaceList kd_intersect(const Ray& ray, const std::vector<KdNode>& kd_tree, int id, const Material& mt) {
    auto node = kd_tree[id];
    if (node.isLeaf) {
        // Construct all leaf triangles
        SurfaceList leaves;
        for (int i : node.triIndex) {
            int k0 = gTriangles[i].indices[0];
            int k1 = gTriangles[i].indices[1];
            int k2 = gTriangles[i].indices[2];

            auto a = Eigen::Vector3f(gPositions[k0].x, gPositions[k0].y, gPositions[k0].z);
            auto b = Eigen::Vector3f(gPositions[k1].x, gPositions[k1].y, gPositions[k1].z);
            auto c = Eigen::Vector3f(gPositions[k2].x, gPositions[k2].y, gPositions[k2].z);

            auto n = ((b-a).cross(c-a)).normalized();

            auto triangle1 = std::unique_ptr<Triangle>(new Triangle(a,b,c,n,mt));
            auto triangle2 = std::unique_ptr<Triangle>(new Triangle(c,b,a,-n,mt));

            leaves.add(std::move(triangle1));
            leaves.add(std::move(triangle2));
        }
        return leaves;
    } else {
        SurfaceList surfaces;

        // Recurse on children whose bounding boxes intersect ray
        auto left_node  = kd_tree[node.leftChildId];
        auto right_node = kd_tree[node.rightChildId];
        if (left_node.boundingBox.intersects(ray)) {
            auto left_triangles = kd_intersect(ray, kd_tree, node.leftChildId, mt);
            surfaces.add(left_triangles);
        }
        if (right_node.boundingBox.intersects(ray)) {
            auto right_triangles = kd_intersect(ray, kd_tree, node.rightChildId, mt);
            surfaces.add(right_triangles);
        }

        return surfaces;
    }
}


#include "filter.h"
#include "../../modules/math/constants.h"
#include "../../synthesis/synthesis.h"
#include <complex>
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

using namespace Eigen;

VectorXcf poly(const VectorXcf& z) {
    VectorXcf poly(z.size() + 1);
    poly.setOnes();

    poly(0) = 1.0;

    for (int j = 0; j < z.size(); ++j) {
        for (int i = j; i >= 0; --i) {
            poly(i + 1) -= z(j) * poly(i);
        }
    }

    return poly;
}

std::vector<std::complex<double>> poly(const std::vector<std::complex<double>>& z) {
    std::vector<std::complex<double>> poly(z.size() + 1, 1.0);

    poly[0] = 1.0;

    for (int j = 0; j < z.size(); ++j) {
        for (int i = j; i >= 0; --i) {
            poly[i + 1] -= z[j] * poly[i];
        }
    }

    return poly;
}

std::vector<std::array<double, 6>> Analysis::butterworthHighpass(int N, double fc, double fs)
{
    const double Wn = fc / (fs / 2.0);
    const double Wo = tanf(Wn * M_PI / 2.0);

    std::vector<std::complex<double>> p;

    // Step 1. Get Butterworth analog lowpass prototype.
    for (int i = 2 + N - 1; i <= 3 * N - 1; i += 2) {
        p.push_back(std::polar<double>(1, (M_PI * i) / (2.0 * N)));
    }

    // Step 2. Transform to high pass filter.
    std::complex<double> Sg = 1.0,
                        prodSp = 1.0,
                        prodSz = 1.0;

    std::vector<std::complex<double>> Sp(p.size()), Sz(p.size());

    for (int i = 0; i < p.size(); ++i) {
        Sg *= -p[i];
        Sp[i] = Wo / p[i];
        Sz[i] = 0.0;
        prodSp *= (1.0 - Sp[i]);
        prodSz *= (1.0 - Sz[i]);
    }
    Sg = 1.0 / Sg;

    // Step 3. Transform to digital filter.
    std::vector<std::complex<double>> P(Sp.size()), Z(Sp.size());

    double G = std::real(Sg * prodSz / prodSp);

    for (int i = 0; i < Sp.size(); ++i) {
        P[i] = (1.0 + Sp[i]) / (1.0 - Sp[i]);
        Z[i] = (1.0 + Sz[i]) / (1.0 - Sz[i]);
    }

    // Step 6. Convert to SOS.

    return zpk2sos(Z, P, G);
}

std::vector<std::array<double, 6>> Analysis::butterworthLowpass(int N, double fc, double fs)
{
    const double Wn = fc / (fs / 2.0);
    const double Wo = tanf(Wn * M_PI / 2.0);

    std::vector<std::complex<double>> p;

    // Step 1. Get Butterworth analog lowpass prototype.
    for (int i = 2 + N - 1; i <= 3 * N - 1; i += 2) {
        p.push_back(std::polar<double>(1, (M_PI * i) / (2.0 * N)));
    }

    // Step 2. Transform to low pass filter.
    std::complex<double> Sg = 1.0,
                        prodSp = 1.0;

    std::vector<std::complex<double>> Sp(p.size()), Sz(0);

    for (int i = 0; i < p.size(); ++i) {
        Sg *= Wo;
        Sp[i] = Wo * p[i];
        prodSp *= (1.0 - Sp[i]);
    }

    // Step 3. Transform to digital filter.
    std::vector<std::complex<double>> P(Sp.size()), Z(Sp.size(), -1);

    double G = std::real(Sg / prodSp);

    for (int i = 0; i < Sp.size(); ++i) {
        P[i] = (1.0 + Sp[i]) / (1.0 - Sp[i]);
    }

    // Step 6. Convert to SOS.

    return zpk2sos(Z, P, G);
}


//
//  Copyright (c) 2010 Athanasios Iliopoulos
//
//  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/numeric/ublas/assignment.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector_sparse.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>

using namespace boost::numeric::ublas;

int main() {
        // Simple vector fill
    vector<double> a(3);
    a <<= 0, 1, 2;
    std::cout << a << std::endl;
    // [ 0 1 2]

    // Vector from vector
    vector<double> b(7);
    b <<= a, 10, a;
    std::cout << b << std::endl;
    // [ 0 1 2 10 0 1 2]

    // Simple matrix fill
    matrix<double> A(3,3);
    A <<= 0, 1, 2,
         3, 4, 5,
         6, 7, 8;
    std::cout << A << std::endl;
    // [ 0 1 2 ]
    // [ 3 4 5 ]
    // [ 6 7 8 ]

    // Matrix from vector
    A <<= 0, 1, 2,
         3, 4, 5,
         a;
    std::cout << A << std::endl;
    // [ 0 1 2 ]
    // [ 3 4 5 ]
    // [ 0 1 2 ]

    // Matrix from vector - column assignment
    A <<= move(0,2), traverse_policy::by_column(),
         a;
    std::cout << A << std::endl;
    // [ 0 1 0 ]
    // [ 3 4 1 ]
    // [ 0 1 2 ]

    // Another matrix from vector example (watch the wraping);
    vector<double> c(9); c <<= 1, 2, 3, 4, 5, 6, 7, 8, 9;
    A <<= c;
    std::cout << A << std::endl;
    // [ 1 2 3 ]
    // [ 4 5 6 ]
    // [ 7 8 9 ]

    // If for performance(Benchmarks are not definite about that) or consistency reasons you need to disable wraping:
    static next_row_manip endr; //This can be defined globally
    A <<= traverse_policy::by_row_no_wrap(),
            1, 2, 3, endr,
            4, 5, 6, endr,
            7, 8, 9, endr;
    // [ 1 2 3 ]
    // [ 4 5 6 ]
    // [ 7 8 9 ]
    // If by default you need to disable wraping define
    // BOOST_UBLAS_DEFAULT_NO_WRAP_POLICY, in the compilation options,
    // so that you avoid typing the "traverse_policy::by_row_no_wrap()".

    //  Plus and minus assign:
    A <<= fill_policy::index_plus_assign(),
         3,2,1;
    std::cout << A << std::endl;
    // [ 4 4 4 ]
    // [ 4 5 6 ]
    // [ 7 8 9 ]

    // Matrix from proxy
    A <<= 0, 1, 2,
         project(b, range(3,6)),
         a;
    std::cout << A << std::endl;
    // [ 0 1 2 ]
    // [10 0 1 ]
    // [ 6 7 8 ]

    // Matrix from matrix
    matrix<double> B(6,6);
    B <<= A, A,
         A, A;
    std::cout << B << std::endl;
    // [ A A ]
    // [ A A ]

    // Matrix range (vector is similar)
    B = zero_matrix<double>(6,6);
    matrix_range<matrix<double> > mrB (B, range (1, 4), range (1, 4));
    mrB <<= 1,2,3,4,5,6,7,8,9;
    std::cout << B << std::endl;
    // [ 0 0 0 0 0 0]
    // [ 0 1 2 3 0 0]
    // [ 0 4 5 6 0 0]
    // [ 0 0 0 0 0 0]
    // [ 0 0 0 0 0 0]
    // [ 0 0 0 0 0 0]

    // Horizontal concatenation can be achieved using this trick:
    matrix<double> BH(3,9);
    BH <<= A, A, A;
    std::cout << BH << std::endl;
    // [ A A A]

    // Vertical concatenation can be achieved using this trick:
    matrix<double> BV(9,3);
    BV <<= A,
          A,
          A;
    std::cout << BV << std::endl;
    // [ A ]
    // [ A ]
    // [ A ]

    // Watch the difference when assigning matrices for different traverse policies:
    matrix<double> BR(9,9, 0);
    BR <<= traverse_policy::by_row(), // This is the default, so this might as well be omitted.
          A, A, A;
    std::cout << BR << std::endl;
    // [ A A A]
    // [ 0 0 0]
    // [ 0 0 0]

    matrix<double> BC(9,9, 0);
    BC <<= traverse_policy::by_column(),
          A, A, A;
    std::cout << BC << std::endl;
    // [ A 0 0]
    // [ A 0 0]
    // [ A 0 0]

    // The following will throw a run-time exception in debug mode (matrix mid-assignment wrap is not allowed) :
    // matrix<double> C(7,7);
    // C <<= A, A, A;

    // Matrix from matrix with index manipulators
    matrix<double> C(6,6,0);
    C <<= A, move(3,0), A;
    // [ A 0 ]
    // [ 0 A ]

    // A faster way for to construct this dense matrix.
    matrix<double> D(6,6);
    D <<= A, zero_matrix<double>(3,3),
         zero_matrix<double>(3,3), A;
    // [ A 0 ]
    // [ 0 A ]

    // The next_row and next_column index manipulators:
    // note: next_row and next_column functions return
    // a next_row_manip and and next_column_manip object.
    // This is the manipulator we used earlier when we disabled
    // wrapping.
    matrix<double> E(2,4,0);
    E <<= 1, 2, next_row(),
         3, 4, next_column(),5;
    std::cout << E << std::endl;
    // [ 1 2 0 5 ]
    // [ 3 4 0 0 ]

    // The begin1 (moves to the begining of the column) index manipulator, begin2 does the same for the row:
    matrix<double> F(2,4,0);
    F <<= 1, 2, next_row(),
         3, 4, begin1(),5;
    std::cout << F << std::endl;
    // [ 1 2 5 0 ]
    // [ 3 4 0 0 ]

    // The move (relative) and move_to(absolute) index manipulators (probably the most useful manipulators):
    matrix<double> G(2,4,0);
    G <<= 1, 2, move(0,1), 3,
         move_to(1,3), 4;
    std::cout << G << std::endl;
    // [ 1 2 0 3 ]
    // [ 0 0 0 4 ]

    // Static equivallents (faster) when sizes are known at compile time:
    matrix<double> Gs(2,4,0);
    Gs <<= 1, 2, move<0,1>(), 3,
         move_to<1,3>(), 4;
    std::cout << Gs << std::endl;
    // [ 1 2 0 3 ]
    // [ 0 0 0 4 ]

    // Choice of traverse policy (default is "row by row" traverse):

    matrix<double> H(2,4,0);
    H <<= 1, 2, 3, 4,
         5, 6, 7, 8;
    std::cout << H << std::endl;
    // [ 1 2 3 4 ]
    // [ 5 6 7 8 ]

    H <<= traverse_policy::by_column(),
        1, 2, 3, 4,
        5, 6, 7, 8;
    std::cout << H << std::endl;
    // [ 1 3 5 7 ]
    // [ 2 4 6 8 ]

    // traverse policy can be changed mid assignment if desired.
     matrix<double> H1(4,4,0);
     H1 <<= 1, 2, 3, traverse_policy::by_column(), 1, 2, 3;

    std::cout << H << std::endl;
    // [1 2 3 1]
    // [0 0 0 2]
    // [0 0 0 3]
    // [0 0 0 0]

    // note: fill_policy and traverse_policy are namespaces, so you can use them
    // by a using statement.

    // For compressed and coordinate matrix types a push_back or insert fill policy can be chosen for faster assginment:
    compressed_matrix<double> I(2, 2);
    I <<=    fill_policy::sparse_push_back(),
            0, 1, 2, 3;
    std::cout << I << std::endl;
    // [ 0 1 ]
    // [ 2 3 ]

    coordinate_matrix<double> J(2,2);
    J<<=fill_policy::sparse_insert(),
        1, 2, 3, 4;
    std::cout << J << std::endl;
    // [ 1 2 ]
    // [ 3 4 ]

    // A sparse matrix from another matrix works as with other types.
    coordinate_matrix<double> K(3,3);
    K<<=fill_policy::sparse_insert(),
        J;
    std::cout << K << std::endl;
    // [ 1 2 0 ]
    // [ 3 4 0 ]
    // [ 0 0 0 ]

    // Be careful this will not work:
    //compressed_matrix<double> J2(4,4);
    //J2<<=fill_policy::sparse_push_back(),
     //   J,J;
    // That's because the second J2's elements
    // are attempted to be assigned at positions
    // that come before the elements already pushed.
    // Unfortunatelly that's the only thing you can do in this case
    // (or of course make a custom agorithm):
    compressed_matrix<double> J2(4,4);
    J2<<=fill_policy::sparse_push_back(),
        J, fill_policy::sparse_insert(),
        J;

    std::cout << J2 << std::endl;
    // [  J   J  ]
    // [ 0 0 0 0 ]
    // [ 0 0 0 0 ]

    // A different traverse policy doesn't change the result, only they order it is been assigned.
    coordinate_matrix<double> L(3,3);
    L<<=fill_policy::sparse_insert(), traverse_policy::by_column(),
        J;
    std::cout << L << std::endl;
    // (same as previous)
    // [ 1 2 0 ]
    // [ 3 4 0 ]
    // [ 0 0 0 ]

    typedef coordinate_matrix<double>::size_type cmst;
    const cmst size = 30;
    //typedef fill_policy::sparse_push_back spb;
    // Although the above could have been used the following is may be faster if
    //  you use the policy often and for relatively small containers.
    static fill_policy::sparse_push_back spb;

    // A block diagonal sparse using a loop:
    compressed_matrix<double> M(size, size, 4*15);
    for (cmst i=0; i!=size; i+=J.size1())
        M <<= spb, move_to(i,i), J;

    // If typedef was used above the last expression should start
    // with M <<= spb()...

    // Displaying so that blocks can be easily seen:
    for (unsigned int i=0; i!=M.size1(); i++) {
        std::cout << M(i,0);
        for (unsigned int j=1; j!=M.size2(); j++) std::cout << ", " << M(i,j);
        std::cout << "\n";
    }
    // [ J 0 0 0 ... 0]
    // [ 0 J 0 0 ... 0]
    // [ 0 . . . ... 0]
    // [ 0 0 ... 0 0 J]

    // A "repeat" trasverser may by provided so that this becomes faster and an on-liner like:
    // M <<= spb, repeat(0, size, J.size1(), 0, size, J.size1()), J;
    // An alternate would be to create a :repeater" matrix and vector expression that can be used in other places as well. The latter is probably better,
    return 0;
}


#include <iostream>
#include <algorithm>
#include <stdio.h>
#include <math.h>
#include <vector>
#include <numeric>
#include <strings.h>
#include <assert.h>

#include <dirent.h>

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

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <pybind11/stl.h>
#include <fstream>
#include <sstream>
BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)

typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > Polygon;

namespace py = pybind11;
using namespace std;

struct box{
  float ry;
  float l;
  float w;
  float h;
  float x;
  float z;
  float y;
  int cls_num;
  float is_obj;
  float wx0;
  float wx1;
  float wx2;
  float wx3;
  float wy0;
  float wy1;
  float wy2;
  float wy3;

};

void compute_4_points(vector<box> &boxes)
{
  for(int i=0;i<boxes.size();i++)
  {
    float ry = boxes[i].ry;
    float x = boxes[i].x;
    float z = boxes[i].z;
    float l = boxes[i].l;
    float w = boxes[i].w;
    using namespace boost::numeric::ublas;
    using namespace boost::geometry;
    matrix<double> mref(2, 2);
    mref(0, 0) = cos(ry); mref(0, 1) = sin(ry);
    mref(1, 0) = -sin(ry); mref(1, 1) = cos(ry);

    matrix<double> corners(2, 4);
    double data[] = {l / 2, l / 2, -l / 2, -l / 2,
                     w / 2, -w / 2, -w / 2, w / 2};
    std::copy(data, data + 8, corners.data().begin());
    matrix<double> gc = prod(mref, corners);
    for (int i = 0; i < 4; ++i) {
        gc(0, i) += x;
        gc(1, i) += z;
    }
    boxes[i].wx0 = gc(0, 0); boxes[i].wy0=gc(1, 0);
    boxes[i].wx1 = gc(0, 1); boxes[i].wy1=gc(1, 1);
    boxes[i].wx2 = gc(0, 2); boxes[i].wy2=gc(1, 2);
    boxes[i].wx3 = gc(0, 3); boxes[i].wy3=gc(1, 3);
  }

}

Polygon toPolygon_py(double ry,double l,double w,double x,double z)
{
    using namespace boost::numeric::ublas;
    using namespace boost::geometry;
    matrix<double> mref(2, 2);
    mref(0, 0) = cos(ry); mref(0, 1) = sin(ry);
    mref(1, 0) = -sin(ry); mref(1, 1) = cos(ry);

    static int count = 0;
    matrix<double> corners(2, 4);
    double data[] = {l / 2, l / 2, -l / 2, -l / 2,
                     w / 2, -w / 2, -w / 2, w / 2};
    std::copy(data, data + 8, corners.data().begin());
    matrix<double> gc = prod(mref, corners);
    for (int i = 0; i < 4; ++i) {
        gc(0, i) += x;
        gc(1, i) += z;
    }

    double points[][2] = {{gc(0, 0), gc(1, 0)},{gc(0, 1), gc(1, 1)},{gc(0, 2), gc(1, 2)},{gc(0, 3), gc(1, 3)},{gc(0, 0), gc(1, 0)}};
    Polygon poly;
    append(poly, points);
    return poly;
}
double groundBoxOverlap_py(double ry1,double l1,double w1,double x1,double z1,\
  double ry2,double l2,double w2,double x2,double z2, int criterion = -1)
{
  using namespace boost::geometry;
  Polygon gp = toPolygon_py(ry1,l1,w1,x1,z1);
  Polygon dp = toPolygon_py(ry2,l2,w2,x2,z2);

  std::vector<Polygon> in, un;
  intersection(gp, dp, in);
  union_(gp, dp, un);

  double inter_area = in.empty() ? 0 : area(in.front());
  double union_area = area(un.front());
  double o;
  if(criterion==-1)     // union
      o = inter_area / union_area;
  else if(criterion==0) // bbox_a
      o = inter_area / area(dp);
  else if(criterion==1) // bbox_b
      o = inter_area / area(gp);

  return o;
}

Polygon toPolygon(box a)
{
    using namespace boost::numeric::ublas;
    using namespace boost::geometry;
    matrix<double> mref(2, 2);
    mref(0, 0) = cos(a.ry); mref(0, 1) = sin(a.ry);
    mref(1, 0) = -sin(a.ry); mref(1, 1) = cos(a.ry);

    static int count = 0;
    matrix<double> corners(2, 4);
    double data[] = {a.l / 2, a.l / 2, -a.l / 2, -a.l / 2,
                     a.w / 2, -a.w / 2, -a.w / 2, a.w / 2};
    std::copy(data, data + 8, corners.data().begin());
    matrix<double> gc = prod(mref, corners);
    for (int i = 0; i < 4; ++i) {
        gc(0, i) += a.x;
        gc(1, i) += a.z;
    }

    double points[][2] = {{gc(0, 0), gc(1, 0)},{gc(0, 1), gc(1, 1)},{gc(0, 2), gc(1, 2)},{gc(0, 3), gc(1, 3)},{gc(0, 0), gc(1, 0)}};
    Polygon poly;
    append(poly, points);
    return poly;
}

float compute_iou_ground(box a,box b, int criterion = -1)
{
  using namespace boost::geometry;
  Polygon gp = toPolygon(a);
  Polygon dp = toPolygon(b);

  std::vector<Polygon> in, un;
  intersection(gp, dp, in);
  union_(gp, dp, un);

  double inter_area = in.empty() ? 0 : area(in.front());
  double union_area = area(un.front());
  double o;
  if(criterion==-1)     // union
      o = inter_area / union_area;
  else if(criterion==0) // bbox_a
      o = inter_area / area(dp);
  else if(criterion==1) // bbox_b
      o = inter_area / area(gp);

  return o;
}

float compute_iou_rect(box rectA,box rectB)
{
  float xa1 = max(max(max(rectA.wx0,rectA.wx1),rectA.wx2),rectA.wx3);
  float xa0 = min(min(min(rectA.wx0,rectA.wx1),rectA.wx2),rectA.wx3);
  float ya1 = max(max(max(rectA.wy0,rectA.wy1),rectA.wy2),rectA.wy3);
  float ya0 = min(min(min(rectA.wy0,rectA.wy1),rectA.wy2),rectA.wy3);

  float xb1 = max(max(max(rectB.wx0,rectB.wx1),rectB.wx2),rectB.wx3);
  float xb0 = min(min(min(rectB.wx0,rectB.wx1),rectB.wx2),rectB.wx3);
  float yb1 = max(max(max(rectB.wy0,rectB.wy1),rectB.wy2),rectB.wy3);
  float yb0 = min(min(min(rectB.wy0,rectB.wy1),rectB.wy2),rectB.wy3);

  if (xa0 > xb1) { return 0.; }
	if (ya0 > yb1) { return 0.; }
	if ((xa1) < xb0) { return 0.; }
	if ((ya1) < yb0) { return 0.; }
	float colInt = min(xa1, xb1) - max(xa0, xb0);
	float rowInt = min(ya1, yb1) - max(ya0, yb0);
	float intersection = colInt * rowInt;
	float areaA = (xa1-xa0) * (ya1-ya0);
	float areaB = (xb1-xb0) * (yb1-yb0);
	float intersectionPercent = intersection / (areaA + areaB - intersection);
	return intersectionPercent;
}

float sigmoid(float x)
{
  float s = 1.0 / (1.0 + exp(-x));
  return s;
}

void nms2(
        const std::vector<box>& srcRects,
        const std::vector<float>& scores,
        std::vector<box>& resRects,
        float thresh,
        int neighbors = 0,
        float minScoresSum = 0.f
        )
{
    resRects.clear();

    const size_t size = srcRects.size();
    if (!size)
    {
        return;
    }

    assert(srcRects.size() == scores.size());

    std::multimap<float, size_t> idxs;
    for (size_t i = 0; i < size; ++i)
    {
        idxs.insert(std::pair<float, size_t>(scores[i], i));
    }

    while (idxs.size() > 0)
    {
        auto lastElem = --std::end(idxs);
        box rect1 = srcRects[lastElem->second];

        int neigborsCount = 0;
        float score = lastElem->first;
        float scoresSum = lastElem->first;

        idxs.erase(lastElem);

        for (auto pos = std::begin(idxs); pos != std::end(idxs); )
        {
            box rect2 = srcRects[pos->second];
            float distance2 = (rect1.x-rect2.x)*(rect1.x-rect2.x)\
              +(rect1.z-rect2.z)*(rect1.z-rect2.z);
            if(distance2>15*15)
            {
              ++pos;
              continue;
            }

            float overlap=0;
            if((abs(rect1.ry)<70.0*3.14158/180 && abs(rect1.ry)>20.0*3.14158/180) || \
              (abs(rect2.ry)<70.0*3.14158/180 && abs(rect2.ry)>20.0*3.14158/180))
            {
              overlap  = compute_iou_ground(rect1,rect2);
            }
            {
              overlap  = compute_iou_rect(rect1,rect2);
            }
            if (overlap > thresh)
            {
                scoresSum += pos->first;
                pos = idxs.erase(pos);
                ++neigborsCount;
            }
            else
            {
                ++pos;
            }
        }
        if (neigborsCount >= neighbors &&
                scoresSum >= minScoresSum)
        {
            resRects.push_back(rect1);
        }
    }
}

py::array_t<float> cal_result(py::array_t<float> &feature_out,\
  float obj_th,float OVERLAP,float Z_MIN, int img_height,int img_width,float DX,float DY,float DZ,\
  float nms_th)
{

  auto feature_map = feature_out.unchecked<3>();

  int feature_height = img_height/8+0.5;
  int feature_width = img_width/8+0.5;
  int grid_height = img_height/feature_height+0.5;
  int grid_width = img_width/feature_width+0.5;

  std::vector<box> objs;
  std::vector<float> scores;
  objs.clear();
  scores.clear();

  float cut_dis = OVERLAP-Z_MIN;

  for(int height_i=0;height_i<feature_height;height_i++)
  {
    for(int width_i=0;width_i<feature_width;width_i++)
    {
      float is_obj = sigmoid(feature_map(height_i,width_i,0));

      float reg_dy = feature_map(height_i,width_i,13);
      reg_dy = reg_dy*grid_height;
      float center_y = height_i*grid_height+reg_dy;
      float m_y = (center_y*DY);
      float reg_dx = feature_map(height_i,width_i,11);
      reg_dx = reg_dx*grid_width;
      float center_x = width_i*grid_width+reg_dx;
      float m_x = (center_x-img_width/2)*DX;
      float sin_theta = feature_map(height_i,width_i,7);
      float cos_theta = feature_map(height_i,width_i,9);
      float theta = atan2(sin_theta,cos_theta)/2;
      float reg_ln_l = feature_map(height_i,width_i,17);
      float reg_l = exp(reg_ln_l);
      float reg_ln_h = feature_map(height_i,width_i,21);
      float reg_h = exp(reg_ln_h);
      if(m_y>cut_dis)
      {
        m_y=m_y-cut_dis-OVERLAP;
        if(m_y-reg_l/2<-10)
          is_obj*=0.2;
      }
      else
      {
        m_x*=-1;
        m_y=-1*(m_y-OVERLAP);
        if(m_y+reg_l/2>10)
          is_obj*=0.2;
      }

      if(m_y>100 && abs(theta)<45*3.14158/180)
      {
        is_obj*=0.2;
      }

      float m_obj_th = obj_th;
      if(m_y>100)
        m_obj_th=obj_th*0.5;
      if(m_y>150)
        m_obj_th=obj_th*0.4;
      if(m_y>180)
        m_obj_th=obj_th*0.3;

      if(is_obj>m_obj_th)
      {
        int cls_num=0;
        float is_cls0=feature_map(height_i,width_i,2);
        float is_cls1=feature_map(height_i,width_i,3);
        float is_cls2=feature_map(height_i,width_i,4);
        float reg_ln_w = feature_map(height_i,width_i,15);
        float reg_w = exp(reg_ln_w);
        float m_z = feature_map(height_i,width_i,19);
        if(is_cls0>is_cls1 && is_cls0>is_cls2)
        {
          cls_num = 0;
        }
        else if(is_cls1>is_cls0 && is_cls1>is_cls2)
        {
          cls_num = 1;
          if (is_obj<0.88 && m_y<100 && abs(m_x)<40 && abs(m_x)>5 && m_y>10)
            continue;
        }
        else
        {
          cls_num = 2;
          if (is_obj<0.88 && m_y<100 && abs(m_x)<40 && abs(m_x)>5 && m_y>10)
            continue;
        }
        box one_obj;
        one_obj.ry = theta;
        one_obj.l = reg_l;
        one_obj.w = reg_w;
        one_obj.x = m_x;
        one_obj.z = m_y;
        one_obj.cls_num=cls_num;
        one_obj.is_obj=is_obj;
        one_obj.h = reg_h;
        one_obj.y = m_z;
        objs.push_back(one_obj);

        scores.push_back(is_obj);

      }

      is_obj= sigmoid(feature_map(height_i,width_i,1));
      if(is_obj>obj_th)
      {
        int cls_num=0;
        float is_cls3=feature_map(height_i,width_i,5);
        float is_cls4=feature_map(height_i,width_i,6);
        if(is_cls3>is_cls4)
        {
          cls_num = 3;
        }
        else
        {
          cls_num = 4;
        }

        float sin_theta = feature_map(height_i,width_i,8);
        float cos_theta = feature_map(height_i,width_i,10);
        float reg_dx = feature_map(height_i,width_i,12);
        float reg_dy = feature_map(height_i,width_i,14);
        float reg_ln_w = feature_map(height_i,width_i,16);
        float reg_ln_l = feature_map(height_i,width_i,18);
        float m_z = feature_map(height_i,width_i,20);
        float reg_ln_h = feature_map(height_i,width_i,22);
        float theta = atan2(sin_theta,cos_theta)/2;
        reg_dx = reg_dx*grid_width;
        reg_dy = reg_dy*grid_height;
        float center_x = width_i*grid_width+reg_dx;
        float center_y = height_i*grid_height+reg_dy;
        float m_x = (center_x-img_width/2)*DX;
        float m_y = (center_y*DY);
        float reg_w = exp(reg_ln_w);
        float reg_l = exp(reg_ln_l);
        float reg_h = exp(reg_ln_h);

        if(m_y>cut_dis)
        {
          m_y=m_y-cut_dis-OVERLAP;
        }
        else
        {
          m_x*=-1;
          m_y=-1*(m_y-OVERLAP);
        }

        box one_obj;
        one_obj.ry = theta;
        one_obj.l = reg_l;
        one_obj.w = reg_w;
        one_obj.x = m_x;
        one_obj.z = m_y;
        one_obj.cls_num=cls_num;
        one_obj.is_obj=is_obj;
        one_obj.h = reg_h;
        one_obj.y = m_z;
        objs.push_back(one_obj);

        scores.push_back(is_obj);

      }
    }
  }

  std::vector<box> results;
  compute_4_points(objs);
  nms2(objs,scores,results,nms_th);

  int obj_num = results.size();

  auto result = py::array_t<float>(obj_num*9);
  result.resize({obj_num,9});
  py::buffer_info buf_result = result.request();
  float* ptr_result = (float*)buf_result.ptr;

  for(int i=0;i<obj_num;i++)
  {
    ptr_result[i*9 + 0] = results[i].is_obj;
    ptr_result[i*9 + 1] = results[i].cls_num;
    ptr_result[i*9 + 2] = results[i].ry;
    ptr_result[i*9 + 3] = results[i].l;
    ptr_result[i*9 + 4] = results[i].w;
    ptr_result[i*9 + 5] = results[i].x;
    ptr_result[i*9 + 6] = results[i].z;
    ptr_result[i*9 + 7] = results[i].h;
    ptr_result[i*9 + 8] = results[i].y;
  }

  return result;
}

PYBIND11_MODULE(lib_cpp, m)
{
    m.def("cal_result", &cal_result);
}

int32_t main() {

  return 0;
}