#include <cassert>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <iostream>
#include <iterator>
#include <map>
#include <set>
#include <sstream>
#include <string>
#include <vector>

#include <Eigen/Core>

#include "igl/AABB.h"
#include "igl/adjacency_list.h"
#include "igl/adjacency_matrix.h"
#include "igl/ambient_occlusion.h"
#include "igl/components.h"
#include "igl/cotmatrix.h"
#include "igl/decimate.h"
#include "igl/dijkstra.h"
#include "igl/embree/ambient_occlusion.h"
#include "igl/embree/EmbreeIntersector.h"
#include "igl/embree/unproject_onto_mesh.h"
#include "igl/gaussian_curvature.h"
#include "igl/invert_diag.h"
#include "igl/jet.h"
#include "igl/per_vertex_attribute_smoothing.h"
#include "igl/per_vertex_normals.h"
#include "igl/polygon_mesh_to_triangle_mesh.h"
#include "igl/principal_curvature.h"
#include "igl/ray_mesh_intersect.h"
#include "igl/readOFF.h"
#include "igl/unproject_onto_mesh.h"
#include "igl/viewer/Viewer.h"
#include "igl/writeOFF.h"

#include "GLFW/glfw3.h"

#include "dijkstra.hxx"

const double mOCC_MAX_RAY_DIST = 10.0;
const int mOCC_NUM_RAYS = 20;
const double mDECIMATE_PERC = 0.6;

enum SelectionType { CURVATURE, DARKNESS };

// Split a string into words.
std::vector<std::string> split(const std::string &input) {
  using namespace std;
  istringstream iss(input);
  vector<string> tokens{istream_iterator<string>{iss},
                        istream_iterator<string>{}};
  return tokens;
}

std::string prettyprint(const std::string &input, int width, int trailing_spaces) {
  using namespace std;
  vector<string> words = split(input);

  string ret;
  int cur_width = 0;
  for (const string &s : words) {
    if (cur_width + s.size() > width) {
      ret += "\n";
      for (int i = 0; i < trailing_spaces; ++i) {
        ret += " ";
      }
      cur_width = 0;
    }
    ret += s + " ";
    cur_width += s.size() + 1;
  }

  return ret;
}

void buildChoiceSide(std::vector<std::string> &prompts,
                     std::vector<SelectionType> &types,
                     std::string side) {
  using namespace std;
  prompts.push_back(string("Insert points along the squamous suture [patient ")
                    + side + "]");
  types.push_back(DARKNESS);
  types.push_back(CURVATURE);
  prompts.push_back(string("Insert points from the squamous x lambdoid sutures "
                           "to the tip of the mastoid process [patient ")
                    + side + "]");
  types.push_back(CURVATURE);
  prompts.push_back(string("Insert points from the tip of the mastoid process "
                           "along the zygomatic bone to the top of the skull "
                           "[patient ")
                    + side + "]");
}

void buildChoiceTop(std::vector<std::string> &prompts,
                    std::vector<SelectionType> &types) {
  using namespace std;

  prompts.push_back("Insert points along the coronal suture, "
                    "starting at the sphenoid bone on one side, "
                    "and ending at the sphenoid bone on the other side.");
  types.push_back(DARKNESS);
  prompts.push_back("Insert points along the sagittal suture.");
  types.push_back(DARKNESS);
  prompts.push_back("Insert points along the lambdoid suture");
  types.push_back(DARKNESS);
}

void buildChoiceFront(std::vector<std::string> &prompts,
                      std::vector<SelectionType> &types) {
  prompts.push_back("Insert points along the chin, starting at the jaw attachment "
                    "point on one side, ending on the other side");
  types.push_back(CURVATURE);
  prompts.push_back("Insert points along the outside of the left eye");
  types.push_back(CURVATURE);
  prompts.push_back("Insert points along the outside of the right eye");
  types.push_back(CURVATURE);
  prompts.push_back("Insert points along the outside of the nose");
  types.push_back(CURVATURE);
}

void buildChoices(std::vector<std::string> &prompts,
                  std::vector<SelectionType> &types) {
  buildChoiceTop(prompts, types);
  buildChoiceSide(prompts, types, "left");
  buildChoiceSide(prompts, types, "right");
  buildChoiceFront(prompts, types);
}

void printChoice(int ptnum, const std::vector<std::string> &prompts) {
  if (ptnum >= prompts.size()) {
    printf("Press 'w' to write points to an output file\n");
  } else {
    std::string msg = prettyprint(prompts[ptnum], 74, 8);
    printf("%2d/%2d -- %s\n", ptnum + 1, prompts.size(), msg.c_str());
  }
}

// Select all points between a and b, given mesh V,F with edge weights defined by w.
// Will output indices to new_points.
void selectPointsBetween(int a, int b,
                         const Eigen::MatrixXd &V, const Eigen::MatrixXi &F,
                         const std::vector<std::vector<int> > &VV,
                         const std::vector<std::vector<double> > &w,
                         std::vector<int> &new_points) {
  // Call the Dijkstra's code.
  Eigen::VectorXd min_distance;
  Eigen::VectorXi previous;
  std::set<int> targets;
  targets.insert(b);
  int vertex_found =
      from_igl::dijkstra_compute_paths(a, targets, VV, w, min_distance,previous);
      //igl::dijkstra_compute_paths(a, targets, VV, min_distance,previous);
  if (vertex_found == -1) {
    // ERROR
    printf("ERROR: No path found!!\n");
  } else {
    igl::dijkstra_get_shortest_path_to(vertex_found, previous, new_points);
  }
}

void getMeanCurvature(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F,
                      Eigen::VectorXd &VCs) {
  Eigen::MatrixXd PD1,PD2;
  Eigen::VectorXd PV1,PV2;
  //igl::principal_curvature(V,F,PD1,PD2,PV1,PV2, 5);
  igl::principal_curvature(V,F,PD1,PD2,PV1,PV2, 8);
  //mean_curve = 0.5 * (PV1 + PV2);
  VCs = PV1.cwiseProduct(PV1) + PV2.cwiseProduct(PV2);
  VCs = VCs.cwiseMin(0.1);
  VCs /= VCs.maxCoeff();

  Eigen::VectorXd ones = Eigen::VectorXd::Constant(VCs.rows(), 1, 1);
  VCs = ones - VCs;

  /*
  // Get the laplacian
  Eigen::SparseMatrix<double> L,M,Minv;
  //   from the cotangent matrix
  igl::cotmatrix(V,F, L);
  //   and the mass matrix
  igl::massmatrix(V,F, igl::MASSMATRIX_TYPE_VORONOI, M);
  igl::invert_diag(M, Minv);

  Eigen::MatrixXd HN = -Minv*(L * V);
  // Mean curviture is this value.
  Eigen::VectorXd VC = HN.rowwise().norm();

  // Smooth this out a bit.
  igl::per_vertex_attribute_smoothing(VC,F, VCs);
  // Cutoff at 1 (to avoid overpowering by high curvature)
  VCs = VCs.cwiseMin(1.0);

  // Normalize
  VCs /= VCs.maxCoeff();
  // Invert so high curvature has low weight.
  Eigen::VectorXd ones = Eigen::VectorXd::Constant(VCs.rows(), 1, 1);
  // Sqrt before switching
  VCs = VCs.cwiseSqrt();
  VCs = ones - VCs;
  //std::cout << VCs;
  */

}

void printHelp(int ptnum) {
  printf("Press 'u' to undo previous selection\n");
  printf("Press '[' or ']' to adjust scene lighting\n");
  printf("Press 'c' to show colors for curvature\n");
  printf("Press 'd' to switch back to current selected points\n");
  printf("Press ' ' (space) to complete the current task\n");
  printf("Press '?' to repeat this message (and the requested user input).\n");
}

