#include <bits/stdc++.h>
#include <iostream>
#include <limits>

#include <open3d/geometry/BoundingVolume.h>
#include <open3d/geometry/KDTreeFlann.h>
#include <primitives_fitting/PrimitivesFitting.h>
#include <primitives_fitting/Segmentation.h>
#include <primitives_fitting/Utils.h>
#include <Eigen/Eigenvalues>

namespace primitives_fitting {

std::tuple<Eigen::Matrix3d, Eigen::Vector3d> ComputeCovarianceMatrixAndMean(
    const Eigen::MatrixX3d &points) {
    const Eigen::Vector3d mean = points.colwise().mean().transpose();
    const Eigen::Matrix3Xd points_ = (points.rowwise() - mean.transpose()).transpose();
    return std::make_tuple((points_ * points_.transpose()) / double(points.rows()), mean);
}

PrimitivesDetectorConfig::PrimitivesDetectorConfig() {
    m_preprocess_param = {0.002, false};
    m_fitting_param = {PrimitivesType::plane, 0.005};
    m_cluster_param = {4, std::numeric_limits<size_t>::max(), 0.01, 15.0};
    m_filtering_param = {0.01, 0.1};
}

PrimitivesDetectorConfig::~PrimitivesDetectorConfig() = default;

PrimitivesDetector::PrimitivesDetector() : m_config(PrimitivesDetectorConfig()) {}

PrimitivesDetector::~PrimitivesDetector() = default;

void PrimitivesDetector::CheckConfiguration() {
    // check point cloud down sample voxel size and clustering point cloud distance
    if (m_config.m_preprocess_param.voxel_size > m_config.m_cluster_param.dist_thresh) {
        std::cout << "invalid param setting: the downsample voxel size must be smaller than "
                     "clustering distance threshold!"
                  << std::endl;
        std::cout
            << "automatically adjust voxel size value into half of clustering distance threshold"
            << std::endl;
        m_config.m_preprocess_param.voxel_size = m_config.m_cluster_param.dist_thresh / 2;
    }

    // check primitives filtering params
    if (m_config.m_filtering_param.min_bound > m_config.m_filtering_param.max_bound) {
        std::cout << "invalid param setting: the min bound of filtering param for sphere is larger "
                     "than max bound!"
                  << std::endl;
        std::cout << "automatically exchange value" << std::endl;
        std::swap(m_config.m_filtering_param.min_bound, m_config.m_filtering_param.max_bound);
    }
}

PrimitivesDetector::PrimitivesDetector(const PrimitivesDetectorConfig &config) {
    m_config = config;
    CheckConfiguration();
}

void PrimitivesDetector::SetConfiguration(const PrimitivesDetectorConfig &config) {
    m_config = config;
    CheckConfiguration();
}

std::tuple<std::shared_ptr<open3d::geometry::PointCloud>, std::vector<std::vector<size_t>>>
PrimitivesDetector::PreProcessPointClouds(const open3d::geometry::PointCloud &points) {
    // voxel downsample pointcloud
    auto o3d_pts_down = points.VoxelDownSample(m_config.m_preprocess_param.voxel_size);

    // set neighbours search radius equal to half of clustering distance threshold
    const double search_radius = m_config.m_cluster_param.dist_thresh / 2;

    std::vector<std::vector<size_t>> neighbours = FindNeighbourHood(*o3d_pts_down, search_radius);

    if (m_config.m_preprocess_param.enable_smoothing) {
        SmoothPointClouds(neighbours, *o3d_pts_down, search_radius);
    }

    if (!o3d_pts_down->HasNormals()) {
        o3d_pts_down->EstimateNormals(open3d::geometry::KDTreeSearchParamKNN(NORMAL_SEARCH_RADIUS));
    }

    o3d_pts_down->NormalizeNormals();
    utils::NormalConsistent(*o3d_pts_down);

    return std::make_tuple(o3d_pts_down, neighbours);
}

std::vector<std::vector<size_t>> PrimitivesDetector::FindNeighbourHood(
    const open3d::geometry::PointCloud &pc, double radius) {
    open3d::geometry::KDTreeFlann kdtree(pc);
    const size_t num = pc.points_.size();
    std::vector<std::vector<size_t>> neighbours(num);

#pragma omp parallel for
    for (size_t i = 0; i < num; ++i) {
        std::vector<int> ret_indices;
        std::vector<double> out_dists_sqr;
        const int nn = kdtree.SearchRadius(pc.points_[i], radius, ret_indices, out_dists_sqr);
        std::vector<size_t> ret(ret_indices.begin(), ret_indices.end());
        neighbours[i] = ret;
    }

    return neighbours;
}

void PrimitivesDetector::SmoothPointClouds(std::vector<std::vector<size_t>> &neighbours,
                                           open3d::geometry::PointCloud &pc, double tolerance) {
    const size_t num = pc.points_.size();

#pragma omp parallel for shared(neighbours, pc)
    for (size_t i = 0; i < num; ++i) {
        Eigen::Matrix3Xd cluster_raw;
        utils::GetMatrixByIndex(pc.points_, neighbours[i], cluster_raw);
        const Eigen::Matrix3Xd cluster_smooth = ZAxisRegression(cluster_raw);

        if ((pc.points_[i] - cluster_smooth.col(0)).norm() < tolerance) {
            pc.points_[i] = cluster_smooth.col(0);
        } else {
            pc.points_[i] = Eigen::Vector3d(0, 0, 0);
        }
    }
}

Eigen::Matrix3Xd PrimitivesDetector::ZAxisRegression(const Eigen::Matrix3Xd &points) {
    const int num = points.cols();

    Eigen::VectorXd target = points.row(2);
    Eigen::Matrix3Xd data = Eigen::Matrix3Xd::Ones(3, num);

    data.row(0) = points.row(0);
    data.row(1) = points.row(1);

    Eigen::Vector3d weight = (data * data.transpose()).inverse() * data * target;

    Eigen::VectorXd new_target = data.transpose() * weight;
    data.row(2) = new_target.transpose();

    return data;
}

std::vector<Eigen::Matrix4d> PrimitivesDetector::FilterClusterAndCalcPose(
    const Clusters &clusters) {
    switch (m_config.m_fitting_param.type) {
    case PrimitivesType::plane:
        return FilterClusterAndCalcPoseUsingPlaneModel(clusters);

    case PrimitivesType::sphere:
        return FilterClusterAndCalcPoseUsingSphereModel(clusters);

    case PrimitivesType::cylinder:
        return FilterClusterAndCalcPoseUsingCylinderModel(clusters);

    default:
        std::cout << "use wrong primitives type, use plane instead" << std::endl;
        return FilterClusterAndCalcPoseUsingPlaneModel(clusters);
    }
}

std::vector<Eigen::Matrix4d> PrimitivesDetector::FilterClusterAndCalcPoseUsingPlaneModel(
    const Clusters &clusters) {
    std::vector<Eigen::Matrix4d> pose_out;

    const double min_bound = m_config.m_filtering_param.min_bound;
    const double max_bound = m_config.m_filtering_param.max_bound;
#pragma omp parallel for shared(clusters)
    for (size_t i = 0; i < clusters.size(); ++i) {
        ransac::RANSACPlane plane_finder;
        ransac::Plane plane_param;
        std::vector<size_t> indices;
        plane_finder.SetParallel(m_config.m_fitting_param.enable_parallel);
        plane_finder.SetMaxIteration(m_config.m_fitting_param.max_iteration);
        plane_finder.SetPointCloud(clusters[i].points_);
        const bool ret = plane_finder.FitModel(m_config.m_fitting_param.threshold,
                                               plane_param, indices);
        if (!ret) {
            continue;
        }

        std::vector<Eigen::Vector3d> inliers;
        utils::GetVectorByIndex(clusters[i].points_, indices, inliers);

        const open3d::geometry::PointCloud o3d(inliers);
        const open3d::geometry::OrientedBoundingBox bbox = o3d.GetOrientedBoundingBox();
        auto extent = bbox.extent_;
#pragma omp critical
        {
            if (extent(0) > min_bound && extent(0) < max_bound && extent(1) > min_bound &&
                extent(1) < max_bound) {
                m_clusters.emplace_back(inliers);

                Eigen::MatrixX3d cluster_mat;
                utils::VectorToEigenMatrix(inliers, cluster_mat);
                auto result = ComputeCovarianceMatrixAndMean(cluster_mat);
                Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(std::get<0>(result));
                Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
                pose.block<3, 1>(0, 3) = std::get<1>(result);
                pose.block<3, 1>(0, 0) = solver.eigenvectors().col(2);
                pose.block<3, 1>(0, 1) = solver.eigenvectors().col(1);
                pose.block<3, 1>(0, 2) =
                    solver.eigenvectors().col(2).cross(solver.eigenvectors().col(1));

                // orient normal direction to origin coordinate
                if (pose(2, 2) > 0) {
                    pose.block<3, 1>(0, 2) *= -1;
                    pose.block<3, 1>(0, 0) *= -1;
                }

                pose_out.emplace_back(pose);
                m_primitives.emplace_back(plane_param);
            }
        }
    }
    return pose_out;
}

std::vector<Eigen::Matrix4d> PrimitivesDetector::FilterClusterAndCalcPoseUsingSphereModel(
    const Clusters &clusters) {
    std::vector<Eigen::Matrix4d> pose_out;

    const double min_bound = m_config.m_filtering_param.min_bound;
    const double max_bound = m_config.m_filtering_param.max_bound;
#pragma omp parallel for shared(clusters)
    for (size_t i = 0; i < clusters.size(); ++i) {
        ransac::RANSACShpere sphere_finder;
        ransac::Sphere sphere_param;
        std::vector<size_t> indices;
        bool ret;
        // sphere fitter has error in FitModel function when in some special case, use exception to
        // prevent from broken
        try {
            sphere_finder.SetPointCloud(clusters[i].points_);
            sphere_finder.SetParallel(m_config.m_fitting_param.enable_parallel);
            sphere_finder.SetMaxIteration(m_config.m_fitting_param.max_iteration);
            ret = sphere_finder.FitModel(m_config.m_fitting_param.threshold,
                                         sphere_param, indices);
        } catch (const std::exception &e) {
            std::cerr << e.what() << '\n';
            ret = false;
        }

        if (!ret) {
            continue;
        }

        std::vector<Eigen::Vector3d> inliers;
        utils::GetVectorByIndex(clusters[i].points_, indices, inliers);
#pragma omp critical
        {
            if (sphere_param.m_parameters(3) > min_bound &&
                sphere_param.m_parameters(3) < max_bound) {
                m_clusters.emplace_back(inliers);

                Eigen::Vector3d direction =
                    Eigen::Vector3d(0, 0, 0) - sphere_param.m_parameters.head<3>();
                direction /= direction.norm();
                const Eigen::Vector3d translation =
                    sphere_param.m_parameters.head<3>() + direction * sphere_param.m_parameters(3);

                Eigen::Vector3d y_axis = direction.cross(Eigen::Vector3d(1, 0, 0));
                Eigen::Vector3d x_axis = y_axis.cross(direction);
                y_axis = direction.cross(x_axis);

                Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
                pose.block<3, 1>(0, 3) = translation;
                pose.block<3, 1>(0, 0) = x_axis;
                pose.block<3, 1>(0, 1) = y_axis;
                pose.block<3, 1>(0, 2) = direction;
                pose_out.emplace_back(pose);
                m_primitives.emplace_back(sphere_param);
            }
        }
    }
    return pose_out;
}

std::vector<Eigen::Matrix4d> PrimitivesDetector::FilterClusterAndCalcPoseUsingCylinderModel(
    const Clusters &clusters) {
    std::vector<Eigen::Matrix4d> pose_out;

    const double min_bound = m_config.m_filtering_param.min_bound;
    const double max_bound = m_config.m_filtering_param.max_bound;
#pragma omp parallel for shared(clusters)
    for (size_t i = 0; i < clusters.size(); ++i) {
        ransac::RANSACCylinder cylinder_finder;
        ransac::Cylinder cylinder_param;
        std::vector<size_t> indices;
        bool ret;
        try {
            cylinder_finder.SetParallel(m_config.m_fitting_param.enable_parallel);
            cylinder_finder.SetMaxIteration(m_config.m_fitting_param.max_iteration);
            cylinder_finder.SetPointCloud(clusters[i].points_);
            cylinder_finder.SetNormals(clusters[i].normals_);
            ret = cylinder_finder.FitModel(m_config.m_fitting_param.threshold,
                                           cylinder_param, indices);
        } catch (const std::exception &e) {
            std::cerr << e.what() << '\n';
            ret = false;
        }

        if (!ret) {
            continue;
        }

        std::vector<Eigen::Vector3d> inliers;
        utils::GetVectorByIndex(clusters[i].points_, indices, inliers);
#pragma omp critical
        {
            if (cylinder_param.m_parameters(6) > min_bound &&
                cylinder_param.m_parameters(6) < max_bound) {
                m_clusters.emplace_back(inliers);

                Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
                pose.block<3, 1>(0, 3) = cylinder_param.m_parameters.head<3>();
                const Eigen::Vector3d x_axis = cylinder_param.m_parameters.segment<3>(3);
                const Eigen::Vector3d z_axis = Eigen::Vector3d(0, 0, 0) - cylinder_param.m_parameters.head<3>();
                Eigen::Vector3d y_axis = z_axis.cross(x_axis);
                pose.block<3, 1>(0, 0) = x_axis;
                pose.block<3, 1>(0, 1) = y_axis;
                pose.block<3, 1>(0, 2) = z_axis;

                pose_out.emplace_back(pose);
                m_primitives.emplace_back(cylinder_param);
            }
        }
    }
    return pose_out;
}

bool PrimitivesDetector::Detect(const std::vector<Eigen::Vector3d> &points) {
    const open3d::geometry::PointCloud o3d_pc(points);
    return Detect(o3d_pc);
}

bool PrimitivesDetector::Detect(const std::vector<Eigen::Vector3d> &points,
                                const std::vector<Eigen::Vector3d> &normals) {
    open3d::geometry::PointCloud o3d_pc(points);
    o3d_pc.normals_ = normals;
    return Detect(o3d_pc);
}

bool PrimitivesDetector::Detect(const open3d::geometry::PointCloud &points) {
    const size_t points_num = points.points_.size();

    // clear previous results
    m_clusters.clear();
    m_primitives.clear();
    m_poses.clear();

    if (points_num == 0) {
        std::cout << "there is no input point clouds" << std::endl;
        return false;
    }

    auto processed_data = PreProcessPointClouds(points);
    auto pcd = std::get<0>(processed_data);
    auto neighbours = std::get<1>(processed_data);

    // init proximity extractor
    segmentation::ProximityExtractor extractor(m_config.m_cluster_param.min_cluster_size,
                                               m_config.m_cluster_param.max_cluster_size);
    segmentation::DistanceNormalsProximityEvaluator evaluator(
        pcd->normals_, m_config.m_cluster_param.dist_thresh, m_config.m_cluster_param.angle_thresh);

    std::vector<std::vector<size_t>> cluster_indices =
        extractor.Segment(pcd->points_, neighbours, evaluator);

    Clusters clusters(cluster_indices.size());
    for (size_t i = 0; i < cluster_indices.size(); ++i) {
        clusters[i] = *pcd->SelectByIndex(cluster_indices[i]);
    }

    m_poses = FilterClusterAndCalcPose(clusters);

    return true;
}

std::vector<std::vector<Eigen::Vector3d>> PrimitivesDetector::GetClusters() {
    std::vector<std::vector<Eigen::Vector3d>> clusters;
    for (auto &c : m_clusters) {
        clusters.push_back(c.points_);
    }

    return clusters;
}

std::vector<ransac::Model> PrimitivesDetector::GetPrimitives() {
    return m_primitives;
}

std::vector<Eigen::Matrix4d> PrimitivesDetector::GetPoses() {
    return m_poses;
}

}  // namespace primitives_fitting


