/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file   testGroup.cpp
 * @brief  Unit tests for groups
 * @author Frank Dellaert
 **/

#include <gtsam/base/Group.h>
#include <gtsam/base/Testable.h>
#include <Eigen/Core>
#include <iostream>

namespace gtsam {

/// Symmetric group
template<int N>
class Symmetric: private Eigen::PermutationMatrix<N> {
  Symmetric(const Eigen::PermutationMatrix<N>& P) :
      Eigen::PermutationMatrix<N>(P) {
  }
public:
  static Symmetric identity() { return Symmetric(); }
  Symmetric() {
    Eigen::PermutationMatrix<N>::setIdentity();
  }
  static Symmetric Transposition(int i, int j) {
    Symmetric g;
    return g.applyTranspositionOnTheRight(i, j);
  }
  Symmetric operator*(const Symmetric& other) const {
    return Eigen::PermutationMatrix<N>::operator*(other);
  }
  bool operator==(const Symmetric& other) const {
    for (size_t i = 0; i < N; i++)
      if (this->indices()[i] != other.indices()[i])
        return false;
    return true;
  }
  Symmetric inverse() const {
    return Eigen::PermutationMatrix<N>(Eigen::PermutationMatrix<N>::inverse());
  }
  friend std::ostream &operator<<(std::ostream &os, const Symmetric& m) {
    for (size_t i = 0; i < N; i++)
      os << m.indices()[i] << " ";
    return os;
  }
  void print(const std::string& s = "") const {
    std::cout << s << *this << std::endl;
  }
  bool equals(const Symmetric<N>& other, double tol = 0) const {
    return this->indices() == other.indices();
  }
};

/// Define permutation group traits to be a model of the Multiplicative Group concept
template<int N>
struct traits<Symmetric<N> > : internal::MultiplicativeGroupTraits<Symmetric<N> >,
    Testable<Symmetric<N> > {
};

} // namespace gtsam

#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>

using namespace std;
using namespace gtsam;

//******************************************************************************
typedef Symmetric<2> S2;
TEST(Group, S2) {
  S2 e, s1 = S2::Transposition(0, 1);
  BOOST_CONCEPT_ASSERT((IsGroup<S2>));
  EXPECT(check_group_invariants(e, s1));
}

//******************************************************************************
typedef Symmetric<3> S3;
TEST(Group, S3) {
  S3 e, s1 = S3::Transposition(0, 1), s2 = S3::Transposition(1, 2);
  BOOST_CONCEPT_ASSERT((IsGroup<S3>));
  EXPECT(check_group_invariants(e, s1));
  EXPECT(assert_equal(s1, s1 * e));
  EXPECT(assert_equal(s1, e * s1));
  EXPECT(assert_equal(e, s1 * s1));
  S3 g = s1 * s2; // 1 2 0
  EXPECT(assert_equal(s1, g * s2));
  EXPECT(assert_equal(e, compose_pow(g, 0)));
  EXPECT(assert_equal(g, compose_pow(g, 1)));
  EXPECT(assert_equal(e, compose_pow(g, 3))); // g is generator of Z3 subgroup
}

//******************************************************************************
// The direct product of S2=Z2 and S3 is the symmetry group of a hexagon,
// i.e., the dihedral group of order 12 (denoted Dih6 because 6-sided polygon)
namespace gtsam {
typedef DirectProduct<S2, S3> Dih6;

std::ostream &operator<<(std::ostream &os, const Dih6& m) {
  os << "( " << m.first << ", " << m.second << ")";
  return os;
}

// Provide traits with Testable

template<>
struct traits<Dih6> : internal::MultiplicativeGroupTraits<Dih6> {
  static void Print(const Dih6& m, const string& s = "") {
    cout << s << m << endl;
  }
  static bool Equals(const Dih6& m1, const Dih6& m2, double tol = 1e-8) {
    return m1 == m2;
  }
};
} // namespace gtsam

TEST(Group, Dih6) {
  Dih6 e, g(S2::Transposition(0, 1),
      S3::Transposition(0, 1) * S3::Transposition(1, 2));
  BOOST_CONCEPT_ASSERT((IsGroup<Dih6>));
  EXPECT(check_group_invariants(e, g));
  EXPECT(assert_equal(e, compose_pow(g, 0)));
  EXPECT(assert_equal(g, compose_pow(g, 1)));
  EXPECT(assert_equal(e, compose_pow(g, 6))); // g is generator of Z6 subgroup
}

//******************************************************************************
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
//******************************************************************************


#include <imgui.h>
#include <pangolin/pangolin.h>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <chrono>

#include <imgui_impl_opengl3.h>
#include <imgui_impl_win32.h>

// Converts degrees to radians.
#define degreesToRadians(angleDegrees) (angleDegrees * M_PI / 180.0)

// Converts radians to degrees.
#define radiansToDegrees(angleRadians) (angleRadians * 180.0 / M_PI)

class RgoHandler3d : public pangolin::Handler3D {
   public:
    enum class KeysEnum : unsigned char {
        AXIS_ALIGN_YX = 'a',
        AXIS_ALIGN_XZ = 'd',
        AXIS_ALIGN_YZ = 'x',
        ROLL_NEG = 'q',
        ROLL_POS = 'e',
        PITCH_NEG = 'w',
        PITCH_POS = 's',
        YAW_NEG = 'z',
        YAW_POS = 'c',
        TOGGLE_WORLD_TO_MODEL = 'm'
    };

    RgoHandler3d::RgoHandler3d(pangolin::OpenGlRenderState& cam_state)
        : pangolin::Handler3D(cam_state) {}
    void Keyboard(pangolin::View& view, unsigned char key, int x, int y,
                  bool pressed) {
        if (!pressed) {
            return;
        }
        auto rotation_requested{false};
        Eigen::Matrix4d transform;
        transform.setIdentity();
        const Eigen::Matrix4d mvmat = cam_state->GetModelViewMatrix();
        Eigen::Quaternion<double> R_env_cam =
            Eigen::Quaternion<double>(mvmat.block<3, 3>(0, 0).transpose());
        R_env_cam.normalize();

        switch (key) {
            case pangolin::PANGO_SPECIAL + pangolin::PANGO_KEY_LEFT:
                transform.block<3, 1>(0, 3) =
                    R_env_cam * Eigen::Vector3d::UnitX() * move_factor_;
                break;
            case pangolin::PANGO_SPECIAL + pangolin::PANGO_KEY_RIGHT:
                transform.block<3, 1>(0, 3) =
                    R_env_cam * -Eigen::Vector3d::UnitX() * move_factor_;
                break;
            case pangolin::PANGO_SPECIAL + pangolin::PANGO_KEY_UP:
                transform.block<3, 1>(0, 3) =
                    R_env_cam * Eigen::Vector3d::UnitZ() * move_factor_;
                break;
            case pangolin::PANGO_SPECIAL + pangolin::PANGO_KEY_DOWN:
                transform.block<3, 1>(0, 3) =
                    R_env_cam * -Eigen::Vector3d::UnitZ() * move_factor_;
                break;
            case KeysEnum::AXIS_ALIGN_YX:
                transform.block<3, 3>(0, 0) = R_env_cam.matrix();
                break;
            case KeysEnum::AXIS_ALIGN_XZ:
                transform.block<3, 3>(0, 0) =
                    R_env_cam.matrix() *
                    Eigen::AngleAxis<double>(degreesToRadians(90),
                                             Eigen::Vector3d::UnitX())
                        .matrix();
                break;
            case KeysEnum::AXIS_ALIGN_YZ:
                transform.block<3, 3>(0, 0) =
                    R_env_cam *
                    Eigen::AngleAxis<double>(degreesToRadians(90),
                                             Eigen::Vector3d::UnitY())
                        .matrix();
                break;
            case KeysEnum::ROLL_NEG:
                transform.block<3, 3>(0, 0) =
                    Eigen::AngleAxis<double>(
                        degreesToRadians(-rotation_factor_deg_),
                        (R_env_cam * Eigen::Vector3d::UnitZ()).normalized())
                        .matrix();
                rotation_requested = true;
                break;
            case KeysEnum::ROLL_POS:
                transform.block<3, 3>(0, 0) =
                    Eigen::AngleAxis<double>(
                        degreesToRadians(rotation_factor_deg_),
                        (R_env_cam * Eigen::Vector3d::UnitZ()).normalized())
                        .matrix();
                rotation_requested = true;
                break;
            case KeysEnum::PITCH_NEG:
                transform.block<3, 3>(0, 0) =
                    Eigen::AngleAxis<double>(
                        degreesToRadians(-rotation_factor_deg_),
                        (R_env_cam * Eigen::Vector3d::UnitX()).normalized())
                        .matrix();
                rotation_requested = true;
                break;
            case KeysEnum::PITCH_POS:
                transform.block<3, 3>(0, 0) =
                    Eigen::AngleAxis<double>(
                        degreesToRadians(rotation_factor_deg_),
                        (R_env_cam * Eigen::Vector3d::UnitX()).normalized())
                        .matrix();
                rotation_requested = true;
                break;
            case KeysEnum::YAW_NEG:
                transform.block<3, 3>(0, 0) =
                    Eigen::AngleAxis<double>(
                        degreesToRadians(-rotation_factor_deg_),
                        (R_env_cam * Eigen::Vector3d::UnitY()).normalized())
                        .matrix();
                rotation_requested = true;
                break;
            case KeysEnum::YAW_POS:
                transform.block<3, 3>(0, 0) =
                    Eigen::AngleAxis<double>(
                        degreesToRadians(rotation_factor_deg_),
                        (R_env_cam * Eigen::Vector3d::UnitY()).normalized())
                        .matrix();
                rotation_requested = true;
                break;
            case KeysEnum::TOGGLE_WORLD_TO_MODEL:
                cam_around_world_ = !cam_around_world_;
                break;
        }

        Eigen::Matrix4d model_view = cam_around_world_ && rotation_requested
                                         ? transform * mvmat
                                         : mvmat * transform;
        cam_state->SetModelViewMatrix(model_view);
    }

   private:
    bool cam_around_world_{false};
    float move_factor_ = 0.4f;
    float rotation_factor_deg_ = 5.f;
};