int main(int argc, char* argv[]) {
  if (argc < 3) {
    fprintf(stderr, "Used for selecting points on the surface of a mesh. Will write the\n"
                    "corresponding vertices to output.off\n");
    fprintf(stderr, "usage: %s <input.off> <output.off> [decimate perc]\n",
            argv[0]);
    return -1;
  }

  Eigen::MatrixXd V,V2;
  Eigen::MatrixXi F,F2;
  igl::embree::EmbreeIntersector ei;

  printf("Reading in mesh...\n");
  // Read in the input file.
  igl::readOFF(argv[1], V, F);

  if (argc == 4) {
    float dec_perc = atof(argv[3]);
    int new_faces = dec_perc * F.rows();
    printf("Decimating by %f to %d faces...\n", dec_perc, new_faces);
    // First things first: decimate the mesh
    Eigen::VectorXi J;
    igl::decimate(V,F, new_faces, V2,F2,J);
    printf("Decimated from V:%d,F:%d to V:%d,F:%d\n",
           V.rows(), F.rows(), V2.rows(), F2.rows());
    V = V2; F = F2;
  }

  // Colors are all white to start.
  Eigen::MatrixXd VC = Eigen::MatrixXd::Constant(V.rows(),3,1);
  // Initialize this thing so we can get the vertices.
  ei.init(V.cast<float>(), F);

  // Ambient occlusion
  Eigen::VectorXd AO;
  Eigen::MatrixXd N;
  igl::per_vertex_normals(V,F,N);
  // Compute ambient occlusion factor using embree
  printf("Building AABB tree...\n");
  igl::AABB<Eigen::MatrixXd, 3> aabb;
  aabb.init(V, F);
  const auto & shoot_ray = [&aabb,&V,&F](
      const Eigen::Vector3f& _s,
      const Eigen::Vector3f& dir)->bool
  {
    Eigen::Vector3f s = _s+1e-4*dir;
    igl::Hit hit;
    if (aabb.intersect_ray(V,F, s.cast<double>().eval(),dir.cast<double>().eval(), hit)) {
      return hit.t < mOCC_MAX_RAY_DIST;
    }
    return false;
  };

  printf("Now setting ambient occlusion vertices to improve contrast (patience)...\n");
  //igl::embree::ambient_occlusion(V,F,V,N,300,AO);
  igl::ambient_occlusion(shoot_ray, V,N, mOCC_NUM_RAYS,AO);
  AO = 1.0 - AO.array();
  for (int i = 0; i < V.rows(); ++i) {
    VC.row(i) *= AO(i);
  }

  printf("Constructing adjacency list...\n");
  // Need the adjacency list
  std::vector<std::vector<int> > VV;
  igl::adjacency_list(F, VV);

  std::vector<std::vector<double> > darkWeight;
  darkWeight.resize(VV.size());
  //std::vector<std::vector<double> > curveWeight;
  //curveWeight.resize(VV.size());
  for (int i = 0; i < VV.size(); ++i) {
    for (int j = 0; j < VV[i].size(); ++j) {
      // Just use the value of the ambient occlusion.
      darkWeight[i].push_back(AO(VV[i][j]));
      //curveWeight[i].push_back(1.0 - AO(VV[i][j]));
    }
  }

  // Also caluculate the curvature values.
  printf("Calculating mean curvature...\n");
  Eigen::VectorXd mean_curve;
  getMeanCurvature(V,F, mean_curve);
  std::vector<std::vector<double> > curveWeight;
  curveWeight.resize(VV.size());
  double maxCurve = 0;
  double minCurve = 0;
  for (int i = 0; i < VV.size(); ++i) {
    for (int j = 0; j < VV[i].size(); ++j) {
      // Difference in gausssian curvature
      //double diff = std::abs(VCs(i) - VCs(j));
      double diff = mean_curve(VV[i][j]);
      curveWeight[i].push_back(diff);
      maxCurve = std::max(diff, maxCurve);
      minCurve = std::min(diff, minCurve);
    }
  }

  // Create the colors for mean curviture to use
  Eigen::MatrixXd curve_colors;
  igl::jet(mean_curve, true, curve_colors);

  // Prompts and types
  std::vector<std::string> prompts;
  std::vector<SelectionType> types;
  buildChoices(prompts, types);

  int point_num = 0;
  std::vector<int> all_points;  // will contain all points, once the entire mesh has been selected.

  std::vector<int> selected_points;
  std::vector<int> additional_points;

  igl::viewer::Viewer viewer;
  viewer.callback_key_down = [&](igl::viewer::Viewer& viewer, unsigned char key, int modifier)->bool {
    printf("key: '%c' modifier: '%d'\n", key, modifier);
    switch(key) {
      case ']':
        viewer.core.lighting_factor += 0.1;
        printf("Turning lighting up to %f\n", viewer.core.lighting_factor);
        break;
      case '[':
        viewer.core.lighting_factor -= 0.1;
        printf("Turning lighting down to %f\n", viewer.core.lighting_factor);
        break;
      case 'U':
      {
        printf("Undoing last point...\n");
        // Undo the last choice
        int last = selected_points.back();
        printf("Undoing last choice... %d\n", last);
        selected_points.pop_back();
        VC.row(last) << 1,1,1;
        VC.row(last) *= AO(last);
        // Also remove points from additional_points
        int this_p = -1;
        while ( this_p != last && additional_points.size() > 0 ) {
          // Get the last point.
          this_p = additional_points.back();
          // Set the color
          VC.row(this_p) << 1,1,1;
          VC.row(this_p) *= AO(this_p);
          // Remove the point.
          additional_points.pop_back();
        }

        // Need to add in any colors we might have removed
        for (int i : additional_points) {
          VC.row(i) << 1,0,0;
        }
        for (int i : selected_points) {
          VC.row(i) << 1,0,0;
        }

        // Set the colors back to what they were.
        viewer.data.set_colors(VC);
        printChoice(point_num, prompts);
        break;
      }
      case ' ':
      {
        printf("Finished with current curve...Moving on to next\n");
        // Add all the points to all_points
        all_points.insert(all_points.end(),
                          additional_points.begin(),
                          additional_points.end());
        additional_points.clear();
        selected_points.clear();

        point_num++;
        printChoice(point_num, prompts);
        break;
      }
      case 'C':
        printf("Setting curvature colors...\n");
        viewer.data.set_colors(curve_colors);
        break;
      case 'D':
        printf("Setting colors to highlight darkness...\n");
        viewer.data.set_colors(VC);
        break;
      case 'W':
      {
        printf("Writing output to %s\n", argv[2]);
        // Write to output file
        FILE* ofs = fopen(argv[2], "w");
        fprintf(ofs, "OFF\n");
        fprintf(ofs, "%d 0 0\n", all_points.size());
        for (int idx : all_points) {
          fprintf(ofs, "%f %f %f\n", V(idx, 0), V(idx, 1), V(idx, 2));
        }
        fclose(ofs);
        // You can quit now.
        printf("Finished! Please close the program.\n");
        break;
      }
      default:
        // Question mark ('?')
        if (key == '/' && modifier == GLFW_MOD_SHIFT) {
          printHelp(selected_points.size());
          printChoice(point_num, prompts);
        }
    }

    // Make sure this stays the same.
    viewer.core.lighting_factor =
        std::min(std::max(viewer.core.lighting_factor,0.f),1.f);
    return false;
  };
  viewer.callback_mouse_down =
      [&](igl::viewer::Viewer &viewer, int, int)->bool
      {
        // fid will be the face ID. Need to get the vertex ID
        int fid, vid;
        // Cast a ray in the view direction starting from the mouse position
        double x = viewer.current_mouse_x;
        double y = viewer.core.viewport(3) - viewer.current_mouse_y;
        if(igl::embree::unproject_onto_mesh(Eigen::Vector2f(x,y), F,
                                            viewer.core.view * viewer.core.model,
                                            viewer.core.proj,
                                            viewer.core.viewport,
                                            ei,
                                            fid, vid)) {

          selected_points.push_back(vid);
          VC.row(vid) << 1,0,0;

          // Update all the colors
          //Eigen::MatrixXd vcol;
          //igl::jet(VC, false, vcol);
          if (selected_points.size() > 1) {
            std::vector<int> path;
            if (types[point_num] == DARKNESS) {
              printf("Using light shading...\n");
              selectPointsBetween(selected_points[selected_points.size() - 2],
                                  selected_points[selected_points.size() - 1],
                                  V,F, VV, darkWeight,
                                  path);
            } else {
              printf("Using curvature...\n");
              selectPointsBetween(selected_points[selected_points.size() - 2],
                                  selected_points[selected_points.size() - 1],
                                  V,F, VV, curveWeight,
                                  path);
            }
            printf("Setting colors of %d...\n", path.size());
            for (int i = 0; i < path.size(); ++i) {
              VC.row(path[i]) << 1,0,0;
            }
            additional_points.insert(additional_points.end(), path.begin(), path.end());
          }

          viewer.data.set_colors(VC);

          return true;
        }
        return false;
      };

  printHelp(0);
  viewer.data.set_mesh(V, F);
  viewer.data.set_colors(curve_colors);
  viewer.core.show_lines = false;
  viewer.launch();
}


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY 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.
//
// FlexibleSUSY 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 FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

#ifndef EIGEN_UTILS_H
#define EIGEN_UTILS_H

#include "compare.hpp"
#include <Eigen/Core>
#include <iomanip>
#include <sstream>
#include <string>
#include <limits>

namespace flexiblesusy {

template <typename Derived>
unsigned closest_index(double mass, const Eigen::ArrayBase<Derived>& v)
{
   unsigned pos;
   typename Derived::PlainObject tmp;
   tmp.setConstant(mass);

   (v - tmp).abs().minCoeff(&pos);

   return pos;
}

/**
 * Divides a by b element wise.  If the quotient is not finite, it is
 * set to zero.
 *
 * @param a numerator
 * @param b denominator
 */
template <typename Scalar, int M, int N>
Eigen::Matrix<Scalar,M,N> div_save(
   const Eigen::Matrix<Scalar,M,N>& a, const Eigen::Matrix<Scalar,M,N>& b)
{
   Eigen::Matrix<Scalar,M,N> result(Eigen::Matrix<Scalar,M,N>::Zero());

   for (int i = 0; i < M; i++) {
      for (int k = 0; k < N; k++) {
         const double quotient = a(i,k) / b(i,k);
         if (std::isfinite(quotient))
            result(i,k) = quotient;
      }
   }

   return result;
}

template <class BinaryOp, class Derived>
Derived binary_map(
   const Eigen::ArrayBase<Derived>& a, const Eigen::ArrayBase<Derived>& b, BinaryOp op)
{
   typename Derived::PlainObject result(a.rows(), b.cols());

   assert(a.rows() == b.rows());
   assert(a.cols() == b.cols());

   for (int i = 0; i < a.rows(); i++)
      for (int k = 0; k < a.cols(); k++)
         result(i,k) = op(a(i,k), b(i,k));

   return result;
}

/**
 * The element of v, which is closest to mass, is moved to the
 * position idx.
 *
 * @param idx new index of the mass eigenvalue
 * @param mass mass to compare against
 * @param v vector of masses
 * @param z corresponding mixing matrix
 */
template <typename DerivedArray, typename DerivedMatrix>
void move_goldstone_to(int idx, double mass, Eigen::ArrayBase<DerivedArray>& v,
                       Eigen::MatrixBase<DerivedMatrix>& z)
{
   int pos = closest_index(mass, v);
   if (pos == idx)
      return;

   const int sign = (idx - pos) < 0 ? -1 : 1;
   int steps = std::abs(idx - pos);

   // now we shuffle the states
   while (steps--) {
      const int new_pos = pos + sign;
      v.row(new_pos).swap(v.row(pos));
      z.row(new_pos).swap(z.row(pos));
      pos = new_pos;
   }
}

/**
 * Copies all elements from src to dst which are not close to the
 * elements in cmp.
 *
 * @param src source vector
 * @param cmp vector with elements to compare against
 * @param dst destination vector
 */
template<class Real, int Nsrc, int Ncmp, int Ndst>
void remove_if_equal(const Eigen::Array<Real,Nsrc,1>& src,
                     const Eigen::Array<Real,Ncmp,1>& cmp,
                     Eigen::Array<Real,Ndst,1>& dst)
{
   static_assert(Nsrc == Ncmp + Ndst,
                 "Error: remove_if_equal: vectors have incompatible length!");

   Eigen::Array<Real,Nsrc,1> non_equal(src);

   for (int i = 0; i < Ncmp; i++) {
      const int idx = closest_index(cmp(i), non_equal);
      non_equal(idx) = std::numeric_limits<double>::infinity();
   }

   std::remove_copy_if(non_equal.data(), non_equal.data() + Nsrc,
                       dst.data(), Is_not_finite<Real>());
}

/**
 * @brief reorders vector v according to ordering in vector v2
 * @param v vector with elementes to be reordered
 * @param v2 vector with reference ordering
 */
template<class Real, int N>
void reorder_vector(
   Eigen::Array<Real,N,1>& v,
   const Eigen::Array<Real,N,1>& v2)
{
   Eigen::PermutationMatrix<N> p;
   p.setIdentity();
   std::sort(p.indices().data(), p.indices().data() + p.indices().size(),
             CompareAbs<Real, N>(v2));

#if EIGEN_VERSION_AT_LEAST(3,1,4)
   v.matrix().transpose() *= p.inverse();
#else
   Eigen::Map<Eigen::Matrix<Real,N,1> >(v.data()).transpose() *= p.inverse();
#endif
}

/**
 * @brief reorders vector v according to ordering of diagonal elements in mass_matrix
 * @param v vector with elementes to be reordered
 * @param matrix matrix with diagonal elements with reference ordering
 */
template<class Derived>
void reorder_vector(
   Eigen::Array<double,Eigen::MatrixBase<Derived>::RowsAtCompileTime,1>& v,
   const Eigen::MatrixBase<Derived>& matrix)
{
   reorder_vector(v, matrix.diagonal().array().eval());
}

template<class Derived>
std::string print_scientific(const Eigen::DenseBase<Derived>& v,
                             unsigned number_of_digits = std::numeric_limits<typename Derived::Scalar>::digits10 + 1)
{
   std::ostringstream sstr;

   for (std::size_t i = 0; i < v.rows(); i++) {
      for (std::size_t k = 0; k < v.cols(); k++) {
         sstr << std::setprecision(number_of_digits)
              << std::scientific << v(i,k) << ' ';
      }
      sstr << '\n';
   }

   return sstr.str();
}

} // namespace flexiblesusy

#endif


#ifndef SPIRIT_USE_CUDA

#include <engine/Hamiltonian_Heisenberg.hpp>
#include <engine/Vectormath.hpp>
#include <engine/Neighbours.hpp>
#include <data/Spin_System.hpp>
#include <utility/Constants.hpp>

#include <Eigen/Dense>

using namespace Data;
using namespace Utility;
using Utility::Constants::mu_B;
using Utility::Constants::mu_0;
using Utility::Constants::Pi;
using Engine::Vectormath::check_atom_type;
using Engine::Vectormath::idx_from_pair;

namespace Engine
{
    // Construct a Heisenberg Hamiltonian with pairs
    Hamiltonian_Heisenberg::Hamiltonian_Heisenberg(
        scalarfield mu_s,
        scalar external_field_magnitude, Vector3 external_field_normal,
        intfield anisotropy_indices, scalarfield anisotropy_magnitudes, vectorfield anisotropy_normals,
        pairfield exchange_pairs, scalarfield exchange_magnitudes,
        pairfield dmi_pairs, scalarfield dmi_magnitudes, vectorfield dmi_normals,
        scalar ddi_radius,
        quadrupletfield quadruplets, scalarfield quadruplet_magnitudes,
        std::shared_ptr<Data::Geometry> geometry,
        intfield boundary_conditions
    ) :
        Hamiltonian(boundary_conditions),
        geometry(geometry),
        mu_s(mu_s),
        external_field_magnitude(external_field_magnitude * mu_B), external_field_normal(external_field_normal),
        anisotropy_indices(anisotropy_indices), anisotropy_magnitudes(anisotropy_magnitudes), anisotropy_normals(anisotropy_normals),
        exchange_pairs_in(exchange_pairs), exchange_magnitudes_in(exchange_magnitudes), exchange_shell_magnitudes(0),
        dmi_pairs_in(dmi_pairs), dmi_magnitudes_in(dmi_magnitudes), dmi_normals_in(dmi_normals), dmi_shell_magnitudes(0), dmi_shell_chirality(0),
        quadruplets(quadruplets), quadruplet_magnitudes(quadruplet_magnitudes),
        ddi_cutoff_radius(ddi_radius)
    {
        // Generate interaction pairs, constants etc.
        this->Update_Interactions();
    }