// Copyright (c) 2018 Evan S Weinberg
// Thick restarted and deflated Lanczos.
// Points out where some type of polynomial
// acceleration could go in.
// Based on arXiv:1512:08135

#include <iostream>
#include <iomanip>
#include <cmath>
#include <string>
#include <sstream>
#include <complex>
#include <random>
#include <vector>

// Borrow dense matrix eigenvalue routines.
#include <Eigen/Dense>

#include "blas/generic_vector.h"

#include "square_laplace.h"

using namespace std;
using namespace Eigen;

typedef Matrix<double, Dynamic, Dynamic, ColMajor> dMatrix;
typedef Matrix<std::complex<double>, Dynamic, Dynamic, ColMajor> cMatrix;

int main(int argc, char** argv)
{
  complex<double> *rhs_cplx;

  // Set output precision to be long.
  cout << setprecision(10);

  // RNG related things.
  std::mt19937 generator (1337u); // RNG, 1337u is the seed.

  ///////////////////////////////////
  // Properties of Linear operator //
  ///////////////////////////////////
  double inv_variance = 6.0; // inverse of variance for gaussian non-compact U(1) links.

  // Basic information about the lattice.
  int length = 8;
  double m_sq = 0.001;

  // Some start-up.
  int volume = length*length;

  // Create a random compact U(1) link.
  complex<double>* gauge_links = allocate_vector<complex<double>>(2*length*length);
  gaussian_real(gauge_links, 2*length*length, generator, 1.0/inv_variance);
  polar(gauge_links, 2*length*length);

  // Vectors.
  rhs_cplx = allocate_vector<complex<double>>(volume);

  // Zero out the vector.
  zero_vector(rhs_cplx, length*length);

  // Structure which gets passed to the function.
  laplace_gauged_struct lapstr_gauged;
  lapstr_gauged.length = length;
  lapstr_gauged.m_sq = m_sq;
  lapstr_gauged.gauge_links = gauge_links;

  // Uncomment this to get the free field.
  //constant_vector(gauge_links, 1.0, 2*volume);

  //////////////////////////////////////
  // Properties for Lanczos algorithm //
  //////////////////////////////////////

  int max_iterations = 20; // maximum number of restarts
  int max_no_TR_candidates = 2; // maximum number of times we press on when there's
                                // no TR candidate
  int max_no_TM_eig_candidates = 2; // maximum number of times we press on when there's
                                    // no candidates in TM
  int m = 20; // Max subspace size before restart
  double tol = 1e-8; // Tolerance of eigenvalue computation

  // We're looking to grab all eigenvalues in the interval below
  // If we switched to a polynomial acceleration scheme, we'd look
  // for all eigenvalues above some value.
  // This is a choice! I'll make it clear below where to modify
  // this stopping condition.
  double eig_ceil = 2.0;
  double eig_floor = 1e-6; // Avoid deflated modes

  //////////////////////////////////////////
  // Prepare memory for Lanczos algorithm //
  //////////////////////////////////////////

  // Allocate Lanczos space. Extra vector needed for restarts.
  complex<double>** Q = new complex<double>*[m+1];
  for (int i = 0; i < m+1; i++) {
    Q[i] = allocate_vector<complex<double>>(volume);
  }

  // Extra beta for restarts
  double beta_restart = 0.0;

  // Storage for locked vectors!
  std::vector<complex<double>> lambda;
  std::vector<complex<double>*> U;

  // Intermediate vectors
  complex<double>* w = allocate_vector<complex<double>>(volume);
  complex<double>* lambda_x_u = allocate_vector<complex<double>>(volume);

  // Allocate T_m matrix (tridiagonal in un-restarted case).
  // Can generally be complex because of the restart
  cMatrix Tm = cMatrix::Zero(m,m);

  // Prepare an eigensolver
  SelfAdjointEigenSolver<cMatrix> eigsolve_Tm(m);

  ////////////////////////
  // Begin the Lanczos! //
  ////////////////////////

  // Prepare a random unit vector for Q[0]!
  gaussian(Q[0], volume, generator);
  normalize(Q[0], volume);

  int iters = 0;
  int l = 0;
  int locked = 0;
  int num_no_TR_candidates = 0;
  int num_no_TM_eig_candidates = 0;

  // Let's gooooo
  while (iters < max_iterations) {
    // last bit of thick restart. put s in correct place.
    if (l > 0) {
      for (int i = 0; i < l; i++) {
        Tm(l,i) = Tm(m-1,i);
        Tm(m-1,i) = 0.0;
        Tm(i,l) = conj(Tm(l,i));
      }
    }

    // If we're not on the first iter, T_m has values filled in.
    // Fill out rest of T_m. Standard lanczos!
    for (int i = l; i < m; i++) {
      zero_vector(w, volume);

      // PUT IN POLY ACCELERATION HERE
      square_laplacian_gauged(w, Q[i], &lapstr_gauged); // w = B q_i
      // deflate
      for (int j = 0; j < locked; j++) {
        complex<double> dotprod = dot(U[j], w, volume);
        caxpy(-dotprod, U[j], w, volume);
      }

      if (i > l) {
        caxpy(-Tm(i-1,i), Q[i-1], w, volume); // w -= beta_i q_{i-1}
      }

      // Compute alpha_i
      Tm(i,i) = re_dot(Q[i], w, volume);

      // w -= alpha_i q_i
      caxpy(-Tm(i,i), Q[i], w, volume);

      // Reorthogonalize
      for (int j = 0; j < i; j++) {
        orthogonal(w, Q[j], volume);
      }

      // Compute beta_{i+1}
      double beta = sqrt(norm2sq(w,volume));

      if (i == m-1) {
        beta_restart = beta;
      } else {
        Tm(i+1,i) = Tm(i,i+1) = beta;
      }

      // Check for breakdown
      if (beta < 1e-10) {
        gaussian(Q[i+1], volume, generator);
        normalize(Q[i+1], volume);
      } else {
        caxy(1.0/beta, w, Q[i+1], volume);
      }
    }

    iters++;

    // Calculate the Ritz values
    eigsolve_Tm.compute(Tm);
    std::cout << "The Ritz values are:\n" << eigsolve_Tm.eigenvalues().transpose() << "\n";
    std::cout << std::flush;

    // Compute candidate pairs---evals between the min and max.
    // There's lots of potential for memory reuse here...
    std::vector<complex<double>> theta_candidate;
    std::vector<complex<double>> s_candidate; // used for restart
    std::vector<complex<double>*> u_candidate;

    for (int i = 0; i < m; i++) {
      double eval = eigsolve_Tm.eigenvalues()(i);
      if (eval > eig_floor && eval < eig_ceil) {
        // Add it as a candidate pair
        theta_candidate.push_back(eval);
        s_candidate.push_back(beta_restart*eigsolve_Tm.eigenvectors().col(i)(m-1));

        // Build up the Ritz vector
        u_candidate.push_back(allocate_vector<complex<double>>(volume));
        zero_vector(u_candidate[u_candidate.size()-1], volume);
        for (int j = 0; j < m; j++) {
          caxpy(eigsolve_Tm.eigenvectors().col(i)(j), Q[j], u_candidate[u_candidate.size()-1], volume);
        }
      }
    }

    // Zero out all Q except the last one!
    for (int i = 0; i < m; i++) {
      zero_vector(Q[i], volume);
    }

    int num_candidate = theta_candidate.size();

    // if no candidates were found, quit.
    // Might be a sign we've found all the eigenvalues... or not.
    // Needs a smarter heuristic. Again, should be safer
    // with poly accel.
    if (num_candidate == 0) {

      num_no_TM_eig_candidates++;
      std::cout << "No TM eigenvalue candidates: " << num_no_TM_eig_candidates << ".\n";
      if (num_no_TM_eig_candidates == max_no_TM_eig_candidates) {
        std::cout << "Max number of times with no candidate TM eigenvalues reached.\n";
        break;
      } else {
        l = 0;
        Tm = cMatrix::Zero(m,m);
        copy_vector(Q[0], Q[m], volume);
        continue;
      }
    }

    // Reset l
    l = 0;

    // Zero out Tm, start rebuilding
    Tm = cMatrix::Zero(m,m);

    for (int i = 0; i < num_candidate; i++) {
      // Compute Rayleigh value. Technically wasteful
      // because we aren't doing poly accel.
      // Reuse w.
      zero_vector(w, volume);
      square_laplacian_gauged(w, u_candidate[i], &lapstr_gauged);

      double rayleigh = re_dot(u_candidate[i], w, volume);

      // Make sure we're in the target range
      if (rayleigh > eig_ceil || rayleigh < eig_floor) { continue; }

      // check convergence
      caxy(rayleigh, u_candidate[i], lambda_x_u, volume);
      if (sqrt(diffnorm2sq(w, lambda_x_u, volume))/rayleigh < tol) {
        // We've got a lock!
        lambda.push_back(rayleigh);
        U.push_back(allocate_vector<complex<double>>(volume));
        copy_vector(U[locked], u_candidate[i], volume);
        normalize(U[locked], volume);
        locked++;
      } else {
        // Add it to the thick restart set
        copy_vector(Q[l], u_candidate[i], volume);
        Tm(l,l) = eigsolve_Tm.eigenvalues()(i);

        // Store s somewhere safe
        Tm(m-1,l) = s_candidate[i];

        l++;
      }
    }

    // Clean up candidates.
    for (int i = 0; i < num_candidate; i++) {
      deallocate_vector(&u_candidate[i]);
    }

    // If there are no vectors in the thick restart set, quit.
    if (l == 0) {
      num_no_TR_candidates++;
      std::cout << "No thick restart vectors: " << num_no_TR_candidates << ".\n";
      if (num_no_TR_candidates == max_no_TR_candidates) {
        std::cout << "Max number of times with no thick restart vectors reached.\n";
        break;
      }
    }

    // Put last Q in place.
    copy_vector(Q[l], Q[m], volume);

  }

  if (iters == max_iterations) {
    std::cout << "Hit maximum iterations.\n" << "\n";
  }

  // Uh... I guess we have eigenvalues.
  std::cout << "Computed eigenvalues.\n";
  for (unsigned int i = 0; i < lambda.size(); i++) {
    std::cout << lambda[i] << "\n";
  }
  std::cout << "\n";

  /**/

  if (volume <= 256) {

    // Let's get the eigenvalues of the full operator!

    // Allocate a sufficiently gigantic matrix.
    cMatrix mat_cplx = cMatrix::Zero(volume, volume);

    // Form matrix elements. This is where it's important that
    // dMatrix and cMatrix are column major.
    // I should probably make this safer by using a "Map".
    for (int i = 0; i < volume; i++)
    {
      // Set a point on the rhs for a matrix element.
      zero_vector(rhs_cplx, volume);
      rhs_cplx[i] = 1.0;

      // Where we put the result of the matrix element.
      complex<double>* mptr = &(mat_cplx(i*volume));

      square_laplacian_gauged(mptr, rhs_cplx, &lapstr_gauged);
    }

    // Get the eigenvalues.
    SelfAdjointEigenSolver<cMatrix> eigsolve_cplx(volume);
    eigsolve_cplx.compute(mat_cplx);

    std::cout << "The eigenvalues are:\n" << eigsolve_cplx.eigenvalues() << "\n";

    ////////////////////////////////
    // COMPARE LOWEST EIGENVECTOR //
    ////////////////////////////////

    std::cout << "Compare results:\n\n";
    std::cout << "Lanczos Exact Ratio\n";
    for (int i = 0; i < volume; i++) {
      std::cout << U[0][i] << " " << eigsolve_cplx.eigenvectors().col(0)(i)
                << " " << U[0][i]/eigsolve_cplx.eigenvectors().col(0)(i) << "\n";
    }
  }

  //////////////
  // CLEAN UP //
  //////////////

  deallocate_vector(&w);
  deallocate_vector(&lambda_x_u);

  for (int i = 0; i <= m; i++) {
    deallocate_vector(&Q[i]);
  }
  delete[] Q;

  deallocate_vector(&rhs_cplx);
  deallocate_vector(&gauge_links);
  return 0;
}


#pragma once

#include "kalman-cpp/kalman.hpp"
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

typedef std::pair<double, double> Point;

class LShapeTracker {
public:

  LShapeTracker(const Point& ,const double& ,const double& ,const double& ,const double& );
  LShapeTracker();//Create a blank estimator