namespace gl {
static void DrawAxisGizmo(const Eigen::Matrix4d& mvmat,
                          Eigen::Vector3d pos = {0.8, 0.8, 0},
                          float line_width = 2.5f, float axis_len = .075f) {
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();

    Eigen::Matrix4d transformation;
    transformation.setIdentity();
    transformation.block<3, 3>(0, 0) = mvmat.block<3, 3>(0, 0);
    transformation.block<3, 1>(0, 3) = pos;

    glPushMatrix();
    glMultMatrixd(transformation.data());
    glPushAttrib(GL_LINE_BIT);
    glLineWidth(line_width);
    pangolin::glDrawAxis(axis_len);
    glPopAttrib();
    glPopMatrix();
}

Eigen::Matrix4f lookAt(const Eigen::Vector3f& eye,
                       const Eigen::Vector3f& target,
                       const Eigen::Vector3f& up) {
    Eigen::Vector3f forward = eye - target;
    forward.normalize();
    Eigen::Vector3f left = up.cross(forward);
    left.normalize();

    Eigen::Matrix3f rotation;
    rotation << left, forward.cross(left), forward;

    Eigen::Matrix4f transformation;
    transformation.setIdentity();
    transformation.block<3, 3>(0, 0) = rotation.transpose();
    transformation.block<3, 1>(0, 3) =
        transformation.block<3, 3>(0, 0) * (-eye);

    return transformation;
}
}  // namespace gl

int main(int /*argc*/, char** /*argv*/) {
    const auto width = 640;
    const auto height = 480;

    auto& window = pangolin::CreateWindowAndBind("Main", width, height);

    // 3D Mouse handler requires depth testing to be enabled
    glEnable(GL_DEPTH_TEST);

    // Issue specific OpenGl we might need
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    const char* glsl_version = "#version 130";
    ImGui_ImplOpenGL3_Init(glsl_version);

    // Setup Dear ImGui style
    ImGui::StyleColorsClassic();
    // Define Projection and initial ModelView matrix
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(width, height, 420, 420, 320, 240, 0.2, 100),
        pangolin::ModelViewLookAt(-12, 2, -12, 0, 0, 0, pangolin::AxisY));

    // Add named OpenGL viewport to window and provide 3D Handler
    pangolin::View& d_cam =
        pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, 0.0, 1.0,
                       -640.0f / 480.0f)
            .SetHandler(new RgoHandler3d(s_cam));

    while (!pangolin::ShouldQuit()) {
        // Clear screen and activate view to render into
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        gl::DrawAxisGizmo(s_cam.GetModelViewMatrix());
        glEnable(GL_DEPTH_TEST);

        ImGui_ImplOpenGL3_NewFrame();
        ImGui_ImplWin32_NewFrame();
        {
            ImGui::NewFrame();
            static float f = 0.0f;
            static int counter = 0;

            ImGui::Begin("UI");
            ImGui::SliderFloat(
                "float", &f, 0.0f,
                1.0f);  // Edit 1 float using a slider from 0.0f to 1.0f
            if (ImGui::Button(
                    "Button"))  // Buttons return true when clicked (most
                                // widgets return true when edited/activated)
                counter++;
            ImGui::SameLine();
            ImGui::Text("counter = %d", counter);
            ImGui::Text("Application average %.3f ms/frame (%.1f FPS)",
                        1000.0f / ImGui::GetIO().Framerate,
                        ImGui::GetIO().Framerate);
            ImGui::End();
            ImGui::EndFrame();
        }

        d_cam.Activate(s_cam);

        // Render OpenGL Cube
        pangolin::glDrawColouredCube();
        pangolin::glDrawAxis(1.f);
        pangolin::glDraw_y0(10.0, 10);

        // Swap frames and Process Events
        ImGui::Render();
        ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData());
        pangolin::FinishFrame();
        using namespace std::chrono_literals;
        std::this_thread::sleep_for(16ms);
    }
    ImGui_ImplOpenGL3_Shutdown();

    return 0;
}


// Copyright 2018-2020 Coen Tempelaars (Falcons)
// SPDX-License-Identifier: Apache-2.0
/*
 * hmNearPosition.hpp
 *
 *  Created on: June 5, 2018
 *      Author: Jan Feitsma
 */

#include <boost/lexical_cast.hpp>

#include "int/heightmaps/hmNearPosition.hpp"

#include "falconsCommon.hpp"

#include "tracing.hpp"

using namespace teamplay;

hmNearPosition::hmNearPosition()
{
    reset();
}

hmNearPosition::~hmNearPosition()
{
}

void hmNearPosition::precalculate()
{
    // cannot do anything yet, need parameters
    return;
}

abstractHeightMap hmNearPosition::refine(const parameterMap_t& params)
{
    // configuration
    float alpha = 10.0; // magic number
    // extract parameters
    float positionX = 0.0;
    float positionY = 0.0;
    auto it = params.find("positionX");
    if (it != params.end())
    {
        positionX = boost::lexical_cast<float>(it->second);
    }
    it = params.find("positionY");
    if (it != params.end())
    {
        positionY = boost::lexical_cast<float>(it->second);
    }
    // calculate the height map
    for (unsigned int i = 0; i < getNrOfHeightMapFieldsInX(); i++)
    {
        for (unsigned int j = 0; j < getNrOfHeightMapFieldsInY(); j++)
        {
            auto distance = calc_distance(Point2D(positionX, positionY), _heightMap(i, j)._center);
            _heightMap(i, j).setValue(100.0 - alpha * distance);
        }
    }
    return *this;
}

std::string hmNearPosition::getDescription() const
{
    return "Near position";
}

std::string hmNearPosition::getFilename() const
{
    return "hmNearPosition";
}


#include "stdafx.h"
#include "DepthFunction.h"

#include "mmcore/param/EnumParam.h"
#include "mmcore/param/IntParam.h"
#include "mmcore/param/StringParam.h"

#include <Eigen/LU>
#include "HD/HD.h"

using namespace megamol;
using namespace megamol::infovis;
using namespace Eigen;

enum DepthType { HALFSPACE_DEPTH = 0, FUNCTIONAL_DEPTH, MAHALANOBIS_DEPTH, SIMPLICAL_DEPTH };

Eigen::VectorXd DepthFunction::halfSpaceDepth(Eigen::MatrixXd dataMatrix) {
    Eigen::VectorXd result = Eigen::VectorXd(dataMatrix.rows());

    int nPoints = dataMatrix.rows();
    int nDims = dataMatrix.cols();

    double** x;
    x = new double*[nPoints];

    for (int i = 0; i < nPoints; i++) {
        x[i] = new double[nDims];
        for (int j = 0; j < nDims; j++) x[i][j] = dataMatrix(i, j);
    }

    double* z;
    z = new double[dataMatrix.cols()];

    for (int pIndex = 0; pIndex < nPoints; pIndex++) {
        for (int j = 0; j < dataMatrix.cols(); j++) {
            z[j] = dataMatrix(pIndex, j);
        }
        result(pIndex) = HalfspaceDepth::HD_Comb2(z, x, nPoints, nDims);
    }

    for (int k = 0; k < nPoints; k++) delete[] x[k];
    delete[] x;
    delete[] z;

    return result;
}

double megamol::infovis::DepthFunction::binomialCoeff(int n, int k) {
    if (k == 0 || k == n) return 1;

    double result = 1.0;

    for (int i = 0; i < k; i++) {
        result *= (n + 1.0 - i) / i;
    }

    return result;
}

Eigen::VectorXd DepthFunction::functionalDepth(
    Eigen::MatrixXd dataMatrix, int samplesCount, int samplesLength, unsigned int seed) {
    srand(seed);

    Eigen::VectorXd result = Eigen::VectorXd(dataMatrix.rows());
    double binom = megamol::infovis::DepthFunction::binomialCoeff(dataMatrix.rows(), samplesCount);

    // count the hits for every data point (= row in the data matrix)
    for (int row = 0; row < dataMatrix.rows(); row++) {
        Eigen::RowVectorXd dataPoint = dataMatrix.row(row);

        int hitsCount = 0;

        // generate random samples of length samplesLength
        for (int sampleIndex = 0; sampleIndex < samplesCount; sampleIndex++) {
            Eigen::VectorXd rndIndices =
                (Eigen::VectorXd::Random(samplesLength).array() + 1.0) / 2.0 * (dataMatrix.rows() - 1.0);

            bool minMaxFound = true;

            // for every property of the data point (= columns)
            for (int dim = 0; dim < dataMatrix.cols(); dim++) {

                double dataPointValue = dataPoint(dim);

                bool dim_maxFound = false;
                bool dim_minFound = false;

                // for every data point in the random set, check if its value
                // is <= or >= than the current data point value
                // (equiv. to: is the data point contained in a random set of points?)
                for (int rndIndexCounter = 0; rndIndexCounter < samplesLength; rndIndexCounter++) {
                    int rndIndex = rndIndices(rndIndexCounter);

                    if (dataMatrix(rndIndex, dim) <= dataPointValue) dim_minFound = true;
                    if (dataMatrix(rndIndex, dim) >= dataPointValue) dim_maxFound = true;

                    if (dim_minFound && dim_maxFound) break;
                }

                // if a dimension was found where neither a min nor a max was found,
                // this datapoint isn't in the random set/sample
                if (!(dim_minFound && dim_maxFound)) {
                    minMaxFound = false;
                    break;
                }
            }

            if (minMaxFound) hitsCount++;
        }

        // result = 1 / binom(n, j) * hitsCount
        // where n = samplesLength and j = samplesCount
        result(row) = (double)hitsCount / (double)binom;
    }

    return result;
}