    // Construct a Heisenberg Hamiltonian from shells
    Hamiltonian_Heisenberg::Hamiltonian_Heisenberg(
        scalarfield mu_s,
        scalar external_field_magnitude, Vector3 external_field_normal,
        intfield anisotropy_indices, scalarfield anisotropy_magnitudes, vectorfield anisotropy_normals,
        scalarfield exchange_shell_magnitudes,
        scalarfield dmi_shell_magnitudes, int dm_chirality,
        scalar ddi_radius,
        quadrupletfield quadruplets, scalarfield quadruplet_magnitudes,
        std::shared_ptr<Data::Geometry> geometry,
        intfield boundary_conditions
    ) :
        Hamiltonian(boundary_conditions),
        geometry(geometry),
        mu_s(mu_s),
        external_field_magnitude(external_field_magnitude * mu_B), external_field_normal(external_field_normal),
        anisotropy_indices(anisotropy_indices), anisotropy_magnitudes(anisotropy_magnitudes), anisotropy_normals(anisotropy_normals),
        exchange_pairs_in(0), exchange_magnitudes_in(0), exchange_shell_magnitudes(exchange_shell_magnitudes),
        dmi_pairs_in(0), dmi_magnitudes_in(0), dmi_normals_in(0), dmi_shell_magnitudes(dmi_shell_magnitudes), dmi_shell_chirality(dm_chirality),
        quadruplets(quadruplets), quadruplet_magnitudes(quadruplet_magnitudes),
        ddi_cutoff_radius(ddi_radius)
    {
        // Generate interaction pairs, constants etc.
        this->Update_Interactions();
    }

    void Hamiltonian_Heisenberg::Update_Interactions()
    {
        #if defined(SPIRIT_USE_OPENMP)
        // When parallelising (cuda or openmp), we need all neighbours per spin
        const bool use_redundant_neighbours = true;
        #else
        // When running on a single thread, we can ignore redundant neighbours
        const bool use_redundant_neighbours = false;
        #endif

        // Exchange
        this->exchange_pairs      = pairfield(0);
        this->exchange_magnitudes = scalarfield(0);
        if( exchange_shell_magnitudes.size() > 0 )
        {
            // Generate Exchange neighbours
            intfield exchange_shells(0);
            Neighbours::Get_Neighbours_in_Shells(*geometry, exchange_shell_magnitudes.size(), exchange_pairs, exchange_shells, use_redundant_neighbours);
            for (unsigned int ipair = 0; ipair < exchange_pairs.size(); ++ipair)
            {
                this->exchange_magnitudes.push_back(exchange_shell_magnitudes[exchange_shells[ipair]]);
            }
        }
        else
        {
            // Use direct list of pairs
            this->exchange_pairs      = this->exchange_pairs_in;
            this->exchange_magnitudes = this->exchange_magnitudes_in;
            if( use_redundant_neighbours )
            {
                for (int i = 0; i < exchange_pairs_in.size(); ++i)
                {
                    auto& p = exchange_pairs_in[i];
                    auto& t = p.translations;
                    this->exchange_pairs.push_back(Pair{p.j, p.i, {-t[0], -t[1], -t[2]}});
                    this->exchange_magnitudes.push_back(exchange_magnitudes_in[i]);
                }
            }
        }

        // DMI
        this->dmi_pairs      = pairfield(0);
        this->dmi_magnitudes = scalarfield(0);
        this->dmi_normals    = vectorfield(0);
        if( dmi_shell_magnitudes.size() > 0 )
        {
            // Generate DMI neighbours and normals
            intfield dmi_shells(0);
            Neighbours::Get_Neighbours_in_Shells(*geometry, dmi_shell_magnitudes.size(), dmi_pairs, dmi_shells, use_redundant_neighbours);
            for (unsigned int ineigh = 0; ineigh < dmi_pairs.size(); ++ineigh)
            {
                this->dmi_normals.push_back(Neighbours::DMI_Normal_from_Pair(*geometry, dmi_pairs[ineigh], this->dmi_shell_chirality));
                this->dmi_magnitudes.push_back(dmi_shell_magnitudes[dmi_shells[ineigh]]);
            }
        }
        else
        {
            // Use direct list of pairs
            this->dmi_pairs      = this->dmi_pairs_in;
            this->dmi_magnitudes = this->dmi_magnitudes_in;
            this->dmi_normals    = this->dmi_normals_in;
            if( use_redundant_neighbours )
            {
                for (int i = 0; i < dmi_pairs_in.size(); ++i)
                {
                    auto& p = dmi_pairs_in[i];
                    auto& t = p.translations;
                    this->dmi_pairs.push_back(Pair{p.j, p.i, {-t[0], -t[1], -t[2]}});
                    this->dmi_magnitudes.push_back(dmi_magnitudes_in[i]);
                    this->dmi_normals.push_back(-dmi_normals_in[i]);
                }
            }
        }

        // Dipole-dipole
        this->ddi_pairs      = Engine::Neighbours::Get_Pairs_in_Radius(*this->geometry, this->ddi_cutoff_radius);
        this->ddi_magnitudes = scalarfield(this->ddi_pairs.size());
        this->ddi_normals    = vectorfield(this->ddi_pairs.size());

        scalar magnitude;
        Vector3 normal;

        for (unsigned int i = 0; i < this->ddi_pairs.size(); ++i)
        {
            Engine::Neighbours::DDI_from_Pair(
                *this->geometry,
                { this->ddi_pairs[i].i, this->ddi_pairs[i].j, this->ddi_pairs[i].translations },
                this->ddi_magnitudes[i], this->ddi_normals[i]);
        }

        // Update, which terms still contribute
        this->Update_Energy_Contributions();
    }

    void Hamiltonian_Heisenberg::Update_Energy_Contributions()
    {
        this->energy_contributions_per_spin = std::vector<std::pair<std::string, scalarfield>>(0);

        // External field
        if (this->external_field_magnitude > 0)
        {
            this->energy_contributions_per_spin.push_back({"Zeeman", scalarfield(0)});
            this->idx_zeeman = this->energy_contributions_per_spin.size()-1;
        }
        else this->idx_zeeman = -1;
        // Anisotropy
        if (this->anisotropy_indices.size() > 0)
        {
            this->energy_contributions_per_spin.push_back({"Anisotropy", scalarfield(0) });
            this->idx_anisotropy = this->energy_contributions_per_spin.size()-1;
        }
        else this->idx_anisotropy = -1;
        // Exchange
        if (this->exchange_pairs.size() > 0)
        {
            this->energy_contributions_per_spin.push_back({"Exchange", scalarfield(0) });
            this->idx_exchange = this->energy_contributions_per_spin.size()-1;
        }
        else this->idx_exchange = -1;
        // DMI
        if (this->dmi_pairs.size() > 0)
        {
            this->energy_contributions_per_spin.push_back({"DMI", scalarfield(0) });
            this->idx_dmi = this->energy_contributions_per_spin.size()-1;
        }
        else this->idx_dmi = -1;
        // Dipole-Dipole
        if (this->ddi_pairs.size() > 0)
        {
            this->energy_contributions_per_spin.push_back({"DD", scalarfield(0) });
            this->idx_ddi = this->energy_contributions_per_spin.size()-1;
        }
        else this->idx_ddi = -1;
        // Quadruplets
        if (this->quadruplets.size() > 0)
        {
            this->energy_contributions_per_spin.push_back({"Quadruplets", scalarfield(0) });
            this->idx_quadruplet = this->energy_contributions_per_spin.size()-1;
        }
        else this->idx_quadruplet = -1;
    }

    void Hamiltonian_Heisenberg::Energy_Contributions_per_Spin(const vectorfield & spins, std::vector<std::pair<std::string, scalarfield>> & contributions)
    {
        if (contributions.size() != this->energy_contributions_per_spin.size())
        {
            contributions = this->energy_contributions_per_spin;
        }

        int nos = spins.size();
        for (auto& contrib : contributions)
        {
            // Allocate if not already allocated
            if (contrib.second.size() != nos) contrib.second = scalarfield(nos, 0);
            // Otherwise set to zero
            else Vectormath::fill(contrib.second, 0);
        }

        // External field
        if (this->idx_zeeman >=0 )     E_Zeeman(spins, contributions[idx_zeeman].second);

        // Anisotropy
        if (this->idx_anisotropy >=0 ) E_Anisotropy(spins, contributions[idx_anisotropy].second);

        // Exchange
        if (this->idx_exchange >=0 )   E_Exchange(spins, contributions[idx_exchange].second);
        // DMI
        if (this->idx_dmi >=0 )        E_DMI(spins,contributions[idx_dmi].second);
        // DD
        if (this->idx_ddi >=0 )        E_DDI(spins, contributions[idx_ddi].second);
        // Quadruplets
        if (this->idx_quadruplet >=0 ) E_Quadruplet(spins, contributions[idx_quadruplet].second);
    }

    void Hamiltonian_Heisenberg::E_Zeeman(const vectorfield & spins, scalarfield & Energy)
    {
        const int N = geometry->n_cell_atoms;

        #pragma omp parallel for
        for (int icell = 0; icell < geometry->n_cells_total; ++icell)
        {
            for (int ibasis = 0; ibasis < N; ++ibasis)
            {
                int ispin = icell*N + ibasis;
                if (check_atom_type(this->geometry->atom_types[ispin]))
                    Energy[ispin] -= this->mu_s[ibasis] * this->external_field_magnitude * this->external_field_normal.dot(spins[ispin]);
            }
        }
    }

    void Hamiltonian_Heisenberg::E_Anisotropy(const vectorfield & spins, scalarfield & Energy)
    {
        const int N = geometry->n_cell_atoms;

        #pragma omp parallel for
        for (int icell = 0; icell < geometry->n_cells_total; ++icell)
        {
            for (int iani = 0; iani < anisotropy_indices.size(); ++iani)
            {
                int ispin = icell*N + anisotropy_indices[iani];
                if (check_atom_type(this->geometry->atom_types[ispin]))
                    Energy[ispin] -= this->anisotropy_magnitudes[iani] * std::pow(anisotropy_normals[iani].dot(spins[ispin]), 2.0);
            }
        }
    }

    void Hamiltonian_Heisenberg::E_Exchange(const vectorfield & spins, scalarfield & Energy)
    {
        #pragma omp parallel for
        for (unsigned int icell = 0; icell < geometry->n_cells_total; ++icell)
        {
            for (unsigned int i_pair = 0; i_pair < exchange_pairs.size(); ++i_pair)
            {
                int ispin = exchange_pairs[i_pair].i + icell*geometry->n_cell_atoms;
                int jspin = idx_from_pair(ispin, boundary_conditions, geometry->n_cells, geometry->n_cell_atoms, geometry->atom_types, exchange_pairs[i_pair]);
                if (jspin >= 0)
                {
                    Energy[ispin] -= 0.5 * exchange_magnitudes[i_pair] * spins[ispin].dot(spins[jspin]);
                    #ifndef _OPENMP
                    Energy[jspin] -= 0.5 * exchange_magnitudes[i_pair] * spins[ispin].dot(spins[jspin]);
                    #endif
                }
            }
        }
    }