  void update(const Point& ,const double& ,const double& ,const double& ,const double& );

  void lshapeToBoxModelConversion(double& x, double& y, double& L1, double& L2, double& th);
  void ClockwisePointSwitch();
  void CounterClockwisePointSwitch();
  void changeStates(const Eigen::Vector4d& new_dynamic_states,const Eigen::Vector3d& new_shape_states);

  KalmanFilter dynamic;
  KalmanFilter shape;

private:

};


/*
 * File: distance.cpp
 * Project: src
 * Created Date: Friday, November 29th 2019, 6:12:26 pm
 * Author: Ramaseshan Ramachandran
 * -----
 * Last Modified: Saturday, 30th November 2019 7:06:29 pm
 * Modified By: Ramaseshan Ramachandran
 * -----
 * Copyright (c) 2019 JCR Labz
 * blah
 * -----
 * HISTORY:
 * Date      	By	Comments
 * ----------	---	----------------------------------------------------------
 */
#include <iostream>
#include <fstream>
#include <map>
#include <utility>
#include <armadillo>
#include <algorithm>

#include "../include/distance.hpp"
using namespace nlp;

distance::distance(std::string wvm_filename, std::string vocab_filename,
                   std::string target_word, nlp::wv_models wv_model, int distance_measure)
{
    this->wvm_filename = wvm_filename;
    this->vocab_filename = vocab_filename;
    this->target_word = target_word;

    if (distance_measure == nlp::COSINE_DISTANCE)
    {
        if (wv_model == nlp::HAL)
            cosine_hal();
        else
            cosine();
    }
}

distance::~distance()
{
}

void distance::cosine()
{
    std::ifstream inf;
    inf.open(this->vocab_filename);
    std::string key;
    std::map<std::string, int> vocab_map;
    unsigned long i = 0;
    while (!inf.eof())
    {
        inf >> key;
        vocab_map[key] = i++;
    }

    uint count = vocab_map.count(this->target_word);
    if (!count)
    {
        std::cerr << this->target_word << " not found in the vocabulary. Use a different word" << std::endl;
        exit(1);
    }
    std::vector<std::pair<double, std::string>> dist;

    arma::mat matrix;
    matrix.load(this->wvm_filename);
    matrix = matrix.t();
    arma::vec target(matrix.n_cols);
    target = matrix.col(vocab_map[this->target_word]); //மொழிகள்  ஊர் கணிதம் xகண்ணன்x கணினி கணினி
    //target = target.t();
    std::ofstream out;
    out.open(this->target_word + ".txt");
    out << "Distance,Word" << std::endl;
    i = 0;
    for (auto &v : vocab_map)
    {
        //vec a = hal_matrix.col(i++);
        if (i < matrix.n_cols)
        {
            //double distance = 1.0 - (arma::dot(target, matrix.col(i)) / (arma::norm(target, 2) * arma::norm(matrix.col(i), 2)));
            double distance = 1.0 - arma::norm_dot(target, matrix.col(i));
            i++;
            //std::cout << distance << " " << v.first << endl;
            dist.push_back(make_pair(distance, v.first));
        }
    }
    sort(dist.begin(), dist.end());
    for (auto &k : dist)
        out << k.first << "," << k.second << std::endl;
}

void distance::cosine_hal()
{
    std::ifstream inf;
    inf.open(this->vocab_filename);
    std::string key;
    std::map<std::string, int> vocab_map;
    unsigned long i = 0;
    while (!inf.eof())
    {
        inf >> key;
        vocab_map[key] = i++;
    }

    uint count = vocab_map.count(this->target_word);
    if (!count)
    {
        std::cerr << this->target_word << " not found in the vocabulary. Use a different word" << std::endl;
        exit(1);
    }
    std::vector<std::pair<double, std::string>> dist;

    arma::sp_mat matrix(vocab_map.size(), vocab_map.size());
    matrix.load(this->wvm_filename);
    arma::vec target(matrix.n_rows);

    //hal_matrix = hal_matrix.t();
    target = matrix.col(vocab_map[this->target_word]); //மொழிகள்  ஊர் கணிதம் xகண்ணன்x கணினி கணினி
    std::ofstream out;
    out.open(this->target_word + ".txt");
    out << "Distance,Word" << std::endl;
    i = 0;
    for (auto &v : vocab_map)
    {
        //vec a = hal_matrix.col(i++);
        if (i < matrix.n_cols)
        {
            double distance = 1.0 - (arma::dot(target, matrix.col(i)) / (arma::norm(target, 2) * arma::norm(matrix.col(i), 2)));
            i++;
            //std::cout << distance << " " << v.first << endl;
            dist.push_back(make_pair(distance, v.first));
        }
    }
    sort(dist.begin(), dist.end());
    for (auto &k : dist)
        out << k.first << "," << k.second << std::endl;
}


/*
	This file is part of solidity.

	solidity is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	solidity is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with solidity.  If not, see <http://www.gnu.org/licenses/>.
*/
/**
 * @file ExpressionClasses.cpp
 * @author Christian <c@ethdev.com>
 * @date 2015
 * Container for equivalence classes of expressions for use in common subexpression elimination.
 */

#include <libevmasm/ExpressionClasses.h>
#include <utility>
#include <tuple>
#include <functional>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/noncopyable.hpp>
#include <libevmasm/Assembly.h>
#include <libevmasm/CommonSubexpressionEliminator.h>
#include <libevmasm/SimplificationRules.h>

using namespace std;
using namespace dev;
using namespace dev::eth;

pair<Pattern, function<Pattern()> > const* Rules::findFirstMatch(
	Expression const& _expr,
	ExpressionClasses const& _classes
)
{
	resetMatchGroups();

	assertThrow(_expr.item, OptimizerException, "");
	for (auto const& rule: m_rules[byte(_expr.item->instruction())])
	{
		if (rule.first.matches(_expr, _classes))
			return &rule;
		resetMatchGroups();
	}
	return nullptr;
}

void Rules::addRules(std::vector<std::pair<Pattern, std::function<Pattern ()> > > const& _rules)
{
	for (auto const& r: _rules)
		addRule(r);
}

void Rules::addRule(std::pair<Pattern, std::function<Pattern()> > const& _rule)
{
	m_rules[byte(_rule.first.instruction())].push_back(_rule);
}

template <class S> S divWorkaround(S const& _a, S const& _b)
{
	return (S)(bigint(_a) / bigint(_b));
}

template <class S> S modWorkaround(S const& _a, S const& _b)
{
	return (S)(bigint(_a) % bigint(_b));
}

Rules::Rules()
{
	// Multiple occurences of one of these inside one rule must match the same equivalence class.
	// Constants.
	Pattern A(Push);
	Pattern B(Push);
	Pattern C(Push);
	// Anything.
	Pattern X;
	Pattern Y;
	Pattern Z;
	A.setMatchGroup(1, m_matchGroups);
	B.setMatchGroup(2, m_matchGroups);
	C.setMatchGroup(3, m_matchGroups);
	X.setMatchGroup(4, m_matchGroups);
	Y.setMatchGroup(5, m_matchGroups);
	Z.setMatchGroup(6, m_matchGroups);

	addRules(vector<pair<Pattern, function<Pattern()>>>{
		// arithmetics on constants
		{{Instruction::ADD, {A, B}}, [=]{ return A.d() + B.d(); }},
		{{Instruction::MUL, {A, B}}, [=]{ return A.d() * B.d(); }},
		{{Instruction::SUB, {A, B}}, [=]{ return A.d() - B.d(); }},
		{{Instruction::DIV, {A, B}}, [=]{ return B.d() == 0 ? 0 : divWorkaround(A.d(), B.d()); }},
		{{Instruction::SDIV, {A, B}}, [=]{ return B.d() == 0 ? 0 : s2u(divWorkaround(u2s(A.d()), u2s(B.d()))); }},
		{{Instruction::MOD, {A, B}}, [=]{ return B.d() == 0 ? 0 : modWorkaround(A.d(), B.d()); }},
		{{Instruction::SMOD, {A, B}}, [=]{ return B.d() == 0 ? 0 : s2u(modWorkaround(u2s(A.d()), u2s(B.d()))); }},
		{{Instruction::EXP, {A, B}}, [=]{ return u256(boost::multiprecision::powm(bigint(A.d()), bigint(B.d()), bigint(1) << 256)); }},
		{{Instruction::NOT, {A}}, [=]{ return ~A.d(); }},
		{{Instruction::LT, {A, B}}, [=]() -> u256 { return A.d() < B.d() ? 1 : 0; }},
		{{Instruction::GT, {A, B}}, [=]() -> u256 { return A.d() > B.d() ? 1 : 0; }},
		{{Instruction::SLT, {A, B}}, [=]() -> u256 { return u2s(A.d()) < u2s(B.d()) ? 1 : 0; }},
		{{Instruction::SGT, {A, B}}, [=]() -> u256 { return u2s(A.d()) > u2s(B.d()) ? 1 : 0; }},
		{{Instruction::EQ, {A, B}}, [=]() -> u256 { return A.d() == B.d() ? 1 : 0; }},
		{{Instruction::ISZERO, {A}}, [=]() -> u256 { return A.d() == 0 ? 1 : 0; }},
		{{Instruction::AND, {A, B}}, [=]{ return A.d() & B.d(); }},
		{{Instruction::OR, {A, B}}, [=]{ return A.d() | B.d(); }},
		{{Instruction::XOR, {A, B}}, [=]{ return A.d() ^ B.d(); }},
		{{Instruction::BYTE, {A, B}}, [=]{ return A.d() >= 32 ? 0 : (B.d() >> unsigned(8 * (31 - A.d()))) & 0xff; }},
		{{Instruction::ADDMOD, {A, B, C}}, [=]{ return C.d() == 0 ? 0 : u256((bigint(A.d()) + bigint(B.d())) % C.d()); }},
		{{Instruction::MULMOD, {A, B, C}}, [=]{ return C.d() == 0 ? 0 : u256((bigint(A.d()) * bigint(B.d())) % C.d()); }},
		{{Instruction::MULMOD, {A, B, C}}, [=]{ return A.d() * B.d(); }},
		{{Instruction::SIGNEXTEND, {A, B}}, [=]() -> u256 {
			if (A.d() >= 31)
				return B.d();
			unsigned testBit = unsigned(A.d()) * 8 + 7;
			u256 mask = (u256(1) << testBit) - 1;
			return u256(boost::multiprecision::bit_test(B.d(), testBit) ? B.d() | ~mask : B.d() & mask);
		}},

		// invariants involving known constants (commutative instructions will be checked with swapped operants too)
		{{Instruction::ADD, {X, 0}}, [=]{ return X; }},
		{{Instruction::SUB, {X, 0}}, [=]{ return X; }},
		{{Instruction::MUL, {X, 0}}, [=]{ return u256(0); }},
		{{Instruction::MUL, {X, 1}}, [=]{ return X; }},
		{{Instruction::DIV, {X, 0}}, [=]{ return u256(0); }},
		{{Instruction::DIV, {0, X}}, [=]{ return u256(0); }},
		{{Instruction::DIV, {X, 1}}, [=]{ return X; }},
		{{Instruction::SDIV, {X, 0}}, [=]{ return u256(0); }},
		{{Instruction::SDIV, {0, X}}, [=]{ return u256(0); }},
		{{Instruction::SDIV, {X, 1}}, [=]{ return X; }},
		{{Instruction::AND, {X, ~u256(0)}}, [=]{ return X; }},
		{{Instruction::AND, {X, 0}}, [=]{ return u256(0); }},
		{{Instruction::OR, {X, 0}}, [=]{ return X; }},
		{{Instruction::OR, {X, ~u256(0)}}, [=]{ return ~u256(0); }},
		{{Instruction::XOR, {X, 0}}, [=]{ return X; }},
		{{Instruction::MOD, {X, 0}}, [=]{ return u256(0); }},
		{{Instruction::MOD, {0, X}}, [=]{ return u256(0); }},
		{{Instruction::EQ, {X, 0}}, [=]() -> Pattern { return {Instruction::ISZERO, {X}}; } },

		// operations involving an expression and itself
		{{Instruction::AND, {X, X}}, [=]{ return X; }},
		{{Instruction::OR, {X, X}}, [=]{ return X; }},
		{{Instruction::XOR, {X, X}}, [=]{ return u256(0); }},
		{{Instruction::SUB, {X, X}}, [=]{ return u256(0); }},
		{{Instruction::EQ, {X, X}}, [=]{ return u256(1); }},
		{{Instruction::LT, {X, X}}, [=]{ return u256(0); }},
		{{Instruction::SLT, {X, X}}, [=]{ return u256(0); }},
		{{Instruction::GT, {X, X}}, [=]{ return u256(0); }},
		{{Instruction::SGT, {X, X}}, [=]{ return u256(0); }},
		{{Instruction::MOD, {X, X}}, [=]{ return u256(0); }},

		// logical instruction combinations
		{{Instruction::NOT, {{Instruction::NOT, {X}}}}, [=]{ return X; }},
		{{Instruction::XOR, {{{X}, {Instruction::XOR, {X, Y}}}}}, [=]{ return Y; }},
		{{Instruction::OR, {{{X}, {Instruction::AND, {X, Y}}}}}, [=]{ return X; }},
		{{Instruction::AND, {{{X}, {Instruction::OR, {X, Y}}}}}, [=]{ return X; }},
		{{Instruction::AND, {{{X}, {Instruction::NOT, {X}}}}}, [=]{ return u256(0); }},
		{{Instruction::OR, {{{X}, {Instruction::NOT, {X}}}}}, [=]{ return ~u256(0); }},
	});

	// Double negation of opcodes with binary result
	for (auto const& op: vector<Instruction>{
		Instruction::EQ,
		Instruction::LT,
		Instruction::SLT,
		Instruction::GT,
		Instruction::SGT
	})
		addRule({
			{Instruction::ISZERO, {{Instruction::ISZERO, {{op, {X, Y}}}}}},
			[=]() -> Pattern { return {op, {X, Y}}; }
		});

	addRule({
		{Instruction::ISZERO, {{Instruction::ISZERO, {{Instruction::ISZERO, {X}}}}}},
		[=]() -> Pattern { return {Instruction::ISZERO, {X}}; }
	});

	addRule({
		{Instruction::ISZERO, {{Instruction::XOR, {X, Y}}}},
		[=]() -> Pattern { return { Instruction::EQ, {X, Y} }; }
	});

	// Associative operations
	for (auto const& opFun: vector<pair<Instruction,function<u256(u256 const&,u256 const&)>>>{
		{Instruction::ADD, plus<u256>()},
		{Instruction::MUL, multiplies<u256>()},
		{Instruction::AND, bit_and<u256>()},
		{Instruction::OR, bit_or<u256>()},
		{Instruction::XOR, bit_xor<u256>()}
	})
	{
		auto op = opFun.first;
		auto fun = opFun.second;
		// Moving constants to the outside, order matters here!
		// we need actions that return expressions (or patterns?) here, and we need also reversed rules
		// (X+A)+B -> X+(A+B)
		addRules(vector<pair<Pattern, function<Pattern()>>>{{
			{op, {{op, {X, A}}, B}},
			[=]() -> Pattern { return {op, {X, fun(A.d(), B.d())}}; }
		}, {
		// X+(Y+A) -> (X+Y)+A
			{op, {{op, {X, A}}, Y}},
			[=]() -> Pattern { return {op, {{op, {X, Y}}, A}}; }
		}, {
		// For now, we still need explicit commutativity for the inner pattern
			{op, {{op, {A, X}}, B}},
			[=]() -> Pattern { return {op, {X, fun(A.d(), B.d())}}; }
		}, {
			{op, {{op, {A, X}}, Y}},
			[=]() -> Pattern { return {op, {{op, {X, Y}}, A}}; }
		}});
	}

	// move constants across subtractions
	addRules(vector<pair<Pattern, function<Pattern()>>>{
		{
			// X - A -> X + (-A)
			{Instruction::SUB, {X, A}},
			[=]() -> Pattern { return {Instruction::ADD, {X, 0 - A.d()}}; }
		}, {
			// (X + A) - Y -> (X - Y) + A
			{Instruction::SUB, {{Instruction::ADD, {X, A}}, Y}},
			[=]() -> Pattern { return {Instruction::ADD, {{Instruction::SUB, {X, Y}}, A}}; }
		}, {
			// (A + X) - Y -> (X - Y) + A
			{Instruction::SUB, {{Instruction::ADD, {A, X}}, Y}},
			[=]() -> Pattern { return {Instruction::ADD, {{Instruction::SUB, {X, Y}}, A}}; }
		}, {
			// X - (Y + A) -> (X - Y) + (-A)
			{Instruction::SUB, {X, {Instruction::ADD, {Y, A}}}},
			[=]() -> Pattern { return {Instruction::ADD, {{Instruction::SUB, {X, Y}}, 0 - A.d()}}; }
		}, {
			// X - (A + Y) -> (X - Y) + (-A)
			{Instruction::SUB, {X, {Instruction::ADD, {A, Y}}}},
			[=]() -> Pattern { return {Instruction::ADD, {{Instruction::SUB, {X, Y}}, 0 - A.d()}}; }
		}
	});
}