bool insideSimplexCheck(Eigen::VectorXd p, Eigen::MatrixXd x) {
    Eigen::MatrixXd tmp(x.rows() + 1, x.cols());
    tmp << x.transpose(), Eigen::MatrixXd::Ones(x.cols(), 1);
    tmp.transposeInPlace();
    double nominator = tmp.determinant();

    tmp.block(0, 0, x.rows(), 1) = p;

    double sum = 0;

    for (int pointIndex = 0; pointIndex < x.cols(); pointIndex++) {

        if (pointIndex != 0)
            tmp.block(0, pointIndex, x.rows(), 1) = x.block(0, pointIndex - 1, x.rows(), 1); // update matrix

        double sign = ((pointIndex % 2) * (-2.0) + 1.0);     // pow(-1,pointIndex)
        double alpha = sign * tmp.determinant() / nominator; // barycentric coordinate

        sum += alpha;
        if (alpha > 1.0 || alpha < 0.0) return false;
    }

    return true;
}

Eigen::VectorXd DepthFunction::simplicalDepth(Eigen::MatrixXd dataMatrix, int samplesCount, unsigned int seed) {
    srand(seed);

    int nPoints = dataMatrix.rows();
    int nDims = dataMatrix.cols();

    Eigen::VectorXd result = Eigen::VectorXd(dataMatrix.rows());

    for (int row = 0; row < dataMatrix.rows(); row++) {
        Eigen::VectorXd dataPoint = dataMatrix.row(row).transpose();

        int hitsCount = 0;

        for (int sampleIndex = 0; sampleIndex < samplesCount; sampleIndex++) {
            Eigen::VectorXd rndIndices =
                (Eigen::VectorXd::Random(nDims + 1).array() + 1.0) / 2.0 * (dataMatrix.rows() - 1.0);

            Eigen::MatrixXd vertices(nDims, nDims + 1);

            for (int rndIndexCounter = 0; rndIndexCounter < nDims + 1; rndIndexCounter++) {
                int rndIndex = rndIndices(rndIndexCounter);

                vertices.block(0, rndIndexCounter, nDims, 1) = dataMatrix.row(rndIndex).transpose();
            }

            if (insideSimplexCheck(dataPoint, vertices)) hitsCount++;
        }

        result(row) = (double)hitsCount / (double)samplesCount;
    }

    return result;
}

Eigen::MatrixXd DepthFunction::mahalanobisDepth(Eigen::MatrixXd dataMatrix) {
    int nPoints = dataMatrix.rows();

    Eigen::MatrixXd mahalaDepth = Eigen::MatrixXd::Zero(nPoints, 1);
    Eigen::MatrixXd invS =
        (1.0 / (nPoints - 1.0) * dataMatrix.transpose() *
            (Eigen::MatrixXd::Identity(nPoints, nPoints) - 1.0 / nPoints * Eigen::MatrixXd::Ones(nPoints, nPoints)) *
            dataMatrix)
            .inverse(); // inverse covariance Matrix

    Eigen::MatrixXd mu = 1.0 / nPoints * dataMatrix.transpose() * Eigen::MatrixXd::Ones(nPoints, 1);
    Eigen::MatrixXd xi;

    //#pragma omp parallel for //doesn't work
    for (int index = 0; index < nPoints; index++) {
        xi = dataMatrix.row(index).transpose();
        mahalaDepth(index, 0) = 1.0 / (1.0 + ((xi - mu).transpose() * invS * (xi - mu))(0, 0));
    }
    return mahalaDepth;
}

std::vector<std::string> split(std::string str, std::string token) {
    std::vector<std::string> result;
    while (str.size()) {
        int index = str.find(token);
        if (index != std::string::npos) {
            result.push_back(str.substr(0, index));
            str = str.substr(index + token.size());
            if (str.size() == 0) result.push_back(str);
        } else {
            result.push_back(str);
            str = "";
        }
    }
    return result;
}

/// Parse string a with format columnIndex1;columnIndex2,columnIndex3,...;...
std::vector<std::vector<int>> parseColumnGroups(std::string& columnGroupString) {
    std::vector<std::vector<int>> columnGroups;
    auto groupStrings = split(columnGroupString, ";");
    for (auto groupString : groupStrings) {
        auto columnStrings = split(groupString, ",");
        std::vector<int> columns;
        for (auto columnString : columnStrings) {
            columns.push_back(std::stoi(columnString));
        }
        columnGroups.push_back(columns);
    }
    return columnGroups;
}

// ---------------------------------------------------------------------------

DepthFunction::DepthFunction(void)
    : megamol::core::Module()
    , dataOutSlot("dataOut", "Ouput")
    , dataInSlot("dataIn", "Input")
    , datahash(0)
    , dataInHash(0)
    , columnInfos()
    , columnGroupsSlot("columnGroups", "Semicolon-separated groups of comma-separated column indices to compute "
                                       "the data depth for. Defaults to one group, all columns.")
    , depthType("depthType", "The depth function to use for computing the depth statistics")
    , sampleCount("sampleCount", "The number of samples, that will be drawn (functional depth only)")
    , sampleLength("sampleLength", "The length of one sample (functional depth only)")
    , randomSeed("randomSeed", "The random seed (functional depth only)") {

    // Data input slot
    this->dataInSlot.SetCompatibleCall<megamol::stdplugin::datatools::table::TableDataCallDescription>();
    this->MakeSlotAvailable(&this->dataInSlot);

    // Data output slot
    this->dataOutSlot.SetCallback(megamol::stdplugin::datatools::table::TableDataCall::ClassName(),
        megamol::stdplugin::datatools::table::TableDataCall::FunctionName(0), &DepthFunction::getDataCallback);
    this->dataOutSlot.SetCallback(megamol::stdplugin::datatools::table::TableDataCall::ClassName(),
        megamol::stdplugin::datatools::table::TableDataCall::FunctionName(1), &DepthFunction::getHashCallback);
    this->MakeSlotAvailable(&this->dataOutSlot);

    // Parameters
    columnGroupsSlot << new ::megamol::core::param::StringParam("");
    this->MakeSlotAvailable(&columnGroupsSlot);

    auto types = new ::megamol::core::param::EnumParam(1);
    types->SetTypePair(HALFSPACE_DEPTH, "Halfspace Depth");
    types->SetTypePair(FUNCTIONAL_DEPTH, "Functional Band Depth");
    types->SetTypePair(MAHALANOBIS_DEPTH, "Mahalanobis Depth");
    types->SetTypePair(SIMPLICAL_DEPTH, "Simplical Depth");
    depthType << types;
    this->MakeSlotAvailable(&depthType);

    sampleCount << new ::megamol::core::param::IntParam(20000);
    this->MakeSlotAvailable(&sampleCount);

    sampleLength << new ::megamol::core::param::IntParam(10);
    this->MakeSlotAvailable(&sampleLength);

    randomSeed << new ::megamol::core::param::IntParam(1337);
    this->MakeSlotAvailable(&randomSeed);

    // Add all parameters to common-parameter-handling list
    params.push_back(&columnGroupsSlot);
    params.push_back(&depthType);
    params.push_back(&sampleCount);
    params.push_back(&sampleLength);
    params.push_back(&randomSeed);
}

DepthFunction::~DepthFunction(void) { this->Release(); }

bool DepthFunction::create(void) { return true; }

void DepthFunction::release(void) {}

bool DepthFunction::getDataCallback(core::Call& c) {
    try {
        megamol::stdplugin::datatools::table::TableDataCall* outCall =
            dynamic_cast<megamol::stdplugin::datatools::table::TableDataCall*>(&c);
        if (outCall == NULL) return false;

        megamol::stdplugin::datatools::table::TableDataCall* inCall =
            this->dataInSlot.CallAs<megamol::stdplugin::datatools::table::TableDataCall>();
        if (inCall == NULL) return false;

        inCall->SetFrameID(outCall->GetFrameID()); // inCall->Set? not outCall???
        if (!(*inCall)()) return false;

        bool finished = apply(inCall);
        if (finished == false) return false;

        outCall->SetFrameCount(inCall->GetFrameCount());
        outCall->SetDataHash(this->datahash);

        // set outCall
        if (this->columnInfos.size() != 0) {
            outCall->Set(this->columnInfos.size(), this->data.size() / this->columnInfos.size(),
                this->columnInfos.data(), this->data.data());
        } else {
            outCall->Set(0, 0, NULL, NULL);
        }

    } catch (...) {
        vislib::sys::Log::DefaultLog.WriteError(_T("Failed to execute %hs::processData\n"), ClassName());
        return false;
    }

    return true;
}

bool DepthFunction::getHashCallback(core::Call& c) {
    try {
        megamol::stdplugin::datatools::table::TableDataCall* outCall =
            dynamic_cast<megamol::stdplugin::datatools::table::TableDataCall*>(&c);
        if (outCall == NULL) return false;

        megamol::stdplugin::datatools::table::TableDataCall* inCall =
            this->dataInSlot.CallAs<megamol::stdplugin::datatools::table::TableDataCall>();
        if (inCall == NULL) return false;

        inCall->SetFrameID(outCall->GetFrameID());
        if (!(*inCall)(1)) return false;

        outCall->SetFrameCount(inCall->GetFrameCount());
        outCall->SetDataHash(this->datahash);
    } catch (...) {
        vislib::sys::Log::DefaultLog.WriteError(_T("Failed to execute %hs::getHashCallback\n"), ClassName());
        return false;
    }

    return true;
}

void megamol::infovis::DepthFunction::assertData(megamol::stdplugin::datatools::table::TableDataCall* inCall) {
    auto columnCountIn = inCall->GetColumnsCount();
    auto columnCountOut = inCall->GetColumnsCount();
    auto rowsCount = inCall->GetRowsCount();
    auto inData = inCall->GetData();

    // Load data in a Matrix
    inDataMat = Eigen::MatrixXd(rowsCount, columnCountOut);
    for (int row = 0; row < rowsCount; row++) {
        for (int col = 0; col < columnCountIn; col++) {
            inDataMat(row, col) = inData[row * columnCountIn + col];
        }
    }
}

bool megamol::infovis::DepthFunction::paramsIsDirty() {
    for (auto param : params) {
        if (param->IsDirty()) return true;
    }

    return false;
}

void megamol::infovis::DepthFunction::paramsResetDirty() {
    for (auto param : params) {
        param->ResetDirty();
    }
}