    void Hamiltonian_Heisenberg::E_DMI(const vectorfield & spins, scalarfield & Energy)
    {
        #pragma omp parallel for
        for (unsigned int icell = 0; icell < geometry->n_cells_total; ++icell)
        {
            for (unsigned int i_pair = 0; i_pair < dmi_pairs.size(); ++i_pair)
            {
                int ispin = dmi_pairs[i_pair].i + icell*geometry->n_cell_atoms;
                int jspin = idx_from_pair(ispin, boundary_conditions, geometry->n_cells, geometry->n_cell_atoms, geometry->atom_types, dmi_pairs[i_pair]);
                if (jspin >= 0)
                {
                    Energy[ispin] -= 0.5 * dmi_magnitudes[i_pair] * dmi_normals[i_pair].dot(spins[ispin].cross(spins[jspin]));
                    #ifndef _OPENMP
                    Energy[jspin] -= 0.5 * dmi_magnitudes[i_pair] * dmi_normals[i_pair].dot(spins[ispin].cross(spins[jspin]));
                    #endif
                }
            }
        }
    }

    void Hamiltonian_Heisenberg::E_DDI(const vectorfield & spins, scalarfield & Energy)
    {
        // The translations are in angstr�m, so the |r|[m] becomes |r|[m]*10^-10
        const scalar mult = mu_0 * std::pow(mu_B, 2) / ( 4*Pi * 1e-30 );

        scalar result = 0.0;

        for (unsigned int i_pair = 0; i_pair < ddi_pairs.size(); ++i_pair)
        {
            if (ddi_magnitudes[i_pair] > 0.0)
            {
                for (int da = 0; da < geometry->n_cells[0]; ++da)
                {
                    for (int db = 0; db < geometry->n_cells[1]; ++db)
                    {
                        for (int dc = 0; dc < geometry->n_cells[2]; ++dc)
                        {
                            std::array<int, 3 > translations = { da, db, dc };
                            int i = ddi_pairs[i_pair].i;
                            int j = ddi_pairs[i_pair].j;
                            int ispin = i + Vectormath::idx_from_translations(geometry->n_cells, geometry->n_cell_atoms, translations);
                            int jspin = idx_from_pair(ispin, boundary_conditions, geometry->n_cells, geometry->n_cell_atoms, geometry->atom_types, ddi_pairs[i_pair]);
                            if (jspin >= 0)
                            {
                                Energy[ispin] -= 0.5 * this->mu_s[i] * this->mu_s[j] * mult / std::pow(ddi_magnitudes[i_pair], 3.0) *
                                    (3 * spins[ispin].dot(ddi_normals[i_pair]) * spins[ispin].dot(ddi_normals[i_pair]) - spins[ispin].dot(spins[ispin]));
                                Energy[jspin] -= 0.5 * this->mu_s[i] * this->mu_s[j] * mult / std::pow(ddi_magnitudes[i_pair], 3.0) *
                                    (3 * spins[ispin].dot(ddi_normals[i_pair]) * spins[ispin].dot(ddi_normals[i_pair]) - spins[ispin].dot(spins[ispin]));
                            }
                        }
                    }
                }
            }
        }
    }// end DipoleDipole

    void Hamiltonian_Heisenberg::E_Quadruplet(const vectorfield & spins, scalarfield & Energy)
    {
        for (unsigned int iquad = 0; iquad < quadruplets.size(); ++iquad)
        {
            for (int da = 0; da < geometry->n_cells[0]; ++da)
            {
                for (int db = 0; db < geometry->n_cells[1]; ++db)
                {
                    for (int dc = 0; dc < geometry->n_cells[2]; ++dc)
                    {
                        std::array<int, 3 > translations = { da, db, dc };
                        int ispin = quadruplets[iquad].i + Vectormath::idx_from_translations(geometry->n_cells, geometry->n_cell_atoms, translations);
                        int jspin = quadruplets[iquad].j + Vectormath::idx_from_translations(geometry->n_cells, geometry->n_cell_atoms, translations, quadruplets[iquad].d_j);
                        int kspin = quadruplets[iquad].k + Vectormath::idx_from_translations(geometry->n_cells, geometry->n_cell_atoms, translations, quadruplets[iquad].d_k);
                        int lspin = quadruplets[iquad].l + Vectormath::idx_from_translations(geometry->n_cells, geometry->n_cell_atoms, translations, quadruplets[iquad].d_l);

                        if ( check_atom_type(this->geometry->atom_types[ispin]) && check_atom_type(this->geometry->atom_types[jspin]) &&
                                check_atom_type(this->geometry->atom_types[kspin]) && check_atom_type(this->geometry->atom_types[lspin]) )
                        {
                            Energy[ispin] -= 0.25*quadruplet_magnitudes[iquad] * (spins[ispin].dot(spins[jspin])) * (spins[kspin].dot(spins[lspin]));
                            Energy[jspin] -= 0.25*quadruplet_magnitudes[iquad] * (spins[ispin].dot(spins[jspin])) * (spins[kspin].dot(spins[lspin]));
                            Energy[kspin] -= 0.25*quadruplet_magnitudes[iquad] * (spins[ispin].dot(spins[jspin])) * (spins[kspin].dot(spins[lspin]));
                            Energy[lspin] -= 0.25*quadruplet_magnitudes[iquad] * (spins[ispin].dot(spins[jspin])) * (spins[kspin].dot(spins[lspin]));
                        }
                    }
                }
            }
        }
    }

    scalar Hamiltonian_Heisenberg::Energy_Single_Spin(int ispin_in, const vectorfield & spins)
    {
        int icell  = ispin_in / this->geometry->n_cell_atoms;
        int ibasis = ispin_in - icell*this->geometry->n_cell_atoms;
        scalar Energy = 0;

        // External field
        if (this->idx_zeeman >= 0)
        {
            if (check_atom_type(this->geometry->atom_types[ispin_in]))
                Energy -= this->mu_s[ibasis] * this->external_field_magnitude * this->external_field_normal.dot(spins[ispin_in]);
        }

        // Anisotropy
        if (this->idx_anisotropy >= 0)
        {
            for (int iani = 0; iani < anisotropy_indices.size(); ++iani)
            {
                if (anisotropy_indices[iani] == ibasis)
                {
                    if (check_atom_type(this->geometry->atom_types[ispin_in]))
                        Energy -= this->anisotropy_magnitudes[iani] * std::pow(anisotropy_normals[iani].dot(spins[ispin_in]), 2.0);
                }
            }
        }

        // Exchange
        if (this->idx_exchange >= 0)
        {
            for (unsigned int ipair = 0; ipair < exchange_pairs.size(); ++ipair)
            {
                if (exchange_pairs[ipair].i == ibasis)
                {
                    int ispin = exchange_pairs[ipair].i + icell*geometry->n_cell_atoms;
                    int jspin = idx_from_pair(ispin, boundary_conditions, geometry->n_cells, geometry->n_cell_atoms, geometry->atom_types, exchange_pairs[ipair]);
                    if (jspin >= 0)
                    {
                        Energy -= 0.5 * this->exchange_magnitudes[ipair] * spins[ispin].dot(spins[jspin]);
                    }
                    #ifndef _OPENMP
                    jspin = idx_from_pair(ispin, boundary_conditions, geometry->n_cells, geometry->n_cell_atoms, geometry->atom_types, exchange_pairs[ipair], true);
                    if (jspin >= 0)
                    {
                        Energy -= 0.5 * this->exchange_magnitudes[ipair] * spins[ispin].dot(spins[jspin]);
                    }
                    #endif
                }
            }
        }

        // DMI
        if (this->idx_dmi >= 0)
        {
            for (unsigned int ipair = 0; ipair < dmi_pairs.size(); ++ipair)
            {
                if (dmi_pairs[ipair].i == ibasis)
                {
                    int ispin = dmi_pairs[ipair].i + icell*geometry->n_cell_atoms;
                    int jspin = idx_from_pair(ispin, boundary_conditions, geometry->n_cells, geometry->n_cell_atoms, geometry->atom_types, dmi_pairs[ipair]);
                    if (jspin >= 0)
                    {
                        Energy -= 0.5 * this->dmi_magnitudes[ipair] * this->dmi_normals[ipair].dot(spins[ispin].cross(spins[jspin]));
                    }
                    #ifndef _OPENMP
                    jspin = idx_from_pair(ispin, boundary_conditions, geometry->n_cells, geometry->n_cell_atoms, geometry->atom_types, dmi_pairs[ipair], true);
                    if (jspin >= 0)
                    {
                        Energy += 0.5 * this->dmi_magnitudes[ipair] * this->dmi_normals[ipair].dot(spins[ispin].cross(spins[jspin]));
                    }
                    #endif
                }
            }
        }

        // DDI
        if (this->idx_ddi >= 0)
        {
            for (unsigned int ipair = 0; ipair < ddi_pairs.size(); ++ipair)
            {
                if (ddi_pairs[ipair].i == ibasis)
                {
                    // The translations are in angstr�m, so the |r|[m] becomes |r|[m]*10^-10
                    const scalar mult = 0.5 * this->mu_s[ddi_pairs[ipair].i] * this->mu_s[ddi_pairs[ipair].j]
                        * Utility::Constants::mu_0 * std::pow(Utility::Constants::mu_B, 2) / ( 4*Utility::Constants::Pi * 1e-30 );

                    int ispin = ddi_pairs[ipair].i + icell*geometry->n_cell_atoms;
                    int jspin = idx_from_pair(ispin, boundary_conditions, geometry->n_cells, geometry->n_cell_atoms, geometry->atom_types, ddi_pairs[ipair]);

                    if (jspin >= 0)
                    {
                        Energy -= mult / std::pow(this->ddi_magnitudes[ipair], 3.0) *
                            (3 * spins[ispin].dot(this->ddi_normals[ipair]) * spins[ispin].dot(this->ddi_normals[ipair]) - spins[ispin].dot(spins[ispin]));

                    }
                    #ifndef _OPENMP
                    jspin = idx_from_pair(ispin, boundary_conditions, geometry->n_cells, geometry->n_cell_atoms, geometry->atom_types, dmi_pairs[ipair], true);
                    if (jspin >= 0)
                    {
                        Energy += mult / std::pow(this->ddi_magnitudes[ipair], 3.0) *
                            (3 * spins[ispin].dot(this->ddi_normals[ipair]) * spins[ispin].dot(this->ddi_normals[ipair]) - spins[ispin].dot(spins[ispin]));
                    }
                    #endif
                }
            }
        }

        // Quadruplets
        if (this->idx_quadruplet >= 0)
        {
            for (unsigned int iquad = 0; iquad < quadruplets.size(); ++iquad)
            {
                auto translations = Vectormath::translations_from_idx(geometry->n_cells, geometry->n_cell_atoms, icell);
                int ispin = quadruplets[iquad].i + icell*geometry->n_cell_atoms;
                int jspin = quadruplets[iquad].j + Vectormath::idx_from_translations(geometry->n_cells, geometry->n_cell_atoms, translations, quadruplets[iquad].d_j);
                int kspin = quadruplets[iquad].k + Vectormath::idx_from_translations(geometry->n_cells, geometry->n_cell_atoms, translations, quadruplets[iquad].d_k);
                int lspin = quadruplets[iquad].l + Vectormath::idx_from_translations(geometry->n_cells, geometry->n_cell_atoms, translations, quadruplets[iquad].d_l);

                if ( check_atom_type(this->geometry->atom_types[ispin]) && check_atom_type(this->geometry->atom_types[jspin]) &&
                     check_atom_type(this->geometry->atom_types[kspin]) && check_atom_type(this->geometry->atom_types[lspin]) )
                {
                    Energy -= 0.25*quadruplet_magnitudes[iquad] * (spins[ispin].dot(spins[jspin])) * (spins[kspin].dot(spins[lspin]));
                }

                #ifndef _OPENMP
                // TODO: mirrored quadruplet when unique quadruplets are used
                // jspin = quadruplets[iquad].j + Vectormath::idx_from_translations(geometry->n_cells, geometry->n_cell_atoms, translations, quadruplets[iquad].d_j, true);
                // kspin = quadruplets[iquad].k + Vectormath::idx_from_translations(geometry->n_cells, geometry->n_cell_atoms, translations, quadruplets[iquad].d_k, true);
                // lspin = quadruplets[iquad].l + Vectormath::idx_from_translations(geometry->n_cells, geometry->n_cell_atoms, translations, quadruplets[iquad].d_l, true);

                // if ( check_atom_type(this->geometry->atom_types[ispin]) && check_atom_type(this->geometry->atom_types[jspin]) &&
                //      check_atom_type(this->geometry->atom_types[kspin]) && check_atom_type(this->geometry->atom_types[lspin]) )
                // {
                //     Energy -= 0.25*quadruplet_magnitudes[iquad] * (spins[ispin].dot(spins[jspin])) * (spins[kspin].dot(spins[lspin]));
                // }
                #endif
            }
        }

        return Energy;
    }