Pattern::Pattern(Instruction _instruction, std::vector<Pattern> const& _arguments):
	m_type(Operation),
	m_instruction(_instruction),
	m_arguments(_arguments)
{
}

void Pattern::setMatchGroup(unsigned _group, map<unsigned, Expression const*>& _matchGroups)
{
	m_matchGroup = _group;
	m_matchGroups = &_matchGroups;
}

bool Pattern::matches(Expression const& _expr, ExpressionClasses const& _classes) const
{
	if (!matchesBaseItem(_expr.item))
		return false;
	if (m_matchGroup)
	{
		if (!m_matchGroups->count(m_matchGroup))
			(*m_matchGroups)[m_matchGroup] = &_expr;
		else if ((*m_matchGroups)[m_matchGroup]->id != _expr.id)
			return false;
	}
	assertThrow(m_arguments.size() == 0 || _expr.arguments.size() == m_arguments.size(), OptimizerException, "");
	for (size_t i = 0; i < m_arguments.size(); ++i)
		if (!m_arguments[i].matches(_classes.representative(_expr.arguments[i]), _classes))
			return false;
	return true;
}

AssemblyItem Pattern::toAssemblyItem(SourceLocation const& _location) const
{
	if (m_type == Operation)
		return AssemblyItem(m_instruction, _location);
	else
		return AssemblyItem(m_type, data(), _location);
}

string Pattern::toString() const
{
	stringstream s;
	switch (m_type)
	{
	case Operation:
		s << instructionInfo(m_instruction).name;
		break;
	case Push:
		if (m_data)
			s << "PUSH " << hex << data();
		else
			s << "PUSH ";
		break;
	case UndefinedItem:
		s << "ANY";
		break;
	default:
		if (m_data)
			s << "t=" << dec << m_type << " d=" << hex << data();
		else
			s << "t=" << dec << m_type << " d: nullptr";
		break;
	}
	if (!m_requireDataMatch)
		s << " ~";
	if (m_matchGroup)
		s << "[" << dec << m_matchGroup << "]";
	s << "(";
	for (Pattern const& p: m_arguments)
		s << p.toString() << ", ";
	s << ")";
	return s.str();
}

bool Pattern::matchesBaseItem(AssemblyItem const* _item) const
{
	if (m_type == UndefinedItem)
		return true;
	if (!_item)
		return false;
	if (m_type != _item->type())
		return false;
	else if (m_type == Operation)
		return m_instruction == _item->instruction();
	else if (m_requireDataMatch)
		return data() == _item->data();
	return true;
}

Pattern::Expression const& Pattern::matchGroupValue() const
{
	assertThrow(m_matchGroup > 0, OptimizerException, "");
	assertThrow(!!m_matchGroups, OptimizerException, "");
	assertThrow((*m_matchGroups)[m_matchGroup], OptimizerException, "");
	return *(*m_matchGroups)[m_matchGroup];
}

u256 const& Pattern::data() const
{
	assertThrow(m_data, OptimizerException, "");
	return *m_data;
}

ExpressionTemplate::ExpressionTemplate(Pattern const& _pattern, SourceLocation const& _location)
{
	if (_pattern.matchGroup())
	{
		hasId = true;
		id = _pattern.id();
	}
	else
	{
		hasId = false;
		item = _pattern.toAssemblyItem(_location);
	}
	for (auto const& arg: _pattern.arguments())
		arguments.push_back(ExpressionTemplate(arg, _location));
}

string ExpressionTemplate::toString() const
{
	stringstream s;
	if (hasId)
		s << id;
	else
		s << item;
	s << "(";
	for (auto const& arg: arguments)
		s << arg.toString();
	s << ")";
	return s.str();
}


/********************************************************************************
  Copyright 2017 Frank Imeson and Stephen L. Smith

  Licensed under the Apache License, Version 2.0 (the "License");
  you may not use this file except in compliance with the License.
  You may obtain a copy of the License at

     http://www.apache.org/licenses/LICENSE-2.0

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

#ifndef TSP_H		// guard
#define TSP_H

/********************************************************************************
 * INCLUDE
 ********************************************************************************/

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <fstream>
#include <sstream>
#include <iostream>
#include <vector>
#include <algorithm>

#include <boost/regex.hpp>

#include "formula.hpp"
#include "minisat/core/Solver.h"
#include "minisat/core/Dimacs.h"

using namespace std;
using namespace Minisat;

/********************************************************************************
 * Defs
 ********************************************************************************/
#define INF 999999

/********************************************************************************
 * Prototypes
 ********************************************************************************/

class TSP;
class MST;

int parse_input(string filename, TSP &graph);
//void tsp2sat(graph);

class TSP {
  private:
    int edge_var_offset;
    int _metric, _symmetric, _tsp_monotonic;
    string lkh_parameters;
  public:
    TSP () : _metric(-1), _symmetric(-1), _tsp_monotonic(-1), tsp_cost_budget(INF), subgraph_cost_budget(INF) {
      lkh_parameters  = "PRECISION         = 10\n";
      lkh_parameters += "MOVE_TYPE         = 5\n";
      lkh_parameters += "PATCHING_C        = 3\n";
      lkh_parameters += "PATCHING_A        = 2\n";
      lkh_parameters += "RUNS              = 1\n";
      lkh_parameters += "TRACE_LEVEL       = 0\n";
    };
    string name, type;
    int tsp_cost_budget, subgraph_cost_budget;
    vector< vector<bool> >  adjacency;
    vector< vector<int> > edge_weight;
    vector< vector<int> > vid_incoming_vars;
    vector< vector<int> > vid_outgoing_vars;
    vector<vector<int>> gtsp_sets, subgraphs;
    vector<int> level00_vars;
    vector<int> theory_vars;

    int size () {return edge_weight.size();};
    int tsp2cnf (Solver* solver);
    int eid2var (const int eid);
    int vid2var (const int vid);
    int var2eid (const int var);
    int var2vid (const int var);
    void eid2edge (const int eid, int &from, int &to);
    int edge2eid (const int from, const int to);
    int eid_cost (int eid);
    bool operator() (int eid00, int eid01) {return eid_cost(eid00) > eid_cost(eid01);};
    int  lit_cost (const Lit &lit);
    bool LKH(const vector<int> &vids, const int &max_cost, vector<int> &soln_tour, int &soln_cost);
    bool solve(const vector<int> &vids, const int tsp_cost_budget, const int subgraph_cost_budget, vector<int> &soln_tour, int &soln_cost);
    bool metric();
    bool symmetric();
    bool tsp_monotonic();
    void assume_metric() {_metric = 1;};
    void assume_non_metric() {_metric = 0;};
    void assume_symmetric() {_symmetric = 1;};
    void assume_non_symmetric() {_symmetric = 0;};
    void assume_tsp_monotonic() {_tsp_monotonic = 1;};
    void assume_non_tsp_monotonic() {_tsp_monotonic = 0;};
    void set_lkh_parameters(string input_file);
    string get_lkh_parameters();
    bool feasible(const vector<int> &soln_tour, const int tsp_cost_budget, const int subgraph_cost_budget);
    void split_vids(const vector<int> &vids, vector<vector<int>> &subgraph_vids);
    bool get_tour_cost(const vector<int> &tour, int &tsp_cost, int &max_subgraph_cost);
};

class MST {
  private:
    TSP*                    graph;
    int                     _size;
    int                     _cost;
    vector<bool>            subset;
    vector< vector<bool> >  adjacency;

    int                     z_vid;                // new vertex to insert
    vector<bool>            new_subset;
    vector< vector<bool> >  new_adjacency;

    void                    _insert(int r_vid, int &t_eid);
  public:
    MST (TSP* graph);
    MST (TSP* graph, const vector<int> subset);
    int insert(int vid);
    int adjacent (int vid, vector<int> &list);
    int size () {return _size;};
    int cost() {return _cost;};

    string str() {
      stringstream output;
      if (subset.size() > 0) {
        output << "[";
        for (int i=0; i<subset.size(); i++)
          if (subset[i])
            output << i << ", ";
        output << "]";
      } else {
        output << "[]";
      }
      return output.str();
    }
};

#endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Daniele Panozzo <daniele.panozzo@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.

#include "Viewer.h"

//#include <chrono>
#include <thread>

#include <Eigen/LU>

#include <cmath>
#include <cstdio>
#include <sstream>
#include <iomanip>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <limits>
#include <cassert>

#include <igl/project.h>
//#include <igl/get_seconds.h>
#include <igl/readOBJ.h>
#include <igl/readOFF.h>
#include <igl/adjacency_list.h>
#include <igl/writeOBJ.h>
#include <igl/writeOFF.h>
#include <igl/massmatrix.h>
#include <igl/file_dialog_open.h>
#include <igl/file_dialog_save.h>
#include <igl/quat_mult.h>
#include <igl/axis_angle_to_quat.h>
#include <igl/trackball.h>
#include <igl/two_axis_valuator_fixed_up.h>
#include <igl/snap_to_canonical_view_quat.h>
#include <igl/unproject.h>
#include <igl/serialize.h>

#include <igl/edge_flaps.h>
#include <igl/shortest_edge_and_midpoint.h>
#include <igl/collapse_edge.h>

#include <set>

#include <igl/readMESH.h>
#include <igl\AABB.h>
#include <igl\point_mesh_squared_distance.h>
#include <igl\per_edge_normals.h>
#include <igl\per_vertex_normals.h>
#include <igl\marching_tets.h>
#include <igl/edge_lengths.h>
#include <igl\slice_mask.h>
#include <igl\upsample.h>
#include <igl\cat.h>
#include <igl\signed_distance.h>
#include <igl\parula.h>

// Internal global variables used for glfw event handling
//static igl::opengl::glfw::Viewer * __viewer;
static double highdpi = 1;
static double scroll_x = 0;
static double scroll_y = 0;

using namespace Eigen;

namespace igl
{
namespace opengl
{
namespace glfw
{

  IGL_INLINE void Viewer::init()
  {

  }

  //IGL_INLINE void Viewer::init_plugins()
  //{
  //  // Init all plugins
  //  for (unsigned int i = 0; i<plugins.size(); ++i)
  //  {
  //    plugins[i]->init(this);
  //  }
  //}

  //IGL_INLINE void Viewer::shutdown_plugins()
  //{
  //  for (unsigned int i = 0; i<plugins.size(); ++i)
  //  {
  //    plugins[i]->shutdown();
  //  }
  //}

  IGL_INLINE Viewer::Viewer():
    data_list(1),
    selected_data_index(0),
    next_data_id(1)
  {
    data_list.front().id = 0;

    // Temporary variables initialization
   // down = false;
  //  hack_never_moved = true;
    scroll_position = 0.0f;

    // Per face
    data().set_face_based(false);

#ifndef IGL_VIEWER_VIEWER_QUIET
    const std::string usage(R"(igl::opengl::glfw::Viewer usage:
  [drag]  Rotate scene
  A,a     Toggle animation (tight draw loop)
  F,f     Toggle face based
  I,i     Toggle invert normals
  L,l     Toggle wireframe
  O,o     Toggle orthographic/perspective projection
  T,t     Toggle filled faces
  [,]     Toggle between cameras
  1,2     Toggle between models
  ;       Toggle vertex labels
  :       Toggle face labels)"
);
    std::cout<<usage<<std::endl;
#endif
  }