bool megamol::infovis::DepthFunction::apply(megamol::stdplugin::datatools::table::TableDataCall* inCall) {
    // Test if input data or parameters have changed.
    if (this->dataInHash == inCall->DataHash() && !paramsIsDirty()) {
        // Do nothing since parameters are the same as before
        return true;
    }

    assertData(inCall);

    std::string columnGroupsString(this->columnGroupsSlot.Param<core::param::StringParam>()->Value().PeekBuffer());
    std::vector<std::vector<int>> columnGroups = parseColumnGroups(columnGroupsString);

    // Default to one group, containing all columns, if empty.
    if (columnGroups.empty()) {
        std::vector<int> columns;
        for (int i = 0; i < inDataMat.cols(); ++i) {
            columns.push_back(i);
        }
        columnGroups.push_back(columns);
    }

    // Compute depths for all column groups.
    Eigen::MatrixXd groupDepths = Eigen::MatrixXd::Zero(inDataMat.rows(), columnGroups.size());
    for (int group = 0; group < columnGroups.size(); group++) {
        auto columns = columnGroups[group];

        // Copy the columns together.
        Eigen::MatrixXd columnGroupMat = Eigen::MatrixXd::Zero(inDataMat.rows(), columns.size());
        for (int i = 0; i < columns.size(); i++) {
            auto column = columns[i];
            columnGroupMat.block(0, i, inDataMat.rows(), 1) = inDataMat.block(0, column, inDataMat.rows(), 1);
        }

        // Compute depth function.
        switch (this->depthType.Param<core::param::EnumParam>()->Value()) {
        case HALFSPACE_DEPTH:
            groupDepths.block(0, group, inDataMat.rows(), 1) = halfSpaceDepth(columnGroupMat);
            break;
        case FUNCTIONAL_DEPTH:
            groupDepths.block(0, group, inDataMat.rows(), 1) =
                functionalDepth(columnGroupMat, this->sampleCount.Param<core::param::IntParam>()->Value(),
                    this->sampleLength.Param<core::param::IntParam>()->Value(),
                    this->randomSeed.Param<core::param::IntParam>()->Value());
            break;
        case MAHALANOBIS_DEPTH:
            groupDepths.block(0, group, inDataMat.rows(), 1) = mahalanobisDepth(columnGroupMat);
            break;
        case SIMPLICAL_DEPTH:
            groupDepths.block(0, group, inDataMat.rows(), 1) =
                simplicalDepth(inDataMat, this->sampleCount.Param<core::param::IntParam>()->Value(),
                    this->randomSeed.Param<core::param::IntParam>()->Value());
            break;
        }
    }

    // Generate new column infos.
    this->columnInfos.clear();
    this->columnInfos.resize(groupDepths.cols());

    for (int group = 0; group < groupDepths.cols(); group++) {
        auto depths = groupDepths.col(group);
        std::string name = "Depth";
        for (int i = 0; i < columnGroups[group].size(); ++i) {
            name += " " + inCall->GetColumnsInfos()[columnGroups[group][i]].Name();
        }
        this->columnInfos[group]
            .SetName(name)
            .SetType(megamol::stdplugin::datatools::table::TableDataCall::ColumnType::QUANTITATIVE)
            .SetMinimumValue(depths.minCoeff())
            .SetMaximumValue(depths.maxCoeff());
    }

    // Set matrix as output.
    this->data.clear();
    this->data.reserve(groupDepths.rows() * groupDepths.cols());
    for (size_t row = 0; row < groupDepths.rows(); row++) {
        for (int group = 0; group < groupDepths.cols(); group++) {
            this->data.push_back(groupDepths(row, group));
        }
    }

    // Update hash.
    this->dataInHash = inCall->DataHash();
    this->datahash++;

    // Reset parameters.
    paramsResetDirty();

    return true;
}


// Refer to the paper: "Robust Point Cloud Based Reconstruction of Large-Scale Outdoor Scenes" in CVPR 2019
// Link: https://arxiv.org/abs/1905.09634

// This file is an implememntation of the Gaussian-Uniform mixture model as described in Sec. 5 of the paper.
// The Gaussian-Uniform mixture model degenerates to the approach in Choi et al. "Robust Reconstruction of Indoor Scenes" in CVPR 2017.
// This implementation adopts some techniques used by Choi et al.

#include <ceres/ceres.h>
#include <iostream>
#include <sophus/se3.hpp>

#include <vector>
#include <fstream>
#include <Eigen/Core>

#include <iomanip>
#include <algorithm>

#include "local_parameterization_se3.hpp"

#define NUM_ITERATION_EM 1000

#define EPSILON 6                             // upper bound of feature matching errors, used in Eq. (27)
#define M_HAT EPSILON * EPSILON               // mean error, used in Eq. (27)
#define P_HAT 0.9                             // trust level, used in Eq. (27)
#define THETA P_HAT / ( 1.0 - P_HAT) * M_HAT  // THETA is solved using Eq. (27)

#define THESHOLD_TO_BE_AN_INLIER_LOOP 0.8 * P_HAT    // theshold to prune outlier loop closures, used to generate results in Tab. 1 and 2

typedef Eigen::Matrix< double, 6, 6, Eigen::RowMajor > InformationMatrix;
typedef Eigen::Matrix< double, 6, 1> Vector6d;

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;

struct FramedTransformation {
  int id1_;
  int id2_;
  int frame_;
  Eigen::Matrix4d transformation_;      // pose in matrix form
  Sophus::SE3d transformation_se3_;     // pose in se3 form

  FramedTransformation( int id1, int id2, int f, Eigen::Matrix4d t )
    : id1_( id1 ), id2_( id2 ), frame_( f ), transformation_( t )
  {
    Eigen::Quaterniond q;
    q = t.block<3,3>(0,0);
    transformation_se3_ = Sophus::SE3d(q, Sophus::SE3d::Point(t(0,3), t(1,3), t(2,3)));
  }

  FramedTransformation(int id1, int id2, int f, Sophus::SE3d t)
    : id1_( id1 ), id2_( id2 ), frame_( f ), transformation_se3_( t )
    {
      transformation_ = t.matrix();
    }
};

struct PCLTrajectory {
  std::vector< FramedTransformation > data_;
  int index_;

  void LoadFromFile(const char* filename ) {
    data_.clear();
    index_ = 0;
    int id1, id2, frame;
    Eigen::Matrix4d trans;
    FILE * f = fopen( filename, "r" );
    if ( f != NULL ) {
      char buffer[1024];
      while ( fgets( buffer, 1024, f ) != NULL ) {
        if ( strlen( buffer ) > 0 && buffer[ 0 ] != '#' ) {
          sscanf( buffer, "%d %d %d", &id1, &id2, &frame);
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf", &trans(0,0), &trans(0,1), &trans(0,2), &trans(0,3) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf", &trans(1,0), &trans(1,1), &trans(1,2), &trans(1,3) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf", &trans(2,0), &trans(2,1), &trans(2,2), &trans(2,3) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf", &trans(3,0), &trans(3,1), &trans(3,2), &trans(3,3) );
          data_.push_back( FramedTransformation( id1, id2, frame, trans ) );
        }
      }
      fclose( f );
    }
  }

  void SaveToFile(const char* filename ) {
    FILE * f = fopen( filename, "w" );
    for ( int i = 0; i < ( int )data_.size(); i++ ) {
      Sophus::SE3d trans_se3 = data_[ i ].transformation_se3_;
      Eigen::Matrix4d trans = trans_se3.matrix();
      fprintf( f, "%d\t%d\t%d\n", data_[ i ].id1_, data_[ i ].id2_, data_[ i ].frame_ );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(0,0), trans(0,1), trans(0,2), trans(0,3) );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(1,0), trans(1,1), trans(1,2), trans(1,3) );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(2,0), trans(2,1), trans(2,2), trans(2,3) );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(3,0), trans(3,1), trans(3,2), trans(3,3) );

    }
    fclose( f );
  }

};

struct FramedInformation {
  int id1_;
  int id2_;
  int frame_;
  InformationMatrix information_;

  FramedInformation( int id1, int id2, int f, InformationMatrix t )
    : id1_( id1 ), id2_( id2 ), frame_( f ), information_( t )
  {}
};

struct PCLInformation {
  std::vector< FramedInformation > data_;

  void LoadFromFile(const char*  filename ) {
    data_.clear();
    int id1, id2, frame;
    InformationMatrix info;
    FILE * f = fopen( filename, "r" );
    if ( f != NULL ) {
      char buffer[1024];
      while ( fgets( buffer, 1024, f ) != NULL ) {
        if ( strlen( buffer ) > 0 && buffer[ 0 ] != '#' ) {
          sscanf( buffer, "%d %d %d", &id1, &id2, &frame);
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf %lf %lf", &info(0,0), &info(0,1), &info(0,2), &info(0,3), &info(0,4), &info(0,5) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf %lf %lf", &info(1,0), &info(1,1), &info(1,2), &info(1,3), &info(1,4), &info(1,5) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf %lf %lf", &info(2,0), &info(2,1), &info(2,2), &info(2,3), &info(2,4), &info(2,5) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf %lf %lf", &info(3,0), &info(3,1), &info(3,2), &info(3,3), &info(3,4), &info(3,5) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf %lf %lf", &info(4,0), &info(4,1), &info(4,2), &info(4,3), &info(4,4), &info(4,5) );
          fgets( buffer, 1024, f );
          sscanf( buffer, "%lf %lf %lf %lf %lf %lf", &info(5,0), &info(5,1), &info(5,2), &info(5,3), &info(5,4), &info(5,5) );
          data_.push_back( FramedInformation( id1, id2, frame, info ) );
        }
      }
      fclose( f );
    }
  }
};

struct GaussianFunctor {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  GaussianFunctor(Sophus::SE3d trans_next_to_current, InformationMatrix info, int id1, int id2)
  : trans_next_to_current_(trans_next_to_current), info_(info), id1_(id1), id2_(id2) {}