    void Hamiltonian_Heisenberg::Gradient(const vectorfield & spins, vectorfield & gradient)
    {
        // Set to zero
        Vectormath::fill(gradient, {0,0,0});

        // External field
        Gradient_Zeeman(gradient);

        // Anisotropy
        Gradient_Anisotropy(spins, gradient);

        // Exchange
        this->Gradient_Exchange(spins, gradient);
        // DMI
        this->Gradient_DMI(spins, gradient);
        // DD
        this->Gradient_DDI(spins, gradient);

        // Quadruplets
        this->Gradient_Quadruplet(spins, gradient);
    }

    void Hamiltonian_Heisenberg::Gradient_Zeeman(vectorfield & gradient)
    {
        const int N = geometry->n_cell_atoms;

        #pragma omp parallel for
        for (int icell = 0; icell < geometry->n_cells_total; ++icell)
        {
            for (int ibasis = 0; ibasis < N; ++ibasis)
            {
                int ispin = icell*N + ibasis;
                if (check_atom_type(this->geometry->atom_types[ispin]))
                    gradient[ispin] -= this->mu_s[ibasis] * this->external_field_magnitude * this->external_field_normal;
            }
        }
    }

    void Hamiltonian_Heisenberg::Gradient_Anisotropy(const vectorfield & spins, vectorfield & gradient)
    {
        const int N = geometry->n_cell_atoms;

        #pragma omp parallel for
        for (int icell = 0; icell < geometry->n_cells_total; ++icell)
        {
            for (int iani = 0; iani < anisotropy_indices.size(); ++iani)
            {
                int ispin = icell*N + anisotropy_indices[iani];
                if (check_atom_type(this->geometry->atom_types[ispin]))
                    gradient[ispin] -= 2.0 * this->anisotropy_magnitudes[iani] * this->anisotropy_normals[iani] * anisotropy_normals[iani].dot(spins[ispin]);
            }
        }
    }

    void Hamiltonian_Heisenberg::Gradient_Exchange(const vectorfield & spins, vectorfield & gradient)
    {
        #pragma omp parallel for
        for (int icell = 0; icell < geometry->n_cells_total; ++icell)
        {
            for (unsigned int i_pair = 0; i_pair < exchange_pairs.size(); ++i_pair)
            {
                int ispin = exchange_pairs[i_pair].i + icell*geometry->n_cell_atoms;
                int jspin = idx_from_pair(ispin, boundary_conditions, geometry->n_cells, geometry->n_cell_atoms, geometry->atom_types, exchange_pairs[i_pair]);
                if (jspin >= 0)
                {
                    gradient[ispin] -= exchange_magnitudes[i_pair] * spins[jspin];
                    #ifndef _OPENMP
                    gradient[jspin] -= exchange_magnitudes[i_pair] * spins[ispin];
                    #endif
                }
            }
        }
    }

    void Hamiltonian_Heisenberg::Gradient_DMI(const vectorfield & spins, vectorfield & gradient)
    {
        #pragma omp parallel for
        for (int icell = 0; icell < geometry->n_cells_total; ++icell)
        {
            for (unsigned int i_pair = 0; i_pair < dmi_pairs.size(); ++i_pair)
            {
                int ispin = dmi_pairs[i_pair].i + icell*geometry->n_cell_atoms;
                int jspin = idx_from_pair(ispin, boundary_conditions, geometry->n_cells, geometry->n_cell_atoms, geometry->atom_types, dmi_pairs[i_pair]);
                if (jspin >= 0)
                {
                    gradient[ispin] -= dmi_magnitudes[i_pair] * spins[jspin].cross(dmi_normals[i_pair]);
                    #ifndef _OPENMP
                    gradient[jspin] += dmi_magnitudes[i_pair] * spins[ispin].cross(dmi_normals[i_pair]);
                    #endif
                }
            }
        }
    }

    void Hamiltonian_Heisenberg::Gradient_DDI(const vectorfield & spins, vectorfield & gradient)
    {
        // The translations are in angstr�m, so the |r|[m] becomes |r|[m]*10^-10
        const scalar mult = mu_0 * std::pow(mu_B, 2) / ( 4*Pi * 1e-30 );

        for (unsigned int i_pair = 0; i_pair < ddi_pairs.size(); ++i_pair)
        {
            if (ddi_magnitudes[i_pair] > 0.0)
            {
                for (int da = 0; da < geometry->n_cells[0]; ++da)
                {
                    for (int db = 0; db < geometry->n_cells[1]; ++db)
                    {
                        for (int dc = 0; dc < geometry->n_cells[2]; ++dc)
                        {
                            scalar skalar_contrib = mult / std::pow(ddi_magnitudes[i_pair], 3.0);
                            std::array<int, 3 > translations = { da, db, dc };

                            int i = ddi_pairs[i_pair].i;
                            int j = ddi_pairs[i_pair].j;
                            int ispin = i + Vectormath::idx_from_translations(geometry->n_cells, geometry->n_cell_atoms, translations);
                            int jspin = idx_from_pair(ispin, boundary_conditions, geometry->n_cells, geometry->n_cell_atoms, geometry->atom_types, ddi_pairs[i_pair]);
                            if (jspin >= 0)
                            {
                                gradient[ispin] -= this->mu_s[j] * skalar_contrib * (3 * ddi_normals[i_pair] * spins[jspin].dot(ddi_normals[i_pair]) - spins[jspin]);
                                gradient[jspin] -= this->mu_s[i] * skalar_contrib * (3 * ddi_normals[i_pair] * spins[ispin].dot(ddi_normals[i_pair]) - spins[ispin]);
                            }
                        }
                    }
                }
            }
        }
    }//end Field_DipoleDipole

    void Hamiltonian_Heisenberg::Gradient_Quadruplet(const vectorfield & spins, vectorfield & gradient)
    {
        for (unsigned int iquad = 0; iquad < quadruplets.size(); ++iquad)
        {
            int i = quadruplets[iquad].i;
            int j = quadruplets[iquad].j;
            int k = quadruplets[iquad].k;
            int l = quadruplets[iquad].l;
            for (int da = 0; da < geometry->n_cells[0]; ++da)
            {
                for (int db = 0; db < geometry->n_cells[1]; ++db)
                {
                    for (int dc = 0; dc < geometry->n_cells[2]; ++dc)
                    {
                        std::array<int, 3 > translations = { da, db, dc };
                        int ispin = i + Vectormath::idx_from_translations(geometry->n_cells, geometry->n_cell_atoms, translations);
                        int jspin = j + Vectormath::idx_from_translations(geometry->n_cells, geometry->n_cell_atoms, translations, quadruplets[iquad].d_j);
                        int kspin = k + Vectormath::idx_from_translations(geometry->n_cells, geometry->n_cell_atoms, translations, quadruplets[iquad].d_k);
                        int lspin = l + Vectormath::idx_from_translations(geometry->n_cells, geometry->n_cell_atoms, translations, quadruplets[iquad].d_l);

                        if ( check_atom_type(this->geometry->atom_types[ispin]) && check_atom_type(this->geometry->atom_types[jspin]) &&
                                check_atom_type(this->geometry->atom_types[kspin]) && check_atom_type(this->geometry->atom_types[lspin]) )
                        {
                            gradient[ispin] -= quadruplet_magnitudes[iquad] * spins[jspin] * (spins[kspin].dot(spins[lspin]));
                            gradient[jspin] -= quadruplet_magnitudes[iquad] * spins[ispin] * (spins[kspin].dot(spins[lspin]));
                            gradient[kspin] -= quadruplet_magnitudes[iquad] * (spins[ispin].dot(spins[jspin])) * spins[lspin];
                            gradient[lspin] -= quadruplet_magnitudes[iquad] * (spins[ispin].dot(spins[jspin])) * spins[kspin];
                        }
                    }
                }
            }
        }
    }

    void Hamiltonian_Heisenberg::Hessian(const vectorfield & spins, MatrixX & hessian)
    {
        int nos = spins.size();

        // Set to zero
        hessian.setZero();

        // Single Spin elements
        for (int da = 0; da < geometry->n_cells[0]; ++da)
        {
            for (int db = 0; db < geometry->n_cells[1]; ++db)
            {
                for (int dc = 0; dc < geometry->n_cells[2]; ++dc)
                {
                    std::array<int, 3 > translations = { da, db, dc };
                    int icell = Vectormath::idx_from_translations(geometry->n_cells, geometry->n_cell_atoms, translations);
                    for (int alpha = 0; alpha < 3; ++alpha)
                    {
                        for ( int beta = 0; beta < 3; ++beta )
                        {
                            for (unsigned int i = 0; i < anisotropy_indices.size(); ++i)
                            {
                                if ( check_atom_type(this->geometry->atom_types[anisotropy_indices[i]]) )
                                {
                                    int idx_i = 3 * icell + anisotropy_indices[i] + alpha;
                                    int idx_j = 3 * icell + anisotropy_indices[i] + beta;
                                    // scalar x = -2.0*this->anisotropy_magnitudes[i] * std::pow(this->anisotropy_normals[i][alpha], 2);
                                    hessian( idx_i, idx_j ) += -2.0 * this->anisotropy_magnitudes[i] *
                                                                    this->anisotropy_normals[i][alpha] *
                                                                    this->anisotropy_normals[i][beta];
                                }
                            }
                        }
                    }
                }
            }
        }

        // Spin Pair elements
        // Exchange
        for (int da = 0; da < geometry->n_cells[0]; ++da)
        {
            for (int db = 0; db < geometry->n_cells[1]; ++db)
            {
                for (int dc = 0; dc < geometry->n_cells[2]; ++dc)
                {
                    std::array<int, 3 > translations = { da, db, dc };
                    for (unsigned int i_pair = 0; i_pair < this->exchange_pairs.size(); ++i_pair)
                    {
                        int ispin = exchange_pairs[i_pair].i + Vectormath::idx_from_translations(geometry->n_cells, geometry->n_cell_atoms, translations);
                        int jspin = idx_from_pair(ispin, boundary_conditions, geometry->n_cells, geometry->n_cell_atoms, geometry->atom_types, exchange_pairs[i_pair]);
                        if (jspin >= 0)
                        {
                            for (int alpha = 0; alpha < 3; ++alpha)
                            {
                                int i = 3 * ispin + alpha;
                                int j = 3 * jspin + alpha;

                                hessian(i, j) += -exchange_magnitudes[i_pair];
                                #ifndef _OPENMP
                                hessian(j, i) += -exchange_magnitudes[i_pair];
                                #endif
                            }
                        }
                    }
                }
            }
        }

        // DMI
        for (int da = 0; da < geometry->n_cells[0]; ++da)
        {
            for (int db = 0; db < geometry->n_cells[1]; ++db)
            {
                for (int dc = 0; dc < geometry->n_cells[2]; ++dc)
                {
                    std::array<int, 3 > translations = { da, db, dc };
                    for (unsigned int i_pair = 0; i_pair < this->dmi_pairs.size(); ++i_pair)
                    {
                        int ispin = dmi_pairs[i_pair].i + Vectormath::idx_from_translations(geometry->n_cells, geometry->n_cell_atoms, translations);
                        int jspin = idx_from_pair(ispin, boundary_conditions, geometry->n_cells, geometry->n_cell_atoms, geometry->atom_types, dmi_pairs[i_pair]);
                        if (jspin >= 0)
                        {
                            int i = 3*ispin;
                            int j = 3*jspin;

                            hessian(i+2, j+1) +=  dmi_magnitudes[i_pair] * dmi_normals[i_pair][0];
                            hessian(i+1, j+2) += -dmi_magnitudes[i_pair] * dmi_normals[i_pair][0];
                            hessian(i, j+2)   +=  dmi_magnitudes[i_pair] * dmi_normals[i_pair][1];
                            hessian(i+2, j)   += -dmi_magnitudes[i_pair] * dmi_normals[i_pair][1];
                            hessian(i+1, j)   +=  dmi_magnitudes[i_pair] * dmi_normals[i_pair][2];
                            hessian(i, j+1)   += -dmi_magnitudes[i_pair] * dmi_normals[i_pair][2];

                            #ifndef _OPENMP
                            hessian(j+1, i+2) +=  dmi_magnitudes[i_pair] * dmi_normals[i_pair][0];
                            hessian(j+2, i+1) += -dmi_magnitudes[i_pair] * dmi_normals[i_pair][0];
                            hessian(j+2, i)   +=  dmi_magnitudes[i_pair] * dmi_normals[i_pair][1];
                            hessian(j, i+2)   += -dmi_magnitudes[i_pair] * dmi_normals[i_pair][1];
                            hessian(j, i+1)   +=  dmi_magnitudes[i_pair] * dmi_normals[i_pair][2];
                            hessian(j+1, i)   += -dmi_magnitudes[i_pair] * dmi_normals[i_pair][2];
                            #endif
                        }
                    }
                }
            }
        }

        //// Dipole-Dipole
        //for (unsigned int i_pair = 0; i_pair < this->DD_indices.size(); ++i_pair)
        //{
        //	// indices
        //	int idx_1 = DD_indices[i_pair][0];
        //	int idx_2 = DD_indices[i_pair][1];
        //	// prefactor
        //	scalar prefactor = 0.0536814951168
        //		* this->mu_s[idx_1] * this->mu_s[idx_2]
        //		/ std::pow(DD_magnitude[i_pair], 3);
        //	// components
        //	for (int alpha = 0; alpha < 3; ++alpha)
        //	{
        //		for (int beta = 0; beta < 3; ++beta)
        //		{
        //			int idx_h = idx_1 + alpha*nos + 3 * nos*(idx_2 + beta*nos);
        //			if (alpha == beta)
        //				hessian[idx_h] += prefactor;
        //			hessian[idx_h] += -3.0*prefactor*DD_normal[i_pair][alpha] * DD_normal[i_pair][beta];
        //		}
        //	}
        //}

        // Quadruplets
    }