  IGL_INLINE Viewer::~Viewer()
  {
  }

  IGL_INLINE bool Viewer::load_mesh_from_file(
      const std::string & mesh_file_name_string)
  {

    // Create new data slot and set to selected
    if(!(data().F.rows() == 0  && data().V.rows() == 0))
    {
      append_mesh();
    }
    data().clear();

    size_t last_dot = mesh_file_name_string.rfind('.');
    if (last_dot == std::string::npos)
    {
      std::cerr<<"Error: No file extension found in "<<
        mesh_file_name_string<<std::endl;
      return false;
    }

    std::string extension = mesh_file_name_string.substr(last_dot+1);

	// Assignment 4 //
	if (extension == "mesh" || extension == "MESH")
	{
		Eigen::MatrixXd V;
		Eigen::MatrixXi F;
		Eigen::MatrixXi T;
		Eigen::MatrixXd FN, VN, EN;
		Eigen::MatrixXi E;
		Eigen::VectorXi EMAP;
		double max_distance = 1;

		if (!igl::readMESH(mesh_file_name_string, V, T, F))
			return false;

		VectorXd sqrD;
		VectorXi I;
		MatrixXd C;
		igl::point_mesh_squared_distance(V, V, F, sqrD, I, C);
		max_distance = sqrt(sqrD.maxCoeff());
		data().tree.init(V, F);

		// Precompute vertex,edge and face normals
		igl::per_face_normals(V, F, FN);
		igl::per_vertex_normals(
			V, F, igl::PER_VERTEX_NORMALS_WEIGHTING_TYPE_ANGLE, FN, VN);
		igl::per_edge_normals(
			V, F, igl::PER_EDGE_NORMALS_WEIGHTING_TYPE_UNIFORM, FN, EN, E, EMAP);

		data().set_mesh(V, F);
		data().save_original_vertices_and_faces();
		data().reset();
		return true;
	}
	else if (extension == "off" || extension == "OFF")
	{
		Eigen::MatrixXd V;
		Eigen::MatrixXi F;
		if (!igl::readOFF(mesh_file_name_string, V, F))
		  return false;

		data().set_mesh(V,F);
		data().save_original_vertices_and_faces();
		data().reset();
	}
    else if (extension == "obj" || extension =="OBJ")
    {
      Eigen::MatrixXd corner_normals;
      Eigen::MatrixXi fNormIndices;

      Eigen::MatrixXd UV_V;
      Eigen::MatrixXi UV_F;
      Eigen::MatrixXd V;
      Eigen::MatrixXi F;
	  Eigen::MatrixXi E;
	  Eigen::VectorXi EMAP;
	  Eigen::MatrixXi EF;
	  Eigen::MatrixXi EI;

      if (!(
            igl::readOBJ(
              mesh_file_name_string,
              V, UV_V, corner_normals, F, UV_F, fNormIndices)))
      {
        return false;
      }

      data().set_mesh(V,F);
      data().set_uv(UV_V,UV_F);
	  data().save_original_vertices_and_faces();
	  data().reset();
    }
    else
    {
      // unrecognized file type
      printf("Error: %s is not a recognized file type.\n",extension.c_str());
      return false;
    }

    data().compute_normals();
    data().uniform_colors(Eigen::Vector3d(51.0/255.0,43.0/255.0,33.3/255.0),
                   Eigen::Vector3d(255.0/255.0,228.0/255.0,58.0/255.0),
                   Eigen::Vector3d(255.0/255.0,235.0/255.0,80.0/255.0));

    // Alec: why?
    if (data().V_uv.rows() == 0)
    {
      data().grid_texture();
    }

    //for (unsigned int i = 0; i<plugins.size(); ++i)
    //  if (plugins[i]->post_load())
    //    return true;
    return true;
  }

  IGL_INLINE bool Viewer::save_mesh_to_file(
      const std::string & mesh_file_name_string)
  {
    // first try to load it with a plugin
    //for (unsigned int i = 0; i<plugins.size(); ++i)
    //  if (plugins[i]->save(mesh_file_name_string))
    //    return true;

    size_t last_dot = mesh_file_name_string.rfind('.');
    if (last_dot == std::string::npos)
    {
      // No file type determined
      std::cerr<<"Error: No file extension found in "<<
        mesh_file_name_string<<std::endl;
      return false;
    }
    std::string extension = mesh_file_name_string.substr(last_dot+1);
    if (extension == "off" || extension =="OFF")
    {
      return igl::writeOFF(
        mesh_file_name_string,data().V,data().F);
    }
    else if (extension == "obj" || extension =="OBJ")
    {
      Eigen::MatrixXd corner_normals;
      Eigen::MatrixXi fNormIndices;

      Eigen::MatrixXd UV_V;
      Eigen::MatrixXi UV_F;

      return igl::writeOBJ(mesh_file_name_string,
          data().V,
          data().F,
          corner_normals, fNormIndices, UV_V, UV_F);
    }
    else
    {
      // unrecognized file type
      printf("Error: %s is not a recognized file type.\n",extension.c_str());
      return false;
    }
    return true;
  }

  IGL_INLINE bool Viewer::load_scene()
  {
    std::string fname = igl::file_dialog_open();
    if(fname.length() == 0)
      return false;
    return load_scene(fname);
  }

  IGL_INLINE bool Viewer::load_scene(std::string fname)
  {
   // igl::deserialize(core(),"Core",fname.c_str());
    igl::deserialize(data(),"Data",fname.c_str());
    return true;
  }

  IGL_INLINE bool Viewer::save_scene()
  {
    std::string fname = igl::file_dialog_save();
    if (fname.length() == 0)
      return false;
    return save_scene(fname);
  }

  IGL_INLINE bool Viewer::save_scene(std::string fname)
  {
    //igl::serialize(core(),"Core",fname.c_str(),true);
    igl::serialize(data(),"Data",fname.c_str());

    return true;
  }

  IGL_INLINE void Viewer::open_dialog_load_mesh()
  {
    std::string fname = igl::file_dialog_open();

    if (fname.length() == 0)
      return;

    this->load_mesh_from_file(fname.c_str());
  }

  IGL_INLINE void Viewer::open_dialog_save_mesh()
  {
    std::string fname = igl::file_dialog_save();

    if(fname.length() == 0)
      return;

    this->save_mesh_to_file(fname.c_str());
  }

  IGL_INLINE ViewerData& Viewer::data(int mesh_id /*= -1*/)
  {
    assert(!data_list.empty() && "data_list should never be empty");
    int index;
    if (mesh_id == -1)
      index = selected_data_index;
    else
      index = mesh_index(mesh_id);

    assert((index >= 0 && index < data_list.size()) &&
      "selected_data_index or mesh_id should be in bounds");
    return data_list[index];
  }

  IGL_INLINE const ViewerData& Viewer::data(int mesh_id /*= -1*/) const
  {
    assert(!data_list.empty() && "data_list should never be empty");
    int index;
    if (mesh_id == -1)
      index = selected_data_index;
    else
      index = mesh_index(mesh_id);

    assert((index >= 0 && index < data_list.size()) &&
      "selected_data_index or mesh_id should be in bounds");
    return data_list[index];
  }

  IGL_INLINE int Viewer::append_mesh(bool visible /*= true*/)
  {
    assert(data_list.size() >= 1);

    data_list.emplace_back();
    selected_data_index = data_list.size()-1;
    data_list.back().id = next_data_id++;
    //if (visible)
    //    for (int i = 0; i < core_list.size(); i++)
    //        data_list.back().set_visible(true, core_list[i].id);
    //else
    //    data_list.back().is_visible = 0;
    return data_list.back().id;
  }

  IGL_INLINE bool Viewer::erase_mesh(const size_t index)
  {
    assert((index >= 0 && index < data_list.size()) && "index should be in bounds");
    assert(data_list.size() >= 1);
    if(data_list.size() == 1)
    {
      // Cannot remove last mesh
      return false;
    }
    data_list[index].meshgl.free();
    data_list.erase(data_list.begin() + index);
    if(selected_data_index >= index && selected_data_index > 0)
    {
      selected_data_index--;
    }

    return true;
  }

  IGL_INLINE size_t Viewer::mesh_index(const int id) const {
    for (size_t i = 0; i < data_list.size(); ++i)
    {
      if (data_list[i].id == id)
        return i;
    }
    return 0;
  }

  //IGL_INLINE void igl::opengl::glfw::Viewer::reset() {
	 // MatrixXd V = data().V;
	 // MatrixXi F = data().F;
	 // VectorXi EMAP;
	 // MatrixXi E, EF, EI;
	 // typedef std::set<std::pair<double, int> > PriorityQueue;
	 // PriorityQueue Q;
	 // std::vector<PriorityQueue::iterator > Qit;
	 // MatrixXd C;

	 // edge_flaps(F, E, EMAP, EF, EI);
	 // Qit.resize(E.rows());

	 // C.resize(E.rows(), V.cols());
	 // VectorXd costs(E.rows());
	 // Q.clear();
	 // for (int e = 0;e < E.rows();e++)
	 // {
		//  double cost = e;
		//  RowVectorXd p(1, 3);
		//  shortest_edge_and_midpoint(e, V, F, E, EMAP, EF, EI, cost, p);
		//  C.row(e) = p;
		//  Qit[e] = Q.insert(std::pair<double, int>(cost, e)).first;
	 // }
	 // data().clear();
	 // data().set_mesh(V, F);
	 // data().set_edges_and_maps(E, EI, EF, EMAP, Q, Qit);
	 // data().set_face_based(true);
  //}

  IGL_INLINE void Viewer::Animate(Eigen::Vector3f root, Eigen::Vector3f endpoint, ViewerData* cy, Eigen::Vector3f destPoint) {
	  Eigen::Vector3f RD(destPoint - root);
	  Eigen::Vector3f RE(endpoint - root);
	  float angle = acos(RE.normalized().dot(RD.normalized()));

	  Eigen::Matrix3f rot = Eigen::AngleAxisf(angle, (RE.cross(RD)).normalized()).matrix();
	  float angleY0 = atan2(rot(0, 1), rot(2, 1));
	  float angleX = acos(rot(1, 1));
	  float angleY1 = atan2(rot(1, 0), -rot(1, 2));

	  cy->MyRotateY(angleY0);
	  cy->MyRotateX(angleX);
	  cy->MyRotateY(angleY1);

	 // cy->MyRotate((RE.cross(RD)).normalized(), angle);
  }

 double getLengthOfVector(Eigen::Vector3d vec) {
	 return sqrt(pow(vec(0), 2) + pow(vec(1), 2) + pow(vec(2), 2));
  }