  template <typename T>
  bool operator()(T const* const sT_current, T const* const sT_next,
                  T* residual) const {

    Eigen::Map<Sophus::SE3<T> const> const T_current(sT_current);
    Eigen::Map<Sophus::SE3<T> const> const T_next(sT_next);

    Sophus::SE3<T> xi_7T = trans_next_to_current_
                          * T_next.inverse()
                          * T_current;

    // xi: local parameterization as in Choi et al.
    Eigen::Matrix<T,6,1> xi;
    xi << xi_7T.data()[4], xi_7T.data()[5], xi_7T.data()[6], xi_7T.data()[0], xi_7T.data()[1], xi_7T.data()[2];

    Eigen::Matrix<T,1,1> sq_error = xi.transpose() * info_ * xi;
    residual[0] = T( sqrt(sq_error(0,0)) );

    return true;
  }

private:
  const Sophus::SE3d trans_next_to_current_;
  const InformationMatrix info_;
  const int id1_, id2_;
};

class RobustPCLReconstruction_GaussianUniform {
public:

  // Odometry contraints
  PCLTrajectory odometry_log_;
  PCLInformation odometry_info_;

  // Loop closure contraints
  PCLTrajectory loop_log_;
  PCLInformation loop_info_;

  // Fragment poses
  PCLTrajectory fragment_poses_;
  PCLTrajectory last_fragment_poses_;   // used to check for EM convergence

  // P_ij table (Eq. 25)
  std::vector< std::vector<double> > P_;
  double average_P_;
  double last_average_P_;   // used to check for EM convergence

  RobustPCLReconstruction_GaussianUniform()
  {
    average_P_ = -1.;
    last_average_P_ = -1.;
  }

  ~RobustPCLReconstruction_GaussianUniform() {}

  void LoadOdometryLog(const char* filename) {
    odometry_log_.LoadFromFile(filename);
  }

  void LoadOdometryInfo(const char* filename) {
    odometry_info_.LoadFromFile(filename);
  }

  void LoadLoopLog(const char* filename) {
    loop_log_.LoadFromFile(filename);
  }

  void LoadLoopInfo(const char* filename) {
    loop_info_.LoadFromFile(filename);
  }

  void InitPosesFromFile(const char* filename) {
    fragment_poses_.LoadFromFile(filename);

    // Initialize P_ij table
    P_.resize(NumPoses());
    for (int i = 0; i < NumPoses(); i++) {
      P_[i].resize(NumPoses());
    }
  }

  // void InitPoses() {
  //   fragment_poses_.data_.clear();
  //   fragment_poses_.index_ = 0;
  //   Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
  //   fragment_poses_.data_.push_back( FramedTransformation( 0, 0, 1, pose ) );

  //   for( std::vector< FramedTransformation >::iterator it = odometry_log_.data_.begin();
  //     it != odometry_log_.data_.end(); it ++)
  //   {
  //     pose = pose * it->transformation_;
  //     fragment_poses_.data_.push_back( FramedTransformation( it->id2_, it->id2_, it->id2_ + 1, pose ) );
  //   }
  //   std::cout << "total data size: " << odometry_log_.data_.size() << std::endl;

  //   P_.resize(NumPoses());
  //   for (int i = 0; i < NumPoses(); i++) {
  //     P_[i].resize(NumPoses());
  //   }
  // }

  void SavePoses(const char* filename) {
    fragment_poses_.SaveToFile(filename);
  }

  void SaveLinks(const char* filename) {
    FILE * f = fopen( filename, "w" );

    for ( int i = 0; i < NumOdometryConstraints(); i++ ) {
      Sophus::SE3d trans_se3 = odometry_log_.data_[ i ].transformation_se3_;
      Eigen::Matrix4d trans = trans_se3.matrix();
      fprintf( f, "%d\t%d\t%d\n", odometry_log_.data_[ i ].id1_, odometry_log_.data_[ i ].id2_, odometry_log_.data_[ i ].frame_ );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(0,0), trans(0,1), trans(0,2), trans(0,3) );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(1,0), trans(1,1), trans(1,2), trans(1,3) );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(2,0), trans(2,1), trans(2,2), trans(2,3) );
      fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(3,0), trans(3,1), trans(3,2), trans(3,3) );
    }

    for (int i = 0; i < NumLoopClosureConstraints(); i++ ) {
      Sophus::SE3d trans_se3 = loop_log_.data_[ i ].transformation_se3_;
      int id1 = loop_log_.data_[ i ].id1_;
      int id2 = loop_log_.data_[ i ].id2_;
      assert(id1 < id2);
      if (id1 != id2 - 1 && P_[id1][id2] > THESHOLD_TO_BE_AN_INLIER_LOOP) {
        Eigen::Matrix4d trans = trans_se3.matrix();
        fprintf( f, "%d\t%d\t%d\n", loop_log_.data_[ i ].id1_, loop_log_.data_[ i ].id2_, loop_log_.data_[ i ].frame_ );
        fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(0,0), trans(0,1), trans(0,2), trans(0,3) );
        fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(1,0), trans(1,1), trans(1,2), trans(1,3) );
        fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(2,0), trans(2,1), trans(2,2), trans(2,3) );
        fprintf( f, "%.8f %.8f %.8f %.8f\n", trans(3,0), trans(3,1), trans(3,2), trans(3,3) );
      }
    }
    fclose( f );
  }

  void Expectation() {
    std::cout << std::endl << "This is the beginning of EM iteration " << fragment_poses_.index_ + 1 << std::endl
              << ">>>> Expectation Step <<<<" << std::endl;

    double sum_P = 0;   // used to compute average_P_ later
    std::vector<int> P_ij_counter(20);
    for (int ii = 0; ii < 20; ii ++)
      P_ij_counter[ii] = 0;

    std::cout << std::setw(5) << "id1"
              << std::setw(5) << "id2"
              << std::setw(10) << "#matches"
              << std::setw(15) << "xiLxi"
              << std::setw(15) << "B_ij"
              << std::setw(15) << "B_ij^2"
              << std::setw(15) << "expterm"
              << std::setw(5) << " --> "
              << std::setw(15) << "P_ij"
              << std::endl;

    // Compute P_ij table
    for (int i = 0; i < NumLoopClosureConstraints(); i ++) {

      FramedTransformation trans = loop_log_.data_[i];
      FramedInformation info = loop_info_.data_[i];

      assert(trans.id1_ == info.id1_ && trans.id2_ == info.id2_ && trans.id1_ < trans.id2_);
      int id1 = info.id1_;
      int id2 = info.id2_;

      // Compute the matrix form of xi
      Sophus::SE3d M_xi = trans.transformation_se3_
                          * fragment_poses_.data_[id2].transformation_se3_.inverse()
                          * fragment_poses_.data_[id1].transformation_se3_;

      // Extract the vector form of xi
      Vector6d xi;
      xi << M_xi.data()[4], M_xi.data()[5], M_xi.data()[6], M_xi.data()[0], M_xi.data()[1], M_xi.data()[2];

      // Compute B_ij (Eq. 23)
      // This approximiation technique is adopted from Choi et al.
      double xiLxi = xi.transpose() * info.information_ * xi;
      double B_ij = xiLxi / info.information_(0,0);

      // Compute P_ij (Eq. 25)
      // The exponential-of-log trick is used to avoid the numerical issues when B_ij is too large.
      double expterm = exp(2 * log(B_ij) - log (THETA));
      P_[id1][id2] = 1./ (1. + expterm);

      // Accumulate sum_P
      sum_P += P_[id1][id2];

      // Visualize intermediate steps
      std::cout << std::setw(5) << id1
                << std::setw(5) << id2
                << std::setw(10) << info.information_(0,0)
                << std::setw(15) << xiLxi
                << std::setw(15) << B_ij
                << std::setw(15) << B_ij * B_ij
                << std::setw(15) << expterm
                << std::setw(5) << " --> "
                << std::setw(15) << P_[id1][id2]
                << std::endl;

      // Use P_ij_counter to visualize the distribution of P_ij
      for (int ii = 0; ii < 20; ii ++)
        if (P_[id1][id2] > 0.05 * ii && P_[id1][id2] <= 0.05 * (ii+1))
          P_ij_counter[ii] ++;

    }

    // Save
    last_average_P_ = average_P_;
    last_fragment_poses_.index_ = fragment_poses_.index_;
    last_fragment_poses_.data_.clear();
    for (int i = 0; i < NumPoses(); i++)
      last_fragment_poses_.data_.push_back( FramedTransformation( fragment_poses_.data_[i].id1_,
                                                                  fragment_poses_.data_[i].id2_,
                                                                  fragment_poses_.data_[i].frame_,
                                                                  fragment_poses_.data_[i].transformation_se3_));
    // Update
    average_P_ = sum_P / NumLoopClosureConstraints();
    fragment_poses_.index_++;

    // Visualize the distribution of P_ij
    std::cout << "P_ij distributed in 20 bins :" << std::endl;
    for (int ii = 0; ii < 20 ; ii++)
      std::cout << "between"
                << std::setw(5) << 0.05 * ii
                << std::setw(5) << "and"
                << std::setw(5) << 0.05 * (ii+1)
                << std::setw(15) << P_ij_counter[ii]
                << std::endl;
  }

  void Maximization() {
    std::cout << std::endl << ">>>> Maximization Step <<<<" << std::endl;

    // Build the problem.
    ceres::Problem problem;

    // Specify local update rule for the parameters
    for (std::vector< FramedTransformation >::iterator it = fragment_poses_.data_.begin();
         it != fragment_poses_.data_.end(); it++ ) {
      problem.AddParameterBlock(it->transformation_se3_.data(), Sophus::SE3d::num_parameters,
                                new Sophus::test::LocalParameterizationSE3);
    }

    // Create and add cost functions. Derivatives will be evaluated via
    // automatic differentiation
    for (int i = 0; i < NumOdometryConstraints(); i++) {

      FramedTransformation trans = odometry_log_.data_[i];
      FramedInformation info = odometry_info_.data_[i];

      assert(trans.id1_ == info.id1_ && trans.id2_ == info.id2_ && trans.id1_ < trans.id2_);
      int id1 = info.id1_;
      int id2 = info.id2_;

      if (info.information_(0,0) <= 1)
        continue;

      ceres::CostFunction* cost_odometry =
          new ceres::AutoDiffCostFunction<GaussianFunctor, 1,
                                          Sophus::SE3d::num_parameters,
                                          Sophus::SE3d::num_parameters>(
              new GaussianFunctor(trans.transformation_se3_, info.information_, id1, id2));
      problem.AddResidualBlock(cost_odometry, NULL,
                               fragment_poses_.data_[id1].transformation_se3_.data(),
                               fragment_poses_.data_[id2].transformation_se3_.data());
    }

    for (int i = 0; i < NumLoopClosureConstraints(); i++) {

      FramedTransformation trans = loop_log_.data_[i];
      FramedInformation info = loop_info_.data_[i];

      assert(trans.id1_ == info.id1_ && trans.id2_ == info.id2_ && trans.id1_ < trans.id2_);
      int id1 = info.id1_;
      int id2 = info.id2_;

      ceres::CostFunction* cost_loop =
          new ceres::AutoDiffCostFunction<GaussianFunctor, 1,
                                          Sophus::SE3d::num_parameters,
                                          Sophus::SE3d::num_parameters>(
              new GaussianFunctor(trans.transformation_se3_, info.information_, id1, id2));
      problem.AddResidualBlock(cost_loop, new ceres::ScaledLoss(NULL, P_[id1][id2], ceres::TAKE_OWNERSHIP),
                               fragment_poses_.data_[id1].transformation_se3_.data(),
                               fragment_poses_.data_[id2].transformation_se3_.data());
    }

    // Set solver options
    ceres::Solver::Options options;
    // options.max_num_iterations = 1000;
    options.gradient_tolerance = 0.01 * Sophus::Constants<double>::epsilon();
    options.function_tolerance = 0.01 * Sophus::Constants<double>::epsilon();
    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;

    // Solve and report
    ceres::Solver::Summary summary;
    Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << std::endl;

    std::cout << "This is the end of EM iteration : " << fragment_poses_.index_ << std::endl;
  }

  int NumPoses() {
    return fragment_poses_.data_.size();
  }

  int NumOdometryConstraints() {
    return odometry_log_.data_.size();
  }

  int NumLoopClosureConstraints() {
    return loop_log_.data_.size();
  }

  bool IsConverged() {
    if (last_average_P_ < 0) {
      return false;         // not started yet
    }

    std::cout << "Checking for the convergence of average_P_ ...\t";
    if (fabs(average_P_ - last_average_P_) > 0.00001) {
      std::cout << "NOT coverged yet, as average_P_ was updated from " << last_average_P_ << " to " << average_P_ << std::endl;
      return false;
    }
    std::cout << "CONVERGED!" << std::endl;

    std::cout << "Checking for the convergence of poses ...\t";
    for (int i = 0; i < NumPoses(); i++)
    {
      Sophus::SE3d last_pose = last_fragment_poses_.data_[i].transformation_se3_;
      Sophus::SE3d current_pose = fragment_poses_.data_[i].transformation_se3_;

      double const mse = (last_pose.inverse() * current_pose).log().squaredNorm();
      bool const converged = mse < 10. * Sophus::Constants<double>::epsilon();

      if (!converged) {
        std::cout << "NOT converged yet." << std::endl;
        return false;
      }
    }
    std::cout << "CONVERGED!" << std::endl;
    return true;
  }
};