    // Hamiltonian name as string
    static const std::string name = "Heisenberg";
    const std::string& Hamiltonian_Heisenberg::Name() { return name; }
}

#endif


/*ckwg +29
 * Copyright 2015-2020 by Kitware, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither name of Kitware, Inc. nor the names of any contributors may be used
 *    to endorse or promote products derived from this software without specific
 *    prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS''
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

// The immediately following copyright notice applies to code in the methods
// Triangulate_DLT, find_optimal_image_points and triangulate_fast_two_view.

 // Copyright (C) 2013 The Regents of the University of California (Regents).
 // All rights reserved.
 //
 // Redistribution and use in source and binary forms, with or without
 // modification, are permitted provided that the following conditions are
 // met:
 //
 //     * Redistributions of source code must retain the above copyright
 //       notice, this list of conditions and the following disclaimer.
 //
 //     * Redistributions in binary form must reproduce the above
 //       copyright notice, this list of conditions and the following
 //       disclaimer in the documentation and/or other materials provided
 //       with the distribution.
 //
 //     * Neither the name of The Regents or University of California nor the
 //       names of its contributors may be used to endorse or promote products
 //       derived from this software without specific prior written permission.
 //
 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 //
 // Please contact the author of this library if you have any questions.
 // Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

/**
 * \file
 * \brief Implementation of triangulation function
 */

#include "triangulate.h"
#include <Eigen/SVD>
#include <arrows/mvg/epipolar_geometry.h>

namespace kwiver {
namespace arrows {
namespace mvg {

// Triangulates 2 views
void
Triangulate_DLT( kwiver::vital::matrix_3x4d const& pose1,
                 kwiver::vital::matrix_3x4d const& pose2,
                 kwiver::vital::vector_2d const& point1,
                 kwiver::vital::vector_2d const& point2,
                 kwiver::vital::vector_4d &triangulated_point)
{
  // code modified from code found at
  // https://github.com/sweeneychris/TheiaSfM/blob/master/src/theia/sfm/triangulation/triangulation.h

  kwiver::vital::matrix_4x4d design_matrix;
  design_matrix.row(0) = point1[0] * pose1.row(2) - pose1.row(0);
  design_matrix.row(1) = point1[1] * pose1.row(2) - pose1.row(1);
  design_matrix.row(2) = point2[0] * pose2.row(2) - pose2.row(0);
  design_matrix.row(3) = point2[1] * pose2.row(2) - pose2.row(1);

  // Extract nullspace.
  Eigen::JacobiSVD<Eigen::Matrix<double,4,4>> svd(design_matrix, Eigen::ComputeFullV);
  triangulated_point = svd.matrixV().rightCols<1>();
}

// Given either a fundamental or essential matrix and two corresponding images
// points such that ematrix * point2 produces a line in the first image,
// this method finds corrected image points such that
// corrected_point1^t * ematrix * corrected_point2 = 0.
void
find_optimal_image_points(kwiver::vital::essential_matrix_sptr ematrix,
                          const vital::vector_2d &point1,
                          const vital::vector_2d &point2,
                          vital::vector_2d &corrected_point1,
                          vital::vector_2d &corrected_point2)
{
  // code modified from code found at
  // https://github.com/sweeneychris/TheiaSfM/blob/master/src/theia/sfm/triangulation/triangulation.cc
  auto E = ematrix->matrix();

  vital::vector_3d point1_homog = point1.homogeneous();
  vital::vector_3d point2_homog = point2.homogeneous();

  // A helper matrix to isolate certain coordinates.
  Eigen::Matrix<double, 2, 3> s_matrix;
  s_matrix << 1, 0, 0, 0, 1, 0;

  const Eigen::Matrix2d e_submatrix = E.topLeftCorner<2, 2>();

  // The epipolar line from one image point in the other image.
  vital::vector_2d epipolar_line1 = s_matrix * E * point2_homog;
  vital::vector_2d epipolar_line2 = s_matrix * E.transpose() * point1_homog;

  const double a = epipolar_line1.transpose() * e_submatrix * epipolar_line2;
  const double b =
    (epipolar_line1.squaredNorm() + epipolar_line2.squaredNorm()) / 2.0;
  const double c = point1_homog.transpose() * E * point2_homog;

  const double d = sqrt(b * b - a * c);

  double lambda = c / (b + d);
  epipolar_line1 -= e_submatrix * lambda * epipolar_line1;
  epipolar_line2 -= e_submatrix.transpose() * lambda * epipolar_line2;

  lambda *=
    (2.0 * d) / (epipolar_line1.squaredNorm() + epipolar_line2.squaredNorm());

  corrected_point1 =
    (point1_homog - s_matrix.transpose() * lambda * epipolar_line1)
    .hnormalized();
  corrected_point2 =
    (point2_homog - s_matrix.transpose() * lambda * epipolar_line2)
    .hnormalized();
}

template <typename T>
Eigen::Matrix<T, 3, 1>
triangulate_fast_two_view(const vital::simple_camera_perspective &camera0,
                          const vital::simple_camera_perspective &camera1,
                          const Eigen::Matrix<T, 2, 1> &point0,
                          const Eigen::Matrix<T, 2, 1> &point1)
{
  // code modified from code found at
  // https://github.com/sweeneychris/TheiaSfM/blob/master/src/theia/sfm/triangulation/triangulation.cc

  auto E = essential_matrix_from_cameras(camera0, camera1);

  const vital::vector_2d pt0 = camera0.get_intrinsics()->unmap(point0.template cast<double>());
  const vital::vector_2d pt1 = camera1.get_intrinsics()->unmap(point1.template cast<double>());

  vital::vector_2d corrected_pt0, corrected_pt1;

  find_optimal_image_points(E, pt0, pt1, corrected_pt0, corrected_pt1);

  vital::vector_4d triangulated_point;
  Triangulate_DLT(camera0.pose_matrix(), camera1.pose_matrix(), corrected_pt0, corrected_pt1, triangulated_point);
  return triangulated_point.hnormalized().template cast<T>();
}

/// Triangulate a 3D point from a set of cameras and 2D image points
template <typename T>
Eigen::Matrix<T,3,1>
triangulate_inhomog(const std::vector<vital::simple_camera_perspective >& cameras,
                    const std::vector<Eigen::Matrix<T,2,1> >& points)
{
  typedef Eigen::Matrix<T,2,1> vector_2;
  typedef Eigen::Matrix<T,3,1> vector_3;
  typedef Eigen::Matrix<T,3,3> matrix_3x3;
  typedef Eigen::Matrix<T, Eigen::Dynamic, 3> data_matrix_t;
  typedef Eigen::Matrix<T, Eigen::Dynamic, 1> data_vector_t;
  const unsigned int num_rows = 2*static_cast<unsigned int>(points.size());
  data_matrix_t A(num_rows, 3);
  data_vector_t b(num_rows);
  for ( unsigned int i=0; i<points.size(); ++i )
  {
    // the camera
    const vital::simple_camera_perspective& cam = cameras[i];
    const matrix_3x3 R(cam.get_rotation().matrix().cast<T>());
    const vector_3 t(cam.translation().cast<T>());
    // the point in normalized coordinates
    const vital::vector_2d p2d = points[i].template cast<double>();
    const vector_2 pt = cam.get_intrinsics()->unmap(p2d).template cast<T>();
    A(2*i,   0) = R(0,0) - pt.x() * R(2,0);
    A(2*i,   1) = R(0,1) - pt.x() * R(2,1);
    A(2*i,   2) = R(0,2) - pt.x() * R(2,2);
    A(2*i+1, 0) = R(1,0) - pt.y() * R(2,0);
    A(2*i+1, 1) = R(1,1) - pt.y() * R(2,1);
    A(2*i+1, 2) = R(1,2) - pt.y() * R(2,2);
    b[2*i  ] = t.z()*pt.x() - t.x();
    b[2*i+1] = t.z()*pt.y() - t.y();
  }
  Eigen::JacobiSVD<data_matrix_t> svd(A, Eigen::ComputeFullU |
                                         Eigen::ComputeFullV);
  return svd.solve(b);
}

/// Triangulate a homogeneous 3D point from a set of cameras and 2D image points
template <typename T>
Eigen::Matrix<T,4,1>
triangulate_homog(const std::vector<vital::simple_camera_perspective >& cameras,
                  const std::vector<Eigen::Matrix<T,2,1> >& points)
{
  typedef Eigen::Matrix<T,2,1> vector_2;
  typedef Eigen::Matrix<T,3,1> vector_3;
  typedef Eigen::Matrix<T,3,3> matrix_3x3;
  typedef Eigen::Matrix<T, Eigen::Dynamic, 4> data_matrix_t;
  const unsigned int num_rows = 2*static_cast<unsigned int>(points.size());
  data_matrix_t A(num_rows, 4);
  for ( unsigned int i=0; i<points.size(); ++i )
  {
    // the camera
    const vital::simple_camera_perspective& cam = cameras[i];
    const matrix_3x3 R(cam.get_rotation().matrix().cast<T>());
    const vector_3 t(cam.translation().cast<T>());
    // the point in normalized coordinates
    const vital::vector_2d p2d = points[i].template cast<double>();
    const vector_2 pt = cam.get_intrinsics()->unmap(p2d).template cast<T>();
    A(2*i,   0) = R(0,0) - pt.x() * R(2,0);
    A(2*i,   1) = R(0,1) - pt.x() * R(2,1);
    A(2*i,   2) = R(0,2) - pt.x() * R(2,2);
    A(2*i,   3) = t.x()  - pt.x() * t.z();
    A(2*i+1, 0) = R(1,0) - pt.y() * R(2,0);
    A(2*i+1, 1) = R(1,1) - pt.y() * R(2,1);
    A(2*i+1, 2) = R(1,2) - pt.y() * R(2,2);
    A(2*i+1, 3) = t.y()  - pt.y() * t.z();
  }
  Eigen::JacobiSVD<data_matrix_t > svd(A, Eigen::ComputeFullV);
  return svd.matrixV().col(3);
}

/// Triangulate a 3D point from a set of RPC cameras and 2D image points
template <typename T>
Eigen::Matrix<T,3,1>
triangulate_rpc(const std::vector<vital::simple_camera_rpc >& cameras,
                const std::vector<Eigen::Matrix<T,2,1> >& points)
{
  // Get the pairs of points to define the rays
  std::vector< std::pair< vital::vector_3d, vital::vector_3d > > pts;

  Eigen::Array3d curr_scale = cameras[0].world_scale().array();
  Eigen::Array3d curr_offset = cameras[0].world_offset().array();
  Eigen::Array3d min_pos = curr_offset - curr_scale;
  Eigen::Array3d max_pos = curr_offset + curr_scale;

  for ( unsigned int i = 0; i < points.size(); ++i )
  {
    // Get world offset and scale to set normalization and sample heights
    curr_scale = cameras[i].world_scale().array();
    curr_offset = cameras[i].world_offset().array();

    min_pos = min_pos.min( curr_offset - curr_scale );
    max_pos = max_pos.max( curr_offset + curr_scale );

    double h1 = ( curr_offset - curr_scale )[2];
    double h2 = ( curr_offset + curr_scale )[2];

    vital::vector_3d pt1 =
      cameras[i].back_project( points[i].template cast<double>(), h1 );
    vital::vector_3d pt2 =
      cameras[i].back_project( points[i].template cast<double>(), h2 );
    pts.push_back(
      std::pair< vital::vector_3d, vital::vector_3d >( pt1, pt2 ) );
  }

  // Get normalization factors for full point set
  vital::vector_3d scale = 0.5*( max_pos.matrix() - min_pos.matrix() );
  vital::vector_3d offset = 0.5*( max_pos.matrix() + min_pos.matrix() );

  vital::matrix_3x3d M = vital::matrix_3x3d::Zero();
  vital::vector_3d v(0., 0., 0.);

  for ( auto& pt : pts )
  {
    // Normalize points
    vital::vector_3d p = ( pt.first - offset ).cwiseQuotient( scale );
    vital::vector_3d x = ( pt.second - offset ).cwiseQuotient( scale );

    // Unit vector along ray
    vital::vector_3d unit_vec = ( x - p ).normalized();

    vital::matrix_3x3d tmp_mat =
      vital::matrix_3x3d::Identity() - unit_vec * unit_vec.transpose();
    M += tmp_mat;
    v += tmp_mat * p;
  }

  // Un-normalize before return
  return ( scale.cwiseProduct(
    M.colPivHouseholderQr().solve( v ) ) + offset ).cast<T>();
}

/// \cond DoxygenSuppress
#define INSTANTIATE_TRIANGULATE(T) \
template KWIVER_ALGO_MVG_EXPORT Eigen::Matrix<T,3,1> \
         triangulate_fast_two_view( \
            const vital::simple_camera_perspective &camera0, \
            const vital::simple_camera_perspective &camera1, \
            const Eigen::Matrix<T, 2, 1> &point0, \
            const Eigen::Matrix<T, 2, 1> &point1); \
template KWIVER_ALGO_MVG_EXPORT Eigen::Matrix<T,4,1> \
         triangulate_homog( \
            const std::vector<vital::simple_camera_perspective >& cameras, \
            const std::vector<Eigen::Matrix<T,2,1> >& points); \
template KWIVER_ALGO_MVG_EXPORT Eigen::Matrix<T,3,1> \
         triangulate_inhomog( \
            const std::vector<vital::simple_camera_perspective >& cameras, \
            const std::vector<Eigen::Matrix<T,2,1> >& points); \
template KWIVER_ALGO_MVG_EXPORT Eigen::Matrix<T,3,1> \
         triangulate_rpc( \
            const std::vector<vital::simple_camera_rpc >& cameras, \
            const std::vector<Eigen::Matrix<T,2,1> >& points);

INSTANTIATE_TRIANGULATE(double);
INSTANTIATE_TRIANGULATE(float);

#undef INSTANTIATE_TRIANGULATE
/// \endcond

} // end namespace mvg
} // end namespace arrows
} // end namespace kwiver