 IGL_INLINE bool Viewer::isIntersectBox(Eigen::AlignedBox3d& box0, Eigen::AlignedBox3d& box1,
	 Eigen::Matrix4d& model0, Eigen::Matrix4d& model1,
	 Eigen::Matrix3d& Rot0, Eigen::Matrix3d& Rot1) {

	 Eigen::Vector3d a = box0.sizes() / 1.5; // a0 = a(0), a1 = a(1), a2 = a(2)
	 //a = a / 2;
	 Eigen::Vector3d b = box1.sizes() / 1.5; // b0 = b(0), b1 = b(1), b2 = b(2)
	 //b = b / 2;
	 Eigen::Vector4d temp1;                         // Help to convert vector of 4 to 3

	 Eigen::Matrix3d A;
	 A << Rot0 * (box0.corner(box0.TopLeftFloor) - box0.corner(box0.TopRightFloor)),   // A0
		  Rot0 * (box0.corner(box0.TopLeftFloor) - box0.corner(box0.BottomLeftFloor)), // A1
		  Rot0 * (box0.corner(box0.TopLeftFloor) - box0.corner(box0.TopLeftCeil));     // A2

	 Eigen::Matrix3d B;
	 B << Rot1 * (box1.corner(box1.TopLeftFloor) - box1.corner(box1.TopRightFloor)),   // B0
	 	  Rot1 * (box1.corner(box1.TopLeftFloor) - box1.corner(box1.BottomLeftFloor)), // B1
		  Rot1 * (box1.corner(box1.TopLeftFloor) - box1.corner(box1.TopLeftCeil));     // B2

	 temp1 << box0.center(), 1;
	 Eigen::Vector3d C0 = (model0 * temp1).block<3, 1>(0, 0);;

	 temp1 << box1.center(), 1;
	 Eigen::Vector3d C1 = (model1 * temp1).block<3, 1>(0, 0); ;

	 Eigen::Vector3d D = C1 - C0;
	 // cout << "D:\n" << D << endl;

	 Eigen::Matrix3d C;
	 for (int i = 0; i <= 2; i++) {
		 for (int j = 0; j <= 2; j++) {
			 C(i, j) = A.col(i).dot(B.col(j));
		 }
	 }
	 //Tamir
	  //C = data_list[0].getRotation().inverse().cast<double>() * data_list[1].getRotation().cast<double>();

	  //---------------------------------------Test-----------------------------------------//

	 double R, R0, R1; // For readability
	 for (int i = 0; i <= 2; i++) {
		 // Tests 1,2,3
		 R0 = a(i);
		 R1 = b(0) * abs(C(i, 0)) + b(1) * abs(C(i, 1)) + b(2) * abs(C(i, 2));
		 R = abs(A.col(i).dot(D));
		 if (R > (R0 + R1)) {
			 // std::cout << "Test " << i + 1 << std::endl;
			 return true;
		 }

		 // Tests 4,5,6
		 R0 = a(0) * abs(C(0, i)) + a(1) * abs(C(1, i)) + a(2) * abs(C(2, i));
		 R1 = b(i);
		 R = abs(B.col(i).dot(D));
		 if (R > (R0 + R1)) {
			 // std::cout << "Test " << i + 4 << std::endl;
			 return true;
		 }

		 // Tests 7,8,9,10,11,12,13,14,15
		 for (int j = 0; j <= 2; j++) {
			 R0 = a((i == 0 ? 1 : 0)) * abs(C((i == 2 ? 1 : 2), j)) + a((i == 2 ? 1 : 2)) * abs(C((i == 0 ? 1 : 0), j));
			 R1 = b((j == 0 ? 1 : 0)) * abs(C(i, (j == 2 ? 1 : 2))) + b((j == 2 ? 1 : 2)) * abs(C(i, (j == 0 ? 1 : 0)));
			 R = abs(C(((i + 1) % 3), j) * A.col(((i + 2) % 3)).dot(D) - C(((i + 2) % 3), j) * A.col(((i + 1) % 3)).dot(D));
			 if (R > (R0 + R1)) {
				 // std::cout << "Test " << i * 3 + j + 7 << std::endl;
				 return true;
			 }
		 }
	 }

	 /*
	 R0 = a(0);
	 R1 = (b(0) * abs(C(0, 0))) + (b(1) * abs(C(0, 1))) + (b(2) * abs(C(0, 2)));
	 R  = abs(A.col(0).dot(D));
	 if (R > (R0 + R1)) {
		 std::cout << "Test 1" << std::endl;
		 return;
	 }

	 R0 = a(1);
	 R1 = (b(0) * abs(C(1, 0))) + (b(1) * abs(C(1, 1))) + (b(2) * abs(C(1, 2)));
	 R = abs(A.col(1).dot(D));
	 if (R > (R0 + R1)) {
		 std::cout << "Test 2" << std::endl;
		 return;
	 }

	 R0 = a(2);
	 R1 = (b(0) * abs(C(2, 0))) + (b(1) * abs(C(2, 1))) + (b(2) * abs(C(2, 2)));
	 R = abs(A.col(2).dot(D));
	 if (R > (R0 + R1)) {
		 std::cout << "Test 3" << std::endl;
		 return;
	 }

	 R0 = (a(0) * abs(C(0, 0))) + (a(1) * abs(C(1, 0))) + (a(2) * abs(C(2, 0)));
	 R1 = b(1);
	 R = abs(B.col(0).dot(D));
	 if (R > (R0 + R1)) {
		 std::cout << "Test 4" << std::endl;
		 return;
	 }

	 R0 = (a(0) * abs(C(0, 1))) + (a(1) * abs(C(1, 1))) + (a(2) * abs(C(2, 1)));
	 R1 = b(1);
	 R = abs(B.col(1).dot(D));
	 if (R > (R0 + R1)) {
		 std::cout << "Test 5" << std::endl;
		 return;
	 }

	 R0 = (a(0) * abs(C(0, 2))) + (a(1) * abs(C(1, 2))) + (a(2) * abs(C(2, 2)));
	 R1 = b(2);
	 R = abs(B.col(2).dot(D));
	 if (R > (R0 + R1)) {
		 std::cout << "Test 6" << std::endl;
		 return;
	 }

	 R0 = (a(1) * abs(C(2, 0))) + (a(2) * abs(C(1, 0)));
	 R1 = (b(1) * abs(C(0, 2))) + (b(2) * abs(C(0, 1)));
	 R = abs((C(1, 0) * A.col(2).dot(D)) - (C(2, 0) * A.col(1).dot(D)));
	 if (R > (R0 + R1)) {
		 std::cout << "Test 7" << std::endl;
		 return;
	 }

	 R0 = (a(1) * abs(C(2, 1))) + (a(2) * abs(C(1, 1)));
	 R1 = (b(0) * abs(C(0, 2))) + (b(2) * abs(C(0, 0)));
	 R = abs((C(1, 1) * A.col(2).dot(D)) - (C(2, 1) * A.col(1).dot(D)));
	 if (R > (R0 + R1)) {
		 std::cout << "Test 8" << std::endl;
		 return;
	 }

	 R0 = (a(1) * abs(C(2, 2))) + (a(2) * abs(C(1, 2)));
	 R1 = (b(0) * abs(C(0, 1))) + (b(1) * abs(C(0, 0)));
	 R = abs((C(1, 2) * A.col(2).dot(D)) - (C(2, 2) * A.col(1).dot(D)));
	 if (R > (R0 + R1)) {
		 std::cout << "Test 9" << std::endl;
		 return;
	 }

	 R0 = (a(0) * abs(C(2, 0))) + (a(2) * abs(C(0, 0)));
	 R1 = (b(1) * abs(C(1, 2))) + (b(2) * abs(C(1, 1)));
	 R = abs((C(2, 0) * A.col(0).dot(D)) - (C(0, 0) * A.col(2).dot(D)));
	 if (R > (R0 + R1)) {
		 std::cout << "Test 10" << std::endl;
		 return;
	 }

	 R0 = (a(0) * abs(C(2, 1))) + (a(2) * abs(C(0, 1)));
	 R1 = (b(0) * abs(C(1, 2))) + (b(2) * abs(C(1, 0)));
	 R = abs((C(2, 1) * A.col(0).dot(D)) - (C(0, 1) * A.col(2).dot(D)));
	 if (R > (R0 + R1)) {
		 std::cout << "Test 11" << std::endl;
		 return;
	 }

	 R0 = (a(0) * abs(C(2, 2))) + (a(2) * abs(C(0, 2)));
	 R1 = (b(0) * abs(C(1, 1))) + (b(1) * abs(C(1, 0)));
	 R = abs((C(2, 2) * A.col(0).dot(D)) - (C(0, 2) * A.col(2).dot(D)));
	 if (R > (R0 + R1)) {
		 std::cout << "Test 12" << std::endl;
		 return;
	 }

	 R0 = (a(0) * abs(C(1, 0))) + (a(1) * abs(C(0, 0)));
	 R1 = (b(1) * abs(C(2, 2))) + (b(2) * abs(C(2, 1)));
	 R = abs((C(0, 0) * A.col(1).dot(D)) - (C(1, 0) * A.col(0).dot(D)));
	 if (R > (R0 + R1)) {
		 std::cout << "Test 13" << std::endl;
		 return;
	 }

	 R0 = (a(0) * abs(C(1, 1))) + (a(1) * abs(C(0, 1)));
	 R1 = (b(0) * abs(C(2, 2))) + (b(2) * abs(C(2, 0)));
	 R = abs((C(0, 1) * A.col(1).dot(D)) - (C(1, 1) * A.col(0).dot(D)));
	 if (R > (R0 + R1)) {
		 std::cout << "Test 14" << std::endl;
		 return;
	 }

	 R0 = (a(0) * abs(C(1, 2))) + (a(1) * abs(C(0, 2)));
	 R1 = (b(0) * abs(C(2, 1))) + (b(1) * abs(C(2, 0)));
	 R = abs((C(0, 2) * A.col(1).dot(D)) - (C(1, 2) * A.col(0).dot(D)));
	 if (R > (R0 + R1)) {
		 std::cout << "Test 15" << std::endl;
		 return;
	 }*/

	 // ALL the tests shows that they intersect so we stop
	// std::cout << "All Tests Failed" << std::endl;
	 return false;
 }

  IGL_INLINE void Viewer::isIntersection(igl::AABB<Eigen::MatrixXd, 3>* tree0, igl::AABB<Eigen::MatrixXd, 3>* tree1,
	  Eigen::Matrix4d& model0, Eigen::Matrix4d& model1,
	  Eigen::Matrix3d& Rot0, Eigen::Matrix3d& Rot1) {
	  //Eigen::Matrix4d model0 = data_list[0].MakeTrans().cast<double>();
	  //Eigen::Matrix4d model1 = data_list[1].MakeTrans().cast<double>();
	  //Eigen::Matrix3d Rot0 = model0.block<3, 3>(0, 0);
	  //Eigen::Matrix3d Rot1 = model1.block<3, 3>(0, 0);
	  //igl::AABB<Eigen::MatrixXd, 3>* tree0 = &data_list[0].tree;
	  //igl::AABB<Eigen::MatrixXd, 3>* tree1 = &data_list[1].tree;
	  if (!tree0 || !tree1) {
		  return;
	  }
	  else if (tree0->is_leaf() && tree1->is_leaf() && !move_models) {
		  data_list[0].drawBox(tree0->m_box, Eigen::RowVector3d::Random().normalized());
		  data_list[1].drawBox(tree1->m_box, Eigen::RowVector3d::Random().normalized());
	  }
	  // ---- SHARON ---- //
	  move_models = isIntersectBox(tree0->m_box, tree1->m_box, model0, model1, Rot0, Rot1);
	  // if the first boxes intersect, go deeper:
	  //if (!move_models) {
		 // isIntersection(tree0->m_left, tree1->m_left, model0, model1, Rot0, Rot1);
		 // isIntersection(tree0->m_left, tree1->m_right, model0, model1, Rot0, Rot1);
		 // isIntersection(tree0->m_right, tree1->m_left, model0, model1, Rot0, Rot1);
		 // isIntersection(tree0->m_right, tree1->m_right, model0, model1, Rot0, Rot1);
	  //}
	  if (!move_models) {
		  data_list[0].drawBox(tree0->m_box, Eigen::RowVector3d::Random().normalized());
		  data_list[1].drawBox(tree1->m_box, Eigen::RowVector3d::Random().normalized());
	  }
  }

} // end namespace
} // end namespace
}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/log_2lo.hpp>
#include <boost/simd/constant/log_2hi.hpp>
#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/constant/eps.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check log_2lo behavior for integral types"
            , (std::uint32_t)(std::uint64_t)
              (std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log_2lo;
  using boost::simd::Log_2lo;
  T ref = T(0);
  STF_TYPE_IS(decltype(Log_2lo<T>()), T);
  STF_EQUAL(Log_2lo<T>(), ref);
  STF_EQUAL(log_2lo( as(T{}) ),ref);
}