int main(int argc, char** argv) {
  if (argc != 8)
  {
    std::cout << "Please provide the following arguments: InputOdometryLog, InputOdometryInfo, InputLoopLog, InputLoopInfo, InputInitPoses, OutputFinalPoses, OutputKeptLoops" << std::endl;
    return 1;
  }
  RobustPCLReconstruction_GaussianUniform RobustPCLReconstruction_g;

  RobustPCLReconstruction_g.LoadOdometryLog(argv[1]);
  RobustPCLReconstruction_g.LoadOdometryInfo(argv[2]);
  RobustPCLReconstruction_g.LoadLoopLog(argv[3]);
  RobustPCLReconstruction_g.LoadLoopInfo(argv[4]);
  RobustPCLReconstruction_g.InitPosesFromFile(argv[5]);

  bool isConverged = false;
  for (int i = 0 ; i < NUM_ITERATION_EM; i ++) {
    RobustPCLReconstruction_g.Expectation();
    RobustPCLReconstruction_g.Maximization();
    if (RobustPCLReconstruction_g.IsConverged()) {
      std::cout << "Optimization ends since EM is converged" << std::endl << std::endl;
      isConverged = true;
      break;
    }
  }

  if (!isConverged)
    std::cout << "Optimization ends after " << NUM_ITERATION_EM << " EM iterations" << std::endl << std::endl;

  RobustPCLReconstruction_g.SavePoses(argv[6]);
  std::cout << "Final poses are saved to " << argv[6] << std::endl;

  RobustPCLReconstruction_g.SaveLinks(argv[7]);
  std::cout << "Good links are saved to " << argv[7] << std::endl;

  std::cout << "There are " << RobustPCLReconstruction_g.NumPoses() << " poses" << std::endl;
  std::cout << "There are " << RobustPCLReconstruction_g.NumOdometryConstraints() << " odometry constraints" << std::endl;
  std::cout << "There are " << RobustPCLReconstruction_g.NumLoopClosureConstraints() << " loops" << std::endl;

  std::cout << std::endl;

  return 0;
}


#include <libv/core/serialization/all.hpp>
#include <libv/graphic/viewer_context.hpp>
#include <libv/lma/lma.hpp>
#include <Eigen/Core>

using namespace v;
using namespace std;
using namespace Eigen;

typedef Matrix<double, 5, 1> Vector5d;

V_DEFINE_CONFIG
(
  V_DEFINE_PROPERTY(config, string("config.ini"), "Main config file")
  V_DEFINE_PROPERTY(input, 1, "The Input camera model")
  V_DEFINE_PROPERTY(output, 0, "The output camera model")
  V_DEFINE_PROPERTY(step, 10., "Distance between two consecutive generated points")
  V_DEFINE_PROPERTY(height, 480., "Height of the image")
  V_DEFINE_PROPERTY(width, 640., "Width of the image")
  V_DEFINE_PROPERTY(center, Array2d(), "Coordinates of the principal point")
  V_DEFINE_PROPERTY(focal, Array2d(), "Focal length")
  V_DEFINE_PROPERTY(xi, 0., "Distortion parameter for the unified model")
  V_DEFINE_PROPERTY(distortion, Vector5d(), "Distortion polynomial")
  V_DEFINE_PROPERTY(lambda, 1e-3, "")
  V_DEFINE_PROPERTY(iteration_count, 25u, "")
  V_DEFINE_PROPERTY(verbose, false, 0)
)

extern Config config;
extern ViewerContext v1, v2, v3;

struct Callbacks
: enable_verbose_output
{
  template<class S, class Algo>
  void at_begin_bundle_adjustment_iteration(const S& solver, const Algo& algo) const
  {
    enable_verbose_output::at_begin_bundle_adjustment_iteration(solver,algo);
    v3.clear();
  }

  template<class S, class Algo>
  void at_end_bundle_adjustment_iteration(const S& solver, const Algo& algo) const
  {
    enable_verbose_output::at_end_bundle_adjustment_iteration(solver,algo);
    v3.update().title("Press enter to continue");
    getchar();
  }
};

struct Error
{
  Array2d p2;
  Vector3d p3;
};

struct Module
{
  virtual void init(Error &) = 0;
  virtual void add(Error &) = 0;
  virtual void run() = 0;
  virtual void run_verbose() = 0;
};