//---------------------------------------------------------------------------//
// Copyright (c) 2020-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Ilias Khairullin <ilias@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_ALGEBRA_CURVES_HASH_TO_CURVE_EXPAND_HPP
#define CRYPTO3_ALGEBRA_CURVES_HASH_TO_CURVE_EXPAND_HPP

#include <nil/crypto3/algebra/curves/detail/h2c/h2c_suites.hpp>
#include <nil/crypto3/algebra/curves/detail/h2c/h2c_sgn0.hpp>

#include <nil/crypto3/algebra/algorithms/strxor.hpp>

#include <nil/crypto3/algebra/curves/bls12.hpp>

#include <nil/crypto3/hash/algorithm/hash.hpp>
#include <nil/crypto3/hash/accumulators/hash.hpp>

#include <boost/assert.hpp>
#include <boost/static_assert.hpp>
#include <boost/concept/assert.hpp>

#include <array>
#include <type_traits>
#include <iterator>

namespace nil {
    namespace crypto3 {
        namespace algebra {
            namespace curves {
                namespace detail {
                    using namespace nil::crypto3::detail;
                    template<std::size_t k, typename HashType,
                             /// HashType::digest_type is required to be uint8_t[]
                             typename = typename std::enable_if<
                                 std::is_same<std::uint8_t, typename HashType::digest_type::value_type>::value>::type>
                    class expand_message_xmd {
                        // https://tools.ietf.org/html/draft-irtf-cfrg-hash-to-curve-10#section-5.4.1
                        BOOST_STATIC_ASSERT_MSG(HashType::block_bits % 8 == 0, "r_in_bytes is not a multiple of 8");
                        BOOST_STATIC_ASSERT_MSG(HashType::digest_bits % 8 == 0, "b_in_bytes is not a multiple of 8");
                        BOOST_STATIC_ASSERT_MSG(HashType::digest_bits >= 2 * k,
                                                "k-bit collision resistance is not fulfilled");

                        constexpr static const std::size_t b_in_bytes = HashType::digest_bits / 8;
                        constexpr static const std::size_t r_in_bytes = HashType::block_bits / 8;

                        constexpr static const std::array<std::uint8_t, r_in_bytes> Z_pad {0};

                    public:
                        template<typename InputMsgType, typename InputDstType, typename OutputType,
                                 typename = typename std::enable_if<
                                     std::is_same<std::uint8_t, typename InputMsgType::value_type>::value &&
                                     std::is_same<std::uint8_t, typename InputDstType::value_type>::value &&
                                     std::is_same<std::uint8_t, typename OutputType::value_type>::value>::type>
                        static inline void process(const std::size_t len_in_bytes, const InputMsgType &msg,
                                                   const InputDstType &dst, OutputType &uniform_bytes) {
                            BOOST_CONCEPT_ASSERT((boost::SinglePassRangeConcept<InputMsgType>));
                            BOOST_CONCEPT_ASSERT((boost::SinglePassRangeConcept<InputDstType>));
                            BOOST_CONCEPT_ASSERT((boost::SinglePassRangeConcept<OutputType>));
                            BOOST_CONCEPT_ASSERT((boost::WriteableRangeConcept<OutputType>));

                            // https://tools.ietf.org/html/draft-irtf-cfrg-hash-to-curve-10#section-5.4.1
                            BOOST_ASSERT(len_in_bytes < 0x10000);
                            BOOST_ASSERT(std::distance(dst.begin(), dst.end()) >= 16 &&
                                         std::distance(dst.begin(), dst.end()) <= 255);
                            BOOST_ASSERT(std::distance(uniform_bytes.begin(), uniform_bytes.end()) >= len_in_bytes);

                            const std::array<std::uint8_t, 2> l_i_b_str = {
                                static_cast<std::uint8_t>(len_in_bytes >> 8u),
                                static_cast<std::uint8_t>(len_in_bytes % 0x100)};
                            const std::size_t ell = static_cast<std::size_t>(len_in_bytes / b_in_bytes) +
                                                    static_cast<std::size_t>(len_in_bytes % b_in_bytes != 0);

                            // https://tools.ietf.org/html/draft-irtf-cfrg-hash-to-curve-10#section-5.4.1
                            BOOST_ASSERT(ell <= 255);

                            // TODO: use accumulators when they will be fixed
                            // accumulator_set<HashType> b0_acc;
                            // hash<HashType>(Z_pad, b0_acc);
                            // hash<HashType>(msg, b0_acc);
                            // hash<HashType>(l_i_b_str, b0_acc);
                            // hash<HashType>(std::array<std::uint8_t, 1> {0}, b0_acc);
                            // hash<HashType>(dst, b0_acc);
                            // hash<HashType>(std::array<std::uint8_t, 1> {static_cast<std::uint8_t>(dst.size())},
                            // b0_acc); typename HashType::digest_type b0 =
                            // accumulators::extract::hash<HashType>(b0_acc);
                            std::vector<std::uint8_t> msg_prime;
                            msg_prime.insert(msg_prime.end(), Z_pad.begin(), Z_pad.end());
                            msg_prime.insert(msg_prime.end(), msg.begin(), msg.end());
                            msg_prime.insert(msg_prime.end(), l_i_b_str.begin(), l_i_b_str.end());
                            msg_prime.insert(msg_prime.end(), static_cast<std::uint8_t>(0));
                            msg_prime.insert(msg_prime.end(), dst.begin(), dst.end());
                            msg_prime.insert(msg_prime.end(),
                                             static_cast<std::uint8_t>(std::distance(dst.begin(), dst.end())));
                            typename HashType::digest_type b0 = hash<HashType>(msg_prime);

                            // TODO: use accumulators when they will be fixed
                            // accumulator_set<HashType> bi_acc;
                            // hash<HashType>(b0, bi_acc);
                            // hash<HashType>(std::array<std::uint8_t, 1> {1}, bi_acc);
                            // hash<HashType>(dst, bi_acc);
                            // hash<HashType>(std::array<std::uint8_t, 1> {static_cast<std::uint8_t>(dst.size())},
                            // bi_acc); typename HashType::digest_type bi =
                            // accumulators::extract::hash<HashType>(bi_acc); std::copy(bi.begin(), bi.end(),
                            // uniform_bytes.begin());
                            std::vector<std::uint8_t> b_i_str;
                            b_i_str.insert(b_i_str.end(), b0.begin(), b0.end());
                            b_i_str.insert(b_i_str.end(), static_cast<std::uint8_t>(1));
                            b_i_str.insert(b_i_str.end(), dst.begin(), dst.end());
                            b_i_str.insert(b_i_str.end(),
                                           static_cast<std::uint8_t>(std::distance(dst.begin(), dst.end())));
                            typename HashType::digest_type bi = hash<HashType>(b_i_str);
                            std::copy(bi.begin(), bi.end(), uniform_bytes.begin());

                            typename HashType::digest_type xored_b;
                            for (std::size_t i = 2; i <= ell; i++) {
                                // TODO: use accumulators when they will be fixed
                                // accumulator_set<HashType> bi_acc;
                                // strxor(b0, bi, xored_b);
                                // hash<HashType>(xored_b, bi_acc);
                                // hash<HashType>(std::array<std::uint8_t, 1> {static_cast<std::uint8_t>(i)}, bi_acc);
                                // hash<HashType>(dst, bi_acc);
                                // hash<HashType>(std::array<std::uint8_t, 1> {static_cast<std::uint8_t>(dst.size())},
                                //                bi_acc);
                                // bi = accumulators::extract::hash<HashType>(bi_acc);
                                // std::copy(bi.begin(), bi.end(), uniform_bytes.begin() + (i - 1) * b_in_bytes);
                                strxor(b0, bi, xored_b);
                                std::vector<std::uint8_t> b_i_str;
                                b_i_str.insert(b_i_str.end(), xored_b.begin(), xored_b.end());
                                b_i_str.insert(b_i_str.end(), static_cast<std::uint8_t>(i));
                                b_i_str.insert(b_i_str.end(), dst.begin(), dst.end());
                                b_i_str.insert(b_i_str.end(),
                                               static_cast<std::uint8_t>(std::distance(dst.begin(), dst.end())));
                                bi = hash<HashType>(b_i_str);
                                std::copy(bi.begin(), bi.end(), uniform_bytes.begin() + (i - 1) * b_in_bytes);
                            }
                        }
                    };
                }    // namespace detail
            }        // namespace curves
        }            // namespace algebra
    }                // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_ALGEBRA_CURVES_HASH_TO_CURVE_EXPAND_HPP


#ifndef ISOLATOR_SHREDDER_HPP
#define ISOLATOR_SHREDDER_HPP

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/random/uniform_01.hpp>

#include "common.hpp"
#include "nlopt/nlopt.h"

// A fast, generic, univariate slice sampler implementatation.
class Shredder
{
    public:
        Shredder(double lower_limit, double upper_limit, double tolerance);
        virtual ~Shredder();