STF_CASE_TPL( "Check log_2lo behavior for double"
            , (double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log_2lo;
  using boost::simd::Log_2lo;
  T ref = T(1.9082149292705877e-10);
  #ifdef HAS_LONG_DOUBLE
  using boost::simd::Log_2hi;
  using boost::simd::Eps;
  long double L_2 = 0.693147180559945309417232121458;
  STF_LESS((double)((long double)(Log_2lo<double>())+(long double)(Log_2hi<double>()))- L_2, Eps<double>()/2);
  #endif
  STF_TYPE_IS(decltype(Log_2lo<T>()), T);
  STF_IEEE_EQUAL(Log_2lo<T>(), ref);
  STF_IEEE_EQUAL(log_2lo( as(T{}) ), ref);
}

STF_CASE_TPL( "Check log_2lo behavior for float"
            , (float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log_2lo;
  using boost::simd::Log_2lo;
  using boost::simd::Log_2hi;
  using boost::simd::Log_2;
  using boost::simd::Eps;
  STF_LESS(float(double(Log_2lo<float>())+double(Log_2hi<float>())- Log_2<double>()), Eps<float>());
  T ref = T(-2.1219444170128554e-4);
  STF_TYPE_IS(decltype(Log_2lo<T>()), T);
  STF_IEEE_EQUAL(Log_2lo<T>(), ref);
  STF_IEEE_EQUAL(log_2lo( as(T{}) ), ref);
}


/*
LICENSE: see isogeometric_application/LICENSE.txt
*/

//
//   Project Name:        Kratos
//   Last modified by:    $Author: hbui $
//   Date:                $Date: Nov 11, 2017 $
//   Revision:            $Revision: 1.1 $
//
//

// System includes
#include <string>

// External includes
#include <boost/python.hpp>

// Project includes
#include "includes/define.h"
#include "containers/variable.h"
#include "custom_utilities/control_point.h"
#include "custom_python/add_control_point_to_python.h"

namespace Kratos
{

namespace Python
{

using namespace boost::python;

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

inline double ControlPoint_GetX(ControlPoint<double>& rDummy)
{
    return rDummy.X();
}

inline double ControlPoint_GetWX(ControlPoint<double>& rDummy)
{
    return rDummy.WX();
}

inline void ControlPoint_SetWX(ControlPoint<double>& rDummy, const double& newWX)
{
    rDummy.WX() = newWX;
}

inline double ControlPoint_GetY(ControlPoint<double>& rDummy)
{
    return rDummy.Y();
}

inline double ControlPoint_GetWY(ControlPoint<double>& rDummy)
{
    return rDummy.WY();
}

inline void ControlPoint_SetWY(ControlPoint<double>& rDummy, const double& newWY)
{
    rDummy.WY() = newWY;
}

inline double ControlPoint_GetWZ(ControlPoint<double>& rDummy)
{
    return rDummy.WZ();
}

inline double ControlPoint_GetZ(ControlPoint<double>& rDummy)
{
    return rDummy.Z();
}

inline void ControlPoint_SetWZ(ControlPoint<double>& rDummy, const double& newWZ)
{
    rDummy.WZ() = newWZ;
}

inline double ControlPoint_GetW(ControlPoint<double>& rDummy)
{
    return rDummy.W();
}

inline void ControlPoint_SetW(ControlPoint<double>& rDummy, const double& newW)
{
    rDummy.W() = newW;
}

inline void ControlPoint_ApplyTransformation(ControlPoint<double>& rDummy, const Transformation<double>& trans)
{
    rDummy.ApplyTransformation(trans);
}

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

void IsogeometricApplication_AddControlPointToPython()
{
    class_<ControlPoint<double>, ControlPoint<double>::Pointer>
    ("ControlPoint", init<>())
    .def(init<const double&, const double&, const double&, const double&>())
    .add_property("WX", ControlPoint_GetWX, ControlPoint_SetWX)
    .add_property("WY", ControlPoint_GetWY, ControlPoint_SetWY)
    .add_property("WZ", ControlPoint_GetWZ, ControlPoint_SetWZ)
    .add_property("W", ControlPoint_GetW, ControlPoint_SetW)
    .def("ApplyTransformation", &ControlPoint_ApplyTransformation)
    .def("X", &ControlPoint_GetX)
    .def("Y", &ControlPoint_GetY)
    .def("Z", &ControlPoint_GetZ)
    .def(self_ns::str(self))
    ;

    class_<Variable<ControlPoint<double> >, bases<VariableData>, boost::noncopyable >( "ControlPointVariable", no_init )
    .def( self_ns::str( self ) )
    ;
}

}  // namespace Python.

} // Namespace Kratos


#include <boost/test/unit_test.hpp>

#include "../math/math.h"

USING_NAMESPACE(sway)

BOOST_AUTO_TEST_SUITE(TPointTestSuite)

/*!
 * Убеждаемся, что конструктор по умолчанию приводит все компоненты к нулю.
 */
BOOST_AUTO_TEST_CASE(TPointTestCase_DefaultConstructor) {
	const math::TPoint<s32> point;

	BOOST_CHECK_EQUAL(point.getX(), 0);
	BOOST_CHECK_EQUAL(point.getY(), 0);
}

/*!
 * Убеждаемся, что конструктор устанавливает все значения компонентов в те,
 * которые были заданы.
 */
BOOST_AUTO_TEST_CASE(TPointTestCase_ComponentConstructor) {
	const s32 x = 1, y = 2;
	const math::TPoint<s32> point(x, y);

	BOOST_CHECK_EQUAL(point.getX(), x);
	BOOST_CHECK_EQUAL(point.getY(), y);
}

BOOST_AUTO_TEST_CASE(TPointTestCase_ScalarConstructor) {
	const s32 x = 1, y = 2;
	const math::TPoint<s32> size(math::TPoint<s32>(x, y));

	BOOST_CHECK_EQUAL(size.getX(), x);
	BOOST_CHECK_EQUAL(size.getY(), y);
}

/*!
 * Тест для оператора равенства.
 */
BOOST_AUTO_TEST_CASE(TPointTestCase_EqualityOperator1) {
	const math::TPoint<s32> point1(0, 0), point2(0, 0);

	BOOST_CHECK_EQUAL(point1 == point2, true);
}

/*!
 * Тест для оператора равенства.
 */
BOOST_AUTO_TEST_CASE(TPointTestCase_EqualityOperator2) {
	const math::TPoint<s32> point1(1, 0), point2(0, 0);

	BOOST_CHECK_EQUAL(point1 == point2, false);
}

/*!
 * Тест для оператора равенства.
 */
BOOST_AUTO_TEST_CASE(TPointTestCase_EqualityOperator3) {
	const math::TPoint<s32> point1(0, 0), point2(0, 2);

	BOOST_CHECK_EQUAL(point1 == point2, false);
}

/*!
 * Тест для оператора неравенства.
 */
BOOST_AUTO_TEST_CASE(TPointTestCase_NonEqualityOperator1) {
	const math::TPoint<s32> point1(1, 2), point2(1, 2);

	BOOST_CHECK_EQUAL(point1 != point2, false);
}

/*!
 * Тест для оператора неравенства.
 */
BOOST_AUTO_TEST_CASE(TPointTestCase_NonEqualityOperator2) {
	const math::TPoint<s32> point1(1, 2), point2(0, 2);

	BOOST_CHECK_EQUAL(point1 != point2, true);
}

/*!
 * Тест для оператора неравенства.
 */
BOOST_AUTO_TEST_CASE(TPointTestCase_NonEqualityOperator3) {
	const math::TPoint<s32> point1(1, 2), point2(1, 0);

	BOOST_CHECK_EQUAL(point1 != point2, true);
}

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MODULE Utils

#include "cunits.hpp"
#include "sunits.hpp"
#include "utils.hpp"

#include "sample_graphs.hpp"

#include <boost/test/unit_test.hpp>

#include <random>
#include <set>
#include <tuple>
#include <vector>

using namespace std;

BOOST_AUTO_TEST_CASE(get_random_int_test)
{
  std::default_random_engine eng;
  for(int i = 0; i < 100; ++i)
    for(int j = 0; j < 10; ++j)
      {
	int n = get_random_int(0, j, eng);
	BOOST_CHECK(0 <= n && n <= j);
      }
}

BOOST_AUTO_TEST_CASE(find_path_su_test)
{
  graph g;
  vector<vertex> vs;
  vector<edge> es;
  sample_graph1(g, vs, es);

  // The su of an empty path.
  SU su1 = find_path_su(g, path());
  BOOST_CHECK(su1.empty());

  // The su of es[0].
  SU su2 = find_path_su(g, path{es[0]});
  BOOST_CHECK((su2 == SU{{0, 2}}));

  // The su of {es[0], es[1]}.
  SU su3 = find_path_su(g, path{es[0], es[1]});
  BOOST_CHECK((su3 == SU{{1, 2}}));
}


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

//#define TIMERS
//#define TRACE_EVALUATOR

#include "../lib/evaluator.h"
#include "../lib/border_system_creator.h"
#include "../lib/layer_partitioning_creator.h"
#include "../lib/classifier_tmc.h"

BOOST_AUTO_TEST_CASE( tmc_paper_dataset_do_one_step ) {
    const std::string names_file("data/tmc_paper/tmc_paper.names");
    const std::string data_file("data/tmc_paper/tmc_paper.data");
    BOOST_TEST_MESSAGE("Creating sample from file: " << data_file);
    std::shared_ptr<FeatureNames> pFN = std::make_shared<FeatureNames>(names_file);

    SampleCreator sample_creator;
    sample_creator.set_feature_names(pFN);

    std::shared_ptr<Sample> pSample = sample_creator.from_file(data_file);

    BOOST_CHECK_EQUAL(pSample->dim(), 2);
    BOOST_CHECK_EQUAL(pSample->size(), 992);
    std::shared_ptr<LayerPartitioningCreator> pLayerPartitioningCreator = std::make_shared<LayerPartitioningCreator>();
    pLayerPartitioningCreator->push_back(pSample);
    BOOST_CHECK_EQUAL(pLayerPartitioningCreator->get_layer_partitioning()->size(), 1);
    BOOST_CHECK_EQUAL(pLayerPartitioningCreator->get_layer_partitioning()->consistent(), true);
    pLayerPartitioningCreator->do_one_step();
    BOOST_CHECK_EQUAL(pLayerPartitioningCreator->get_layer_partitioning()->size(), 2);
    BOOST_CHECK_EQUAL(pLayerPartitioningCreator->get_layer_partitioning()->consistent(), true);
    pLayerPartitioningCreator->do_one_step();
    BOOST_CHECK_EQUAL(pLayerPartitioningCreator->get_layer_partitioning()->size(), 3);
    BOOST_CHECK_EQUAL(pLayerPartitioningCreator->get_layer_partitioning()->consistent(), true);
    pLayerPartitioningCreator->do_one_step();
    BOOST_CHECK_EQUAL(pLayerPartitioningCreator->get_layer_partitioning()->size(), 4);
    BOOST_CHECK_EQUAL(pLayerPartitioningCreator->get_layer_partitioning()->consistent(), true);
}

BOOST_AUTO_TEST_CASE( tmc_paper_dataset_optimize ) {
    const std::string names_file("data/tmc_paper/tmc_paper.names");
    const std::string data_file("data/tmc_paper/tmc_paper.data");
    const std::string eval_file("data/tmc_paper/tmc_paper.test");
    BOOST_TEST_MESSAGE("Creating train sample from file: " << data_file);
    BOOST_TEST_MESSAGE("Creating evaluation sample from file: " << eval_file);

    std::shared_ptr<FeatureNames> pFN = std::make_shared<FeatureNames>(names_file);

    SampleCreator sample_creator;
    sample_creator.set_feature_names(pFN);

    std::shared_ptr<Sample> pSample = sample_creator.from_file(data_file);
    std::shared_ptr<Sample> pSampleEval = sample_creator.from_file(eval_file);

    BOOST_CHECK_EQUAL(pSample->dim(), 2);
    BOOST_CHECK_EQUAL(pSample->size(), 992);
    BOOST_CHECK_EQUAL(pSampleEval->size(), 992);
    std::shared_ptr<LayerPartitioningCreator> pLayerPartitioningCreator = std::make_shared<LayerPartitioningCreator>();
    pLayerPartitioningCreator->push_back(pSample);

    pLayerPartitioningCreator->optimize();
    BOOST_CHECK_EQUAL(pLayerPartitioningCreator->get_layer_partitioning()->size(), 29);
    BOOST_CHECK_EQUAL(pLayerPartitioningCreator->get_layer_partitioning()->consistent(), true);

    auto pEvaluator = std::make_shared<Evaluator>();
    std::pair<std::pair<double, double>, std::pair<double, double>> confusion_matrix_train, confusion_matrix_eval;
    double roc_err_train, roc_err_eval, err_rate_train, err_rate_eval;

    const auto pLP = pLayerPartitioningCreator->get_layer_partitioning();
    const auto pBSC = std::make_shared<BorderSystemCreator>();
    const auto pBS = pBSC->from_layer_partitioning(pLP);
    (*pEvaluator).set_classifier(std::make_shared<ClassifierTmc>(pBS));

    (*pEvaluator).set_sample(pSample);
    confusion_matrix_train = pEvaluator->get_confusion_matrix();
    roc_err_train = pEvaluator->get_roc_error();
    err_rate_train = pEvaluator->get_error_rate();

    (*pEvaluator).set_sample(pSampleEval);
    confusion_matrix_eval = pEvaluator->get_confusion_matrix();
    roc_err_eval = pEvaluator->get_roc_error();
    err_rate_eval = pEvaluator->get_error_rate();

    BOOST_TEST_MESSAGE("");
    BOOST_TEST_MESSAGE("################################################");
    BOOST_TEST_MESSAGE("Train conf matr ### Eval conf matr");
    BOOST_TEST_MESSAGE(confusion_matrix_train.first.first << "  " << confusion_matrix_train.first.second << "    ###    "
                                                          << confusion_matrix_eval.first.first << "   " << confusion_matrix_eval.first.second);
    BOOST_TEST_MESSAGE(confusion_matrix_train.second.first << "  " << confusion_matrix_train.second.second << "    ###    "
                                                           << confusion_matrix_eval.second.first << "   " << confusion_matrix_eval.second.second);
    BOOST_TEST_MESSAGE("------------------------------------------------");
    BOOST_TEST_MESSAGE("Train rank err ### Eval rank err");
    BOOST_TEST_MESSAGE( roc_err_train << "    ###    " << roc_err_eval);
    BOOST_TEST_MESSAGE("------------------------------------------------");
    BOOST_TEST_MESSAGE("Train err rate ### Eval err rate");
    BOOST_TEST_MESSAGE( err_rate_train << "    ###    " << err_rate_eval);
    BOOST_TEST_MESSAGE("################################################");

    BOOST_CHECK_EQUAL(roc_err_train, 0.070182427159209151);
    BOOST_CHECK_EQUAL(roc_err_eval, 0.11113911290322581);
    std::pair<std::pair<double, double>, std::pair<double, double>> confusion_matrix_train_({{415,65},{81,431}});
    BOOST_CHECK(confusion_matrix_train == confusion_matrix_train_);
    std::pair<std::pair<double, double>, std::pair<double, double>> confusion_matrix_eval_({{391,79.5},{105,416.5}});
    BOOST_CHECK(confusion_matrix_eval == confusion_matrix_eval_);
}

BOOST_AUTO_TEST_CASE( tmc_paper_dataset_evaluation ) {
    const std::string names_file("data/tmc_paper/tmc_paper.names");
    const std::string data_file("data/tmc_paper/tmc_paper.data");
    const std::string eval_file("data/tmc_paper/tmc_paper.test");
    BOOST_TEST_MESSAGE("Creating train sample from file: " << data_file);
    BOOST_TEST_MESSAGE("Creating evaluation sample from file: " << eval_file);

    std::shared_ptr<FeatureNames> pFN = std::make_shared<FeatureNames>(names_file);

    SampleCreator sample_creator;
    sample_creator.set_feature_names(pFN);

    std::shared_ptr<Sample> pSample = sample_creator.from_file(data_file);
    std::shared_ptr<Sample> pSampleEval = sample_creator.from_file(eval_file);

    BOOST_CHECK_EQUAL(pSample->dim(), 2);
    BOOST_CHECK_EQUAL(pSample->size(), 992);
    BOOST_CHECK_EQUAL(pSampleEval->size(), 992);
    std::shared_ptr<LayerPartitioningCreator> pLayerPartitioningCreator = std::make_shared<LayerPartitioningCreator>();
    pLayerPartitioningCreator->push_back(pSample);
    auto pEvaluator = std::make_shared<Evaluator>();
    std::pair<std::pair<double, double>, std::pair<double, double>> confusion_matrix_train, confusion_matrix_eval;
    double roc_err_train, roc_err_eval, err_rate_train, err_rate_eval;
    size_t counter = 0;
    do {
        const auto pLP = pLayerPartitioningCreator->get_layer_partitioning();
        const auto pBSC = std::make_shared<BorderSystemCreator>();
        const auto pBS = pBSC->from_layer_partitioning(pLP);
        (*pEvaluator).set_classifier(std::make_shared<ClassifierTmc>(pBS));

        (*pEvaluator).set_sample(pSample);
        confusion_matrix_train = pEvaluator->get_confusion_matrix();
        roc_err_train = pEvaluator->get_roc_error();
        err_rate_train = pEvaluator->get_error_rate();

        (*pEvaluator).set_sample(pSampleEval);
        confusion_matrix_eval = pEvaluator->get_confusion_matrix();
        roc_err_eval = pEvaluator->get_roc_error();
        err_rate_eval = pEvaluator->get_error_rate();

        BOOST_TEST_MESSAGE("");
        BOOST_TEST_MESSAGE("############      Iteration " << counter++ << "       ############");
        BOOST_TEST_MESSAGE("Train conf matr ### Eval conf matr");
        BOOST_TEST_MESSAGE(confusion_matrix_train.first.first << "  " << confusion_matrix_train.first.second << "    ###    "
                                                              << confusion_matrix_eval.first.first << "   " << confusion_matrix_eval.first.second);
        BOOST_TEST_MESSAGE(confusion_matrix_train.second.first << "  " << confusion_matrix_train.second.second << "    ###    "
                                                               << confusion_matrix_eval.second.first << "   " << confusion_matrix_eval.second.second);
        BOOST_TEST_MESSAGE("------------------------------------------------");
        BOOST_TEST_MESSAGE("Train rank err ### Eval rank err");
        BOOST_TEST_MESSAGE( roc_err_train << "    ###    " << roc_err_eval);
        BOOST_TEST_MESSAGE("------------------------------------------------");
        BOOST_TEST_MESSAGE("Train err rate ### Eval err rate");
        BOOST_TEST_MESSAGE( err_rate_train << "    ###    " << err_rate_eval);
        BOOST_TEST_MESSAGE("################################################");

    }
    while (pLayerPartitioningCreator->do_one_step());
    BOOST_CHECK_EQUAL(roc_err_train, 0.070182427159209151);
    BOOST_CHECK_EQUAL(roc_err_eval, 0.11113911290322581);
    std::pair<std::pair<double, double>, std::pair<double, double>> confusion_matrix_train_({{415,65},{81,431}});
    BOOST_CHECK(confusion_matrix_train == confusion_matrix_train_);
    std::pair<std::pair<double, double>, std::pair<double, double>> confusion_matrix_eval_({{391,79.5},{105,416.5}});
    BOOST_CHECK(confusion_matrix_eval == confusion_matrix_eval_);
}


/*
This file is a part of Raman-Scattering-Code-Conversion.
<https://github.com/Kirbologist/Raman-Scattering-Code-Conversion>

Written by Siwan Li for the UQ School of Maths and Physics.
Based on the SMARTIES MATLAB package by W.R.C. Somerville, B. Auguié, E.C. Le Ru
Copyright (C) 2021-2022 Siwan Li

This source code form is subject to the terms of the MIT License.
If a copy of the MIT License was not distributed with this file,
you can obtain one at <https://opensource.org/licenses/MIT>.

This code contains all common or basic header files, typedefs, enums and structs used by the rest
of the SMARTIES code. Typedefs declared here are based on existing Eigen typedefs.
The -r suffix or 'Real' class usually denotes a scalar type that's used to represent real numbers.
The -c suffix denotes that each element is the complex extension of 'Real'.
*/

#ifndef SMARTIES_CORE_H
#define SMARTIES_CORE_H

#include <cassert>
#include <cmath>
#include <complex>
#include <iostream>
#include <string>
#include <memory>
#include <Eigen/Core>
#include <Eigen/CXX11/Tensor>

using namespace std;
using namespace Eigen;

namespace Smarties {
  using RowArrayXi = Array<int, 1, Dynamic>;

  using ArrayXb = Array<bool, Dynamic, 1>;

  using RowArrayXb = Array<bool, 1, Dynamic>;

  using RowArrayXf = Array<float, 1, Dynamic>;

  using RowArrayXd = Array<double, 1, Dynamic>;

  template <class Real>
  using ArrayXXr = Array<Real, Dynamic, Dynamic>;

  template <class Real>
  using ArrayXr = Array<Real, Dynamic, 1>;

  template <class Real>
  using RowArrayXr = Array<Real, 1, Dynamic>;

  template <class Real>
  using ArrayXXc = Array<complex<Real>, Dynamic, Dynamic>;

  template <class Real>
  using ArrayXc = Array<complex<Real>, Dynamic, 1>;

  template <class Real>
  using RowArrayXc = Array<complex<Real>, 1, Dynamic>;

  template <class Real>
  using MatrixXr = Matrix<Real, Dynamic, Dynamic>;

  template <class Real>
  using MatrixXc = Matrix<complex<Real>, Dynamic, Dynamic>;

  template <class Real>
  using VectorXr = Matrix<Real, Dynamic, 1>;

  template <class Real>
  using VectorXc = Matrix<complex<Real>, Dynamic, 1>;

  template <class Real>
  using Tensor3r = Tensor<Real, 3>;

  template <class Real>
  using Tensor3c = Tensor<complex<Real>, 3>;

  template <class Real>
  using Tensor4c = Tensor<complex<Real>, 4>;

  template <class Real>
  using Tensor1c = Tensor<complex<Real>, 1>;

  /*
  Used to describe the incident type of a photon in an electric field. The first x,y,z refers to
  wavevector direction and the second x,y,z (necessarily different to the first) is the direction of the
  linearly-polarised electric field. Otherwise, GENERAL if incident type can't be defined as such.
  */
  enum sIncType {Kx_Ez, Kx_Ey, Ky_Ex, Ky_Ez, Kz_Ex, Kz_Ey, GENERAL};

  /* Struct with parameters of an incident electric field */
  template<class Real>
  struct stIncPar {
    sIncType type;
    Real theta_p;
    Real phi_p;
    Real alpha_p;
    ArrayXi abs_m_vec;

    stIncPar() {};

    stIncPar(const stIncPar<Real>& base) {
      this->type = base.type;
      this->theta_p = base.theta_p;
      this->phi_p = base.phi_p;
      this->alpha_p = base.alpha_p;
      this->abs_m_vec = base.abs_m_vec;
    }
  };

  /*
  Various parameters used throughout SMARTIES functions to describe
  incident spheroids, electric fields and T matrices
  */
  template <class Real>
  struct stParams {
    Real a; // Spheroid semi-axis radius along x,y.
    Real c; // Spheroid semi-axis radius along z.
    ArrayXr<Real> k1; // Wavevector in embedding medium (possibly a wavelength-dependent vector)
    ArrayXr<Real> s; // Relative-refractive index
    int N; // The number of multipoles requested for the T (and R) matrix. The higher it is, the better the convergence
    int Nb_theta; // Number of angles used in Gaussian quadratures for the evaluation of P and Q matrix integrals
    sIncType inc_type;
    unique_ptr<stIncPar<Real>> inc_par;
    int Nb_theta_pst; // Number of angles for post-processing (should typically be larger than Nb_theta)
    ArrayXr<Real> lambda; // Wavelength (in free space. Same unit as a and c)
    Real epsilon1; // Relative dielectric constant of embedding medium (real positive)
    ArrayXr<Real> epsilon2; // Dielectric function of scatterer (may be complex, but that hasn't been implemented)
    bool output = true; // Originally not in the specification of User Guide, but some functions still use this anyway
  };

  /* Optional settings */
  struct stOptions {
    bool get_R = false; // Calculate R matrices and internal field coefficients
    int delta = 0; // Number of extra multipoles for P and Q matrices. If -1, sphEstimateDelta gets called
    int NB = 0; // Number of multipoles to compute the Bessel functions (NB >= N + delta)
    ArrayXi abs_m_vec; // Vector of values of abs(m) for which the T matrix is to be computed (0 <= abs(m) <= N)
    bool get_symmetric_T = false; // If true, T matrix is symmetrised
    bool output = true; // If true, prints some output to the terminal

    stOptions() {};

    stOptions(int N) {
      this->abs_m_vec = ArrayXi::LinSpaced(N + 1, 0, N); // Values of m in most cases of interest
    }
  };
}

#endif


/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt is distributed in the hope that it will be useful, but
   WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#include <boost/numeric/ublas/matrix_proxy.hpp>  // For row()

#include "bayesopt/bayesopt.h"               // For the C API
#include "bayesopt/bayesopt.hpp"             // For the C++ API
#include "lhs.hpp"
#include "specialtypes.hpp"

/* Function to be used for C-API testing */
double testFunction(unsigned int n, const double *x,
		    double *gradient, /* NULL if not needed */
		    void *func_data)
{
  double f = 10.;
  for (unsigned int i = 0; i < n; ++i)
    {
      f += (x[i] - .53) * (x[i] - .53);
    }
  return f;
}

/* Class to be used for C++-API testing */
class ExampleDisc: public bayesopt::DiscreteModel
{
 public:

  ExampleDisc(const vecOfvec & validSet, bayesopt::Parameters param):
    DiscreteModel(validSet,param) {}

  double evaluateSample( const vectord &Xi )
  {
    double x[100];
    for (size_t i = 0; i < Xi.size(); ++i)
      {
	x[i] = Xi(i);
      }
    return testFunction(Xi.size(),x,NULL,NULL);
  };

  bool checkReachability( const vectord &query )
  { return true; };
};

int main(int nargs, char *args[])
{
  // Common configuration
  // See parameters.h for the available options
  // This is just an example of what can be done.
  bayesopt::Parameters par;

  par.surr_name = "sStudentTProcessJef";
  par.noise = 1e-10;
  par.n_iterations = 70;       // Number of iterations
  par.n_init_samples = 20;
  /*******************************************/

  const size_t nPoints = 1000;          // Number of discrete points
  const size_t n = 6;                   // Number of dimensions

  // Let's generate a discrete grid by latin hypercube sampling in the n-dimensional
  // space.
  randEngine mtRandom;
  matrixd xPoints(nPoints,n);

  bayesopt::utils::lhs(xPoints,mtRandom);

  // Set of discrete points using C arrays.
  // Thanks to the quirks of Visual Studio, the following expression is invalid, so we
  // have to replace it by a literal.  WARNING: Make sure to update the size of the array
  // if the number of points or dimensions change.
#ifdef _MSC_VER
  double xPointsArray[6000];
#else
  const size_t nPinArr = n*nPoints;
  double xPointsArray[nPinArr];
#endif

  // Set of discrete points using ublas vectors. For the C++ interface.
  vecOfvec xP;

  // Lets copy the generated points to the specific structure for each interface.
  for(size_t i = 0; i<nPoints; ++i)
    {
      vectord point = row(xPoints,i);
      xP.push_back(point);
      for(size_t j=0; j<n; ++j)
	{
	  xPointsArray[i*n+j] = point(j);
	}
    }

  /*******************************************/
  // Run C++ interface
  std::cout << "Running C++ interface" << std::endl;
  ExampleDisc opt(xP,par);
  vectord result(n);
  opt.optimize(result);

  // Run C interface
  std::cout << "Running C interface" << std::endl;
  double x[128], fmin[128];
  bayes_optimization_disc(n, &testFunction, NULL, xPointsArray, nPoints,
			  x, fmin, par.generate_bopt_params());

  // Find the optimal value by brute force
  size_t min = 0;
  double minvalue = opt.evaluateSample(row(xPoints,min));
  for(size_t i = 1; i<nPoints; ++i)
    {
      vectord point = row(xPoints,i);
      if (opt.evaluateSample(point) < minvalue)
	{
	  min = i;
	  minvalue = opt.evaluateSample(row(xPoints,min));
	  std::cout << i << "," << minvalue << std::endl;
	}
    }

  std::cout << "Final result C++: " << result << " | Value:" << opt.evaluateSample(result) << std::endl;
  std::cout << "Final result C: ["<< n << "](";
  for (int i = 0; i < n; i++ )
    std::cout << x[i] << ", ";
  std::cout << ")" << " | Value:" << fmin[0] << std::endl;
  std::cout << "Optimal: " << row(xPoints,min) << " | Value:" << minvalue << std::endl;
}


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

#include <armadillo>
#include "catch.hpp"

using namespace arma;

TEST_CASE("fn_any_1")
  {
  vec a(5, fill::zeros);
  vec b(5, fill::zeros);  b(0) = 1.0;
  vec c(5, fill::ones );

  REQUIRE( any(a) == false);
  REQUIRE( any(b) == true );
  REQUIRE( any(c) == true );

  REQUIRE( any(a(span::all)) == false);
  REQUIRE( any(b(span::all)) == true );
  REQUIRE( any(c(span::all)) == true );

  REQUIRE( any(  c -  c) == false);
  REQUIRE( any(2*c -2*c) == false);

  REQUIRE( any(c < 0.5) == false);
  REQUIRE( any(c > 0.5) == true );
  }

TEST_CASE("fn_any_2")
  {
  mat A(5, 6, fill::zeros);
  mat B(5, 6, fill::zeros);  B(0,0) = 1.0;
  mat C(5, 6, fill::ones );

  REQUIRE( any(vectorise(A)) == false);
  REQUIRE( any(vectorise(B)) == true );
  REQUIRE( any(vectorise(C)) == true );

  REQUIRE( any(vectorise(A(span::all,span::all))) == false);
  REQUIRE( any(vectorise(B(span::all,span::all))) == true );
  REQUIRE( any(vectorise(C(span::all,span::all))) == true );

  REQUIRE( any(vectorise(  C -  C)) == false);
  REQUIRE( any(vectorise(2*C -2*C)) == false);

  REQUIRE( any(vectorise(C) < 0.5) == false);
  REQUIRE( any(vectorise(C) > 0.5) == true );
  }

TEST_CASE("fn_any_3")
  {
  mat A(5, 6, fill::zeros);
  mat B(5, 6, fill::zeros);  B(0,0) = 1.0;
  mat C(5, 6, fill::ones );
  mat D(5, 6, fill::ones );  D(0,0) = 0.0;

  REQUIRE( accu( any(A)   == urowvec({0, 0, 0, 0, 0, 0}) ) == 6 );
  REQUIRE( accu( any(A,0) == urowvec({0, 0, 0, 0, 0, 0}) ) == 6 );
  REQUIRE( accu( any(A,1) == uvec   ({0, 0, 0, 0, 0}   ) ) == 5 );

  REQUIRE( accu( any(B)   == urowvec({1, 0, 0, 0, 0, 0}) ) == 6 );
  REQUIRE( accu( any(B,0) == urowvec({1, 0, 0, 0, 0, 0}) ) == 6 );
  REQUIRE( accu( any(B,1) == uvec   ({1, 0, 0, 0, 0}   ) ) == 5 );

  REQUIRE( accu( any(C)   == urowvec({1, 1, 1, 1, 1, 1}) ) == 6 );
  REQUIRE( accu( any(C,0) == urowvec({1, 1, 1, 1, 1, 1}) ) == 6 );
  REQUIRE( accu( any(C,1) == uvec   ({1, 1, 1, 1, 1}   ) ) == 5 );

  REQUIRE( accu( any(D)   == urowvec({1, 1, 1, 1, 1, 1}) ) == 6 );
  REQUIRE( accu( any(D,0) == urowvec({1, 1, 1, 1, 1, 1}) ) == 6 );
  REQUIRE( accu( any(D,1) == uvec   ({1, 1, 1, 1, 1}   ) ) == 5 );
  }