Module *init_unified();
Module *init_pinhole_indirect_radial();


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[register_box_templated
//` Show the use of the macro BOOST_GEOMETRY_REGISTER_BOX_TEMPLATED

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/register/box.hpp>

template <typename P>
struct my_box
{
    P ll, ur;
};

// Register the box type
BOOST_GEOMETRY_REGISTER_BOX_TEMPLATED(my_box, ll, ur)

int main()
{
    typedef my_box<boost::geometry::model::d2::point_xy<double> > box;
    box b = boost::geometry::make<box>(0, 0, 2, 2);
    std::cout << "Area: "  << boost::geometry::area(b) << std::endl;
    return 0;
}

//]

//[register_box_templated_output
/*`
Output:
[pre
Area: 4
]
*/
//]


#ifndef Field_hpp
#define Field_hpp

/** @file src/libField/Field.hpp
 */

#include <ostream>
#include <type_traits>
#include <typeinfo>
#include <vector>
#include <array>

#include <boost/multi_array.hpp>

#include "CoordinateSystem.hpp"
#include "Utils.hpp"

/** @class Field
 * @brief A class for storing data in a field.
 * @author C.D. Clark III
 * @date 06/13/17
 *
 * A field is a quantity defined at every point in space. For example, the temperature
 * distribution throughout a solid, the pressure at every point in a room, or the density of a fluid.
 * A field may be 1-, 2-, or 3-dimensional, and is parameterized by a coordinate system.
 *
 * This class associates a coordinate system with a multi-dimensional array. Field elements are
 * allocated in a single, multi-dimensional array. And a CoordinateSystem is allocated for the coordinates.
 */
template<typename T, std::size_t N>
using arrayND = boost::multi_array<T, N, std::allocator<T>>;
template<typename T, std::size_t N>
using viewND = boost::detail::multi_array::multi_array_view<T, N>;

template<typename QUANT, size_t NUMDIMS, typename COORD = QUANT,
         template<typename, size_t> class ARRAYND = arrayND,
         template<typename> class ARRAY1D         = array1D>
class Field
{
 public:
  typedef ARRAYND<QUANT, NUMDIMS>                   array_type;
  typedef typename array_type::index                index_type;
  typedef CoordinateSystem<COORD, NUMDIMS, ARRAY1D> cs_type;

 protected:
  std::shared_ptr<array_type> d;
  std::shared_ptr<cs_type>    cs;

 protected:
  /**
   * @internal
   * Utility function for converting 1d index to an Nd
   * array of indices.
   */
  auto _1d2nd(size_t i) const
  {
    auto shape = d->shape();

    std::array<size_t, NUMDIMS> ind;
    int                         NN = shape[0];
    for (size_t j = 1; j < NUMDIMS; ++j) NN *= shape[j];
    for (size_t j = 0; j < NUMDIMS; ++j) {
      NN /= shape[j];
      ind[j] = i / NN;
      i -= ind[j] * NN;
    }
    return ind;
  }

 public:
#if SERIALIZATION_ENABLED
  template<class Archive>
  void serialize(Archive& ar, const unsigned int version)
  {
    ar& d;
    ar& cs;
  }
#endif

  /**
   * @brief Create an empty field with no elements allocated.
   *
   * Memory can be allocated later with Field::reset() method. The default constructor is provided to
   * support storing fields in some containers that require a default constructor.
   */
  Field()        = default;
  Field(Field&&) = default;
  ~Field() = default;

  Field(const Field& f) { reset(*f.cs, *f.d); }

  /**
   * @brief Create a new field and allocate memory for grid defined by dims.
   *
   * @param dims a list of sizes for each dimension.
   *
   * This constructor allows the caller to pass dimension sizes directly to the constructor.
   *
   * @code
   * libField<double,2> f(10,20);
   * @endcode
   *
   * This will create a field with 200 elements, 10 along the first dimension, 20 along the second.
   *
   */
  template<typename... Dims>
  Field(Dims... dims)
  {
    reset(dims...);
  }

  /**
   * @brief Create a new field and allocate memory for the grid defined sizes.
   *
   * @param sizes an array of integers specifying the size of the field along each dimension.
   */
  template<typename I>
  Field(std::array<I, NUMDIMS> sizes)
  {
    reset(sizes);
  }
  Field(std::shared_ptr<cs_type> cs_) { reset(cs_); }

  Field(cs_type& cs_, array_type& d_) { reset(cs_, d_); };

  /**
   * @brief Reallocate a field with new dimensions.
   *
   * @param dims The size of the new field along each dimension.
   */
  template<typename... Dims>
  void reset(Dims... dims)
  {
    cs = std::make_shared<cs_type>(dims...);
    std::array<size_t, NUMDIMS> sizes({static_cast<size_t>(dims)...});
    d = std::make_shared<array_type>(sizes);
  }

  /**
   * @brief Reallocate a field with new dimensions.
   *
   * @param sizes An array of the new field sizes along each dimension.
   */
  template<typename I>
  void reset(std::array<I, NUMDIMS> sizes)
  {
    cs = std::make_shared<cs_type>(sizes);
    d = std::make_shared<array_type>(sizes);
  }

  /**
   * @brief Reallocate a field from an existing coordinate system.
   *
   * @param cs_ a shared pointer to an existing coordinate system.
   *
   * New memory will be allocated for the field elements, but not for
   * the coordinate system.
   */
  void reset(std::shared_ptr<cs_type> cs_)
  {
    cs = cs_;

    std::vector<size_t> sizes(NUMDIMS);
    for (size_t i = 0; i < NUMDIMS; ++i) sizes[i] = cs->size(i);

    d = std::make_shared<array_type>(sizes);
  }

  /**
   * @brief Reallocate a field from an existing coordinate system and field elements.
   *
   * @param cs_ a shared pointer to an existing coordinate system.
   * @param d_ a shared pointer to an existing array of field elements.
   *
   * No new memory is allocated. References to existing field elements
   * and coordinate system will be used.
   */
  void reset(cs_type& cs_, array_type& d_)
  {
    d = std::make_shared<array_type>(d_);
    cs = std::make_shared<cs_type>(cs_.getAxes());
  };

  // ELEMENT ACCESS

  /**
   * @brief Return const reference to an element of field with given index.
   * @param i An array-like container of indices.
   *
   * Example:
   *
   * The elements of a 2-dimensional array can be accessed using a 2-element vector.
   * @code
   * Field<double,2> f(10,20);
   * ...
   * std::vector<int> ind;
   *
   * ind[0] = 2;
   * ind[1] = 4;
   *
   * double val = f(ind); // get the (2,4) element of the field.
   * @endcode
   *
   * Any container that provides a subscript operator (operator[](int)) can be used.
   */
  template<typename I,
           typename std::enable_if<IsIndexCont<I>::value, int>::type = 0>
  const auto& operator()(I i) const
  {
    return (*d)(i);
  }

  /**
   * @brief Return a reference to an element of field with given index.
   * @param i An array-like container of indices.
   *
   * Example:
   *
   * The elements of a 2-dimensional array can be accessed using a 2-element vector.
   * @code
   * Field<double,2> f(10,20);
   * ...
   * std::vector<int> ind;
   *
   * ind[0] = 2;
   * ind[1] = 4;
   *
   * f(ind) = 10; // set the (2,4) element of the field.
   * @endcode
   *
   * Any container that provides a subscript operator (operator[](int)) can be used.
   */
  template<typename I,
           typename std::enable_if<IsIndexCont<I>::value, int>::type = 0>
  auto& operator()(I i)
  {
    return (*d)(i);
  }

  /**
   * @brief Return a const reference to an element of field with given index.
   * @param i the index of the element along the first dimension.
   * @param args the indexes of the element along the remaining dimensions.
   *
   * This function is a variadic template that allow a natural access to the field elements using the operator()(), rather
   * than having to use an index container..
   *
   * Example:
   *
   * @code
   * Field<double,2> f(10,20);
   * ...
   *
   * double val = f(2,4); // get the (2,4) element of the field.
   * @endcode
   */
  template<typename I, typename... Args,
           typename std::enable_if<std::is_integral<I>::value, int>::type = 0>
  const auto& operator()(I i, Args... args) const
  {
    return (*d)(std::array<I, NUMDIMS>({i, args...}));
  }

  /**
   * @brief Return a reference to an element of field with given index.
   * @param i the index of the element along the first dimension.
   * @param args the indexes of the element along the remaining dimensions.
   *
   *
   * This function is a variadic template that allow a natural access to the field elements using the operator()(), rather
   * than having to use an index container..
   *
   * Example:
   *
   * @code
   * Field<double,2> f(10,20);
   * ...
   *
   * f(2,4) = 10; // set the (2,4) element of the field.
   * @endcode
   */
  template<typename I, typename... Args,
           typename std::enable_if<std::is_integral<I>::value, int>::type = 0>
  auto& operator()(I i, Args... args)
  {
    return (*d)(std::array<I, NUMDIMS>({i, args...}));
  }

  template<typename I>
  auto operator[](I i) const
  {
    return (*d)[i];
  }

  /**
   * @brief Return a shared pointer to the coordinate system used by the field.
   */
  auto        getCoordinateSystemPtr() { return cs; };

  /**
   * @brief Return a reference to the coordinate system used by the field.
   */
  auto&       getCoordinateSystem() { return *cs; };

  /**
   * @brief Return a reference to the i'th axis in the coordinate system used by the field.
   * @param i The index (zero-offset) of the axis to return.
   */
  auto&       getAxis(size_t i) { return cs->getAxis(i); }

  /**
   * @brief Return a const reference to the coordinate system used by the field.
   */
  const auto& getCoordinateSystem() const { return *cs; };
  /**
   * @brief Return a const reference to the i'th axis in the coordinate system used by the field.
   * @param i The index (zero-offset) of the axis to return.
   */
  const auto& getAxis(size_t i) const { return cs->getAxis(i); }

  /**
   * @brief Set the coordinates of the coordinate system.
   * @param args Coordinates specification. The arguments are passed directly to CoordinateSystem::set() of the coordinate system used by the field.
   *
   * Coordinate system coordinate are set using a range discretizer. See range_discretizers::UnitformImp<T> for example.
   *
   * Example:
   *
   * To configure a field over the range [-1,1] along the x direction and [0:4] along the y direction,
   * @code
   * Field<double,2> f(10,20);
   * f.setCoordinateSystem( Uniform(-1,1), Uniform(0,4) );
   * @endcode
   */
  template<typename... Args>
  auto setCoordinateSystem(Args... args)
  {
    cs->set(args...);
  }

  /**
   * @brief Return the coordinate element specified by args.
   * @param args The index of the coordinate to retrief. The arguments are passed directly to CoordinateSystem::getCoord() of the coordinate system used by the field.
   *
   * Example:
   *
   * @code
   * Field<double,2> f(10,20);
   * ...
   *
   * auto coord = f.getCoord(2,4); // get the coordinate for the field element at (2,4)
   * double x = coord[0]; // get the x coordinate;
   * double y = coord[1]; // get the x coordinate;
   * @endcode
   */
  template<typename... Args>
  auto getCoord(Args... args) const
  {
    return cs->getCoord(args...);
  }

  /**
   * @brief Returns index of stored coordinate that lower bounds the given coordinate.
   *
   * This function forwards the arguments to Coordinate::lower_bound() of the coordinate system used by the field.
   * Along with upper_bound, it is useful for finding a range that bounds the given coordinate.
   */
  template<typename... Args>
  auto lower_bound(Args... args) const
  {
    return cs->lower_bound(args...);
  }

  /**
   * @brief Returns index of stored coordinate that upper bounds the given coordinate.
   *
   * This function forwards the arguments to Coordinate::upper_bound() of the coordinate system used by the field.
   * Along with lower_bound, it is useful for finding a range that bounds the given coordinate.
   */
  template<typename... Args>
  auto upper_bound(Args... args) const
  {
    return cs->upper_bound(args...);
  }

  /**
   * @brief returns index of stored coordinate that is closest to the given coordinate.
   */
  template<typename... Args>
  auto nearest(Args... args) const
  {
    return cs->nearest(args...);
  }

  // data access
  auto        getDataPtr() { return d; };
  const auto& getData() const { return *d; };
  auto&       getData() { return *d; };

  const auto data() const { return d->data(); }
  auto       data() { return d->data(); }

  template<int NDims>
  const auto slice(
      const boost::detail::multi_array::index_gen<NUMDIMS, NDims>& ind) const
  {
    // get sliced data
    auto d_ = d->operator[](ind);
    // get sliced coordinate system
    auto cs_ = cs->slice(ind);

    return Field<QUANT, NDims, COORD, viewND, view1D>(cs_, d_);
  }

  template<int NDims>
  auto slice(const boost::detail::multi_array::index_gen<NUMDIMS, NDims>& ind)
  {
    // get sliced data
    auto d_ = d->operator[](ind);
    // get sliced coordinate system
    auto cs_ = cs->slice(ind);

    return Field<QUANT, NDims, COORD, viewND, view1D>(cs_, d_);
  }

  auto size() const { return d->num_elements(); }
  auto size(int i) const { return d->shape()[i]; }

  /**
   * @brief Set each element of a field using a callable that takes an array-like container of *coordinates* as an argument and returns the element's value.
   *
   * This function evaluates the callable f for each coordinate and sets the field value to value
   * returned by callable. Field may be evaluated in PARRALLEL. Callable should
   * NOT depend on the order of being called.
   *
   * @param f a callable object (function, funtor, lambda, std::function, etc.)
   * that accepts one argument and returns a value.
   *
   * Argument passed to callable f will be an array of coordinates.
   *
   */
  template<typename F>
  auto set_f(F f) -> decltype((*d)(0) = f(cs->getCoord(_1d2nd(0))), void())
  {
    auto N = d->num_elements();
#pragma omp parallel for
    for (size_t i = 0; i < N; ++i) {
      auto ind             = this->_1d2nd(i);
      d->  operator()(ind) = f(cs->getCoord(ind));
    }
  }

  /**
   * @brief Set each element of a field using a callable that takes an array-like container of *coordinates* as an argument and returns the element's value.
   *
   * This function evaluates the callable f that returns an optional type (boost::optional or
   * std::optional) for each coordinate. If the optional is set, then the value of
   * of the field element is set. Otherwise, the field is left untouched. Field may be
   * evaluated in PARRALLEL. Callable should NOT depend on the order of being
   * called.
   *
   * @param f a callable object (function, funtor, lambda, std::function, etc.)
   * that accepts one argument and returns an optional value.
   *
   * Argument passed to callable f will be an array of coordinates.
   *
   */
  template<typename F>
  auto set_f(F f)
      -> decltype((bool)f(cs->getCoord(_1d2nd(0))),
                  (*d)(0) = f(cs->getCoord(_1d2nd(0))).value(), void())
  {
    auto N = d->num_elements();
#pragma omp parallel for
    for (size_t i = 0; i < N; ++i) {
      auto ind = this->_1d2nd(i);
      auto val = f(cs->getCoord(ind));
      if (val) d->operator()(ind) = val.value();
    }
  }

  /**
   * @brief Set each element of a field using a callable that takes an array-like container of *indices* and a pointer to a coordinate system as arguments and returns the element's value.
   *
   * Evaluates callable f for each indices and sets the field value to value
   * returned by callable. Callable is passed a container of indices and a
   * pointer to the coordinate system. Field may be evaluated in PARRALLEL.
   * Callable should NOT depend on the order of being called.
   *
   * @param f a callable object (function, funtor, lambda, std::function, etc.)
   * that accepts two arguments and returns a value.
   *
   * Arguments passed to callable f will be an array of indices, and a pointer
   * to the coordinate system.
   *
   */
  template<typename F>
  auto set_f(F f) -> decltype((*d)(0) = f(_1d2nd(0), cs), void())
  {
    auto N = d->num_elements();
#pragma omp parallel for
    for (size_t i = 0; i < N; ++i) {
      auto ind             = this->_1d2nd(i);
      d->  operator()(ind) = f(ind, cs);
    }
  }

  /**
   * @brief Set each element of a field using a callable that takes an array-like container of *indices* and a pointer to a coordinate system as arguments and returns the element's value.
   *
   * Evaluates callable f for each coordinate index. If the optional returned by
   * f is set, the field for the coordinate is set. Otherwise, the field is left
   * untouched. Callable is passed a container of indices and a pointer to the
   * coordinate system. Field may be evaluated in PARRALLEL. Callable should NOT
   * depend on the order of being called.
   *
   * @param f a callable object (function, funtor, lambda, std::function, etc.)
   * that accepts two arguments and returns a value.
   *
   * Arguments passed to callable f will be an array of indices, and a pointer
   * to the coordinate system.
   *
   */
  template<typename F>
  auto set_f(F f) -> decltype((bool)f(_1d2nd(0), cs),
                              (*d)(0) = f(_1d2nd(0), cs).value(), void())
  {
    auto N = d->num_elements();
#pragma omp parallel for
    for (size_t i = 0; i < N; ++i) {
      auto ind = this->_1d2nd(i);
      auto val = f(ind, cs);
      if (val) d->operator()(ind) = val.value();
    }
  }

  // NOTE: we wanted to combined set and set_f into a single function, but this
  // isn't possible in general. We cannot assume that the set_f version should
  // be called if a function is passed in, because the user may actually want to
  // store the functions in the field.

  /**
   * @brief Set all elements of a field to the value specified.
   * @param q the value to set each element to.
   *
   * The method will set element values in parallel using OpenMP.
   *
   */
  template<typename Q>
  auto set(Q q)
  {
    auto N = d->num_elements();
#pragma omp parallel for
    for (size_t i = 0; i < N; ++i) {
      auto ind             = this->_1d2nd(i);
      d->  operator()(ind) = std::move(q);
    }
  }

  // operator overloads

  friend std::ostream& operator<<(std::ostream& output, const Field& F)
  {
    auto N        = F.d->num_elements();
    auto last_ind = F._1d2nd(0);
    for (size_t i = 0; i < N; ++i) {
      auto ind = F._1d2nd(i);
      // we want to print out blank lines whenever an index gets reset
      for (size_t j = 0; j < NUMDIMS; ++j)
        if (ind[j] < last_ind[j]) output << "\n";

      for (size_t j = 0; j < NUMDIMS; ++j)
        output << F.cs->getAxis(j)[ind[j]] << " ";
      output << F.d->operator()(ind) << "\n";

      last_ind = ind;
    }
    return output;
  }

  /**
   * @brief Set the elements of a field to the value given.
   *
   * Field elements are set in parallel using OpenMP.
   */
  template<typename Q>
  Field& operator=(const Q& q)
  {
    auto N = d->num_elements();
#pragma omp parallel for
    for (size_t i = 0; i < N; ++i) {
      auto ind             = this->_1d2nd(i);
      d->  operator()(ind) = q;
    }
    return *this;
  }

  /**
   * @brief Add a constant value to each element in the field.
   * @param The value to add to each element.
   *
   * Field elements are updated in parallel using OpenMP.
   */
  template<typename Q>
  Field& operator+=(const Q& q)
  {
    auto N = d->num_elements();
#pragma omp parallel for
    for (size_t i = 0; i < N; ++i) {
      auto ind = this->_1d2nd(i);
      d->  operator()(ind) += q;
    }
    return *this;
  }

  /**
   * @brief Subtract a constant value to each element in the field.
   * @param The value to subtract from each element.
   *
   * Field elements are updated in parallel using OpenMP.
   */
  template<typename Q>
  Field& operator-=(const Q& q)
  {
    auto N = d->num_elements();
#pragma omp parallel for
    for (size_t i = 0; i < N; ++i) {
      auto ind = this->_1d2nd(i);
      d->  operator()(ind) -= q;
    }
    return *this;
  }

  /**
   * @brief Multiply each element in a field by a constant value.
   * @param The value to multiply each element by.
   *
   * Field elements are updated in parallel using OpenMP.
   */
  template<typename Q>
  Field& operator*=(const Q& q)
  {
    auto N = d->num_elements();
#pragma omp parallel for
    for (size_t i = 0; i < N; ++i) {
      auto ind = this->_1d2nd(i);
      d->  operator()(ind) *= q;
    }
    return *this;
  }

  /**
   * @brief Divide each element in a field by a constant value.
   * @param The value to divide each element by.
   *
   * Field elements are updated in parallel using OpenMP.
   */
  template<typename Q>
  Field& operator/=(const Q& q)
  {
    auto N = d->num_elements();
#pragma omp parallel for
    for (size_t i = 0; i < N; ++i) {
      auto ind = this->_1d2nd(i);
      d->  operator()(ind) /= q;
    }
    return *this;
  }

  Field& operator=(Field f)
  {
    d.swap(f.d);
    cs.swap(f.cs);
    return *this;
  }

  /**
   * @brief Add the element of a second field to each element of the field.
   * @param The field containing elements to be added to this field.
   *
   * Field elements are updated in parallel using OpenMP.
   *
   * Fields must be of the size.
   */
  Field& operator+=(const Field& f)
  {
    BOOST_ASSERT(f.size() == this->size());
    auto N = d->num_elements();
#pragma omp parallel for
    for (size_t i = 0; i < N; ++i) {
      auto ind = this->_1d2nd(i);
      d->  operator()(ind) += f(ind);
    }
    return *this;
  }

  /**
   * @brief Subtract the element of a second field from each element of the field.
   * @param The field containing elements to be subtracted from this field.
   *
   * Field elements are updated in parallel using OpenMP.
   *
   * Fields must be of the size.
   */
  Field& operator-=(const Field& f)
  {
    BOOST_ASSERT(f.size() == this->size());
    auto N = d->num_elements();
#pragma omp parallel for
    for (size_t i = 0; i < N; ++i) {
      auto ind = this->_1d2nd(i);
      d->  operator()(ind) -= f(ind);
    }
    return *this;
  }

  /**
   * @brief Multiply each element in the field by the corresponding element in a second field.
   * @param The field containing elements to be multiply by.
   *
   * Field elements are updated in parallel using OpenMP.
   *
   * Fields must be of the size.
   */
  Field& operator*=(const Field& f)
  {
    BOOST_ASSERT(f.size() == this->size());
    auto N = d->num_elements();
#pragma omp parallel for
    for (size_t i = 0; i < N; ++i) {
      auto ind = this->_1d2nd(i);
      d->  operator()(ind) *= f(ind);
    }
    return *this;
  }

  /**
   * @brief Divide each element in the field by the corresponding element in a second field.
   * @param The field containing elements to divide by.
   *
   * Field elements are updated in parallel using OpenMP.
   *
   * Fields must be of the size.
   */
  Field& operator/=(const Field& f)
  {
    BOOST_ASSERT(f.size() == this->size());
    auto N = d->num_elements();
#pragma omp parallel for
    for (size_t i = 0; i < N; ++i) {
      auto ind = this->_1d2nd(i);
      d->  operator()(ind) /= f(ind);
    }
    return *this;
  }
};

#endif