        // change the tolerance
        void set_tolerance(double tolerance);

        // Generate a new sample given the current value x0.
        double sample(rng_t& rng, double x0);

        // Generate a maximum likelihood estimate
        double optimize(double x0);

        // These are kept as public fields to ease debugging and diagnostics.
        double x_min, x_max;

    protected:
        // Probability function. Return the log-probibility at x, as well as the
        // derivative d.
        virtual double f(double x, double& d) = 0;

        // Bounds on the parameter being sampled over
        double lower_limit, upper_limit, tolerance;

        boost::random::uniform_01<double> random_uniform_01;

    private:
        double find_slice_edge(double x0, double slice_height, double lp0,
                               double d0, int direction);

        nlopt_opt opt;

        friend double shredder_opt_objective(unsigned int _n, const double* _x,
                                             double* _grad, void* data);
};

// Some common distribution functions, with derivatives
//

#if 0
class PoissonLogPdf
{
    public:
        double f(float lambda, unsigned int k);
        double df_dlambda(float lambda, unsigned int k);
};
#endif

class NormalLogPdf
{
    public:
        double f(double mu, double sigma, const double* xs, size_t n);
        float f(float mu, float sigma, const float* xs, size_t n);

        double df_dx(double mu, double sigma, const double* xs, size_t n);

        double df_dmu(double mu, double sigma, const double* xs, size_t n);
        float df_dmu(float mu, float sigma, const float* xs, size_t n);

        double df_dsigma(double mu, double sigma, const double* xs, size_t n);
        float df_dsigma(float mu, float sigma, const float* xs, size_t n);

        double f(double mu, double sigma, double x);
        double df_dx(double mu, double sigma, double x);
};

class LogNormalLogPdf
{
    public:
        double f(double mu, double sigma, const double* xs, size_t n);
        double df_dx(double mu, double sigma, double x);
        double df_dmu(double mu, double sigma, const double* xs, size_t n);
        double df_dsigma(double mu, double sigma, const double* xs, size_t n);
};

#if 0
class StudentsTLogPdf
{
    public:
        double f(double nu, double mu, double sigma, const double* xs, size_t n);
        float f(float nu, float mu, float sigma, const float* xs, size_t n);
        double df_dx(double nu, double mu, double sigma, const double* xs, size_t n);
        double df_dmu(double nu, double mu, double sigma, const double* xs, size_t n);
        float df_dmu(float nu, float mu, float sigma, const float* xs, size_t n);
        double df_dsigma(double nu, double mu, double sigma, const double* xs, size_t n);
};
#endif

#if 0
class GammaLogPdf
{
    public:
        double f(double alpha, double beta, const double* xs, size_t n);
        float f(float alpha, float beta, const float* xs, size_t n);
        double df_dx(double alpha, double beta, const double* xs, size_t n);
        double df_dalpha(double alpha, double beta, const double* xs, size_t n);
        double df_dbeta(double alpha, double beta, const double* xs, size_t n);
        float df_dbeta(float alpha, float beta, const float* xs, size_t n);
};
#endif

#if 0
// Gamma parameterized by mean and shape
class AltGammaLogPdf
{
    public:
        double f(double mean, double shape, const double* xs, size_t n);
        float f(float mean, float shape, const float* xs, size_t n);
        double f(double mean, double shape, double x);
        float f(float mean, float shape, float x);
        double df_dx(double mean, double shape, const double* xs, size_t n);
        double df_dx(double mean, double shape, double x);
        double df_dmean(double mean, double shape, const double* xs, size_t n);
        float df_dmean(float mean, float shape, const float* xs, size_t n);
        double df_dshape(double shape, double mean, const double* xs, size_t n);
        float df_dshape(float shape, float mean, const float* xs, size_t n);
        float df_dshape(float mean, float shape, float x);
};
#endif

class InvGammaLogPdf
{
    public:
        double f(double alpha, double beta, const double* xs, size_t n);
        double df_dx(double alpha, double beta, const double* xs, size_t n);
        double df_dalpha(double alpha, double beta, const double* xs, size_t n);
        double df_dbeta(double alpha, double beta, const double* xs, size_t n);
};

class SqInvGammaLogPdf
{
    public:
        double f(double alpha, double beta, const double* xs, size_t n);
        double df_dx(double alpha, double beta, const double* xs, size_t n);
        double df_dalpha(double alpha, double beta, const double* xs, size_t n);
        double df_dbeta(double alpha, double beta, const double* xs, size_t n);
};

class BetaLogPdf
{
    public:
        double f(double alpha, double beta, double x);
        double df_dx(double alpha, double beta, double x);
        double df_dgamma(double gamma, double c, double x);
};

class DirichletLogPdf
{
    public:
        double f(double alpha,
                 const boost::numeric::ublas::matrix<double>* mean,
                 const boost::numeric::ublas::matrix<double>* data,
                 size_t n, size_t m);

        double df_dalpha(double alpha,
                         const boost::numeric::ublas::matrix<double>* mean,
                         const boost::numeric::ublas::matrix<double>* data,
                         size_t n, size_t m);
};

// 1-d logistic normal
class LogisticNormalLogPdf
{
    public:
        double f(double mu, double sigma, double x);
        double df_dx(double mu, double sigma, double x);

        // TODO: do these when we need them
        //double df_dmu(double x, double mu, double sigma);
        //double df_dsigma(double x, double mu ,double sigma);
};

#endif


#include "stdafx.h"
#include "pop.h"
#include "params.h"
#include "data.h"
#include "rnd.h"
#include "state.h"
#include "Line2Eqn.h"
#include "EvalEqnStr.h"
#include <unordered_map>
#if defined(_WIN32)
	#include <regex>
#else
	#include <boost/regex.hpp>
#endif
#include "FitnessEstimator.h"
#include "Fitness.h"

//void getEqnForm(std::string& eqn,std::string& eqn_form);
//float getCorr(vector<float>& output,vector<float>& target,float meanout,float meantarget,int off);
//int getComplexity(string& eqn);

//void getEqnForm(std::string& eqn,std::string& eqn_form)
//{
////replace numbers with the letter c
//#if defined(_WIN32)
//	std::regex e ("(([0-9]+)(\.)([0-9]+))|([0-9]+)");
//	std::basic_string<char> tmp = "c";
//	std::regex_replace (std::back_inserter(eqn_form), eqn.begin(), eqn.end(), e,tmp);
//#else
//	boost::regex e ("(([0-9]+)(\.)([0-9]+))|([0-9]+)");
//	eqn_form = boost::regex_replace(eqn,e,"c");
//#endif
//	//(\d+\.\d+)|(\d+)
//	//eqn_form = eqn;
//	//eqn_form=std::tr1::regex_replace(eqn,e,tmp.c_str(),std::tr1::regex_constants::match_default);
//	//std::string result;
//
//
//	//std::regex_replace(std::back_inserter(eqn_form),eqn.begin(),eqn.end(),e,"c",std::regex_constants::match_default);
//	//std::regex_replace(std::back_inserter(eqn_form),eqn.begin(),eqn.end(),e,"c",std::tr1::regex_constants::match_default
//    //std::cout << result;
//	//std::cout << eqn << "\t" << eqn_form <<"\n";
//}
//int getComplexity(string& eqn)
//{
//	int complexity=0;
//	char c;
//	for(int m=0;m<eqn.size();m++){
//		c=eqn[m];
//
//		if(c=='/')
//			complexity=complexity+2;
//		else if (c=='s'){
//			if(m+2<eqn.size()){
//				if ( eqn[m+1]=='i' && eqn[m+2] == 'n'){
//					complexity=complexity+3;
//					m=m+2;
//				}
//			}
//		}
//		else if (c=='c'){
//			if(m+2<eqn.size()){
//				if ( eqn[m+1]=='o' && eqn[m+2] == 's'){
//					complexity=complexity+3;
//					m=m+2;
//				}
//			}
//		}
//		else if (c=='e'){
//			if(m+2<eqn.size()){
//				if ( eqn[m+1]=='x' && eqn[m+2] == 'p'){
//					complexity=complexity+4;
//					m=m+2;
//				}
//			}
//		}
//		else if (c=='l'){
//			if(m+2<eqn.size()){
//				if ( eqn[m+1]=='o' && eqn[m+2] == 'g'){
//					complexity=complexity+4;
//					m=m+2;
//				}
//			}
//		}
//		else if (isalpha(c) && (m+1)<eqn.size()){
//			bool pass=true;
//			while ((m+1)<eqn.size() && pass){
//				if (isalpha(eqn[m+1])) m++;
//				else pass=0;
//			}
//			complexity++;
//		}
//		else
//			complexity++;
//	}
//
//	return complexity;
//}
//void eval(node& n,vector<float>& outstack)
//{
//	switch(n.type)
//	{
//	case 'n':
//		outstack.push_back(n.value);
//		break;
//	case 'v':
//		if (n.valpt==NULL)
//			cout<<"problem";
//		else
//			outstack.push_back(*n.valpt);
//		break;
//	case '+':
//		if(outstack.size()>=2){
//				float n1 = outstack.back(); outstack.pop_back();
//				float n2 = outstack.back(); outstack.pop_back();
//
//				outstack.push_back(n2+n1);
//		}
//		break;
//	case '-':
//		if(outstack.size()>=2){
//				float n1 = outstack.back(); outstack.pop_back();
//				float n2 = outstack.back(); outstack.pop_back();
//
//				outstack.push_back(n2-n1);
//		}
//		break;
//	case '*':
//		if(outstack.size()>=2){
//				float n1 = outstack.back(); outstack.pop_back();
//				float n2 = outstack.back(); outstack.pop_back();
//
//				outstack.push_back(n2*n1);
//		}
//		break;
//	case '/':
//		if(outstack.size()>=2){
//				float n1 = outstack.back(); outstack.pop_back();
//				float n2 = outstack.back(); outstack.pop_back();
//				if(abs(n1)<0.0001)
//					outstack.push_back(0);
//				else
//					outstack.push_back(n2/n1);
//		}
//		break;
//	case 's':
//		if(outstack.size()>=1){
//				float n1 = outstack.back(); outstack.pop_back();
//				outstack.push_back(sin(n1));
//		}
//		break;
//	case 'c':
//		if(outstack.size()>=1){
//				float n1 = outstack.back(); outstack.pop_back();
//				outstack.push_back(cos(n1));
//		}
//		break;
//	case 'e':
//		if(outstack.size()>=1){
//				float n1 = outstack.back(); outstack.pop_back();
//				outstack.push_back(exp(n1));
//		}
//		break;
//	case 'l':
//		if(outstack.size()>=1){
//				float n1 = outstack.back(); outstack.pop_back();
//				if (abs(n1)<0.0001)
//					outstack.push_back(0);
//				else
//					outstack.push_back(log(n1));
//		}
//		break;
//	}
//}