#include "slice.h"

#include <cstdio>
#include <iostream>
#include <map>
#include <stdint.h>

#include <Eigen/Sparse>

using namespace Eigen;

using std::cout;
using std::endl;
using std::map;
using std::ostream;
using std::pair;
using std::vector;

void debug_print_sparse(const SparseMatrix<uint8_t> &mat) {
    for (int i = 0; i < mat.cols(); i++) {
        cout << "\t" << i;
    }
    cout << endl;
    for (int i = 0; i < mat.rows(); i++) {
        cout << i << ":\t";
        for (int j = 0; j < mat.cols(); j++) {
            cout << (int) mat.coeff(i, j) << "\t";
        }
        cout << endl;
    }
}

#define OUT_OF_PLANE (0)
#define POINT_IN_PLANE (1)
#define FACE_IN_PLANE (2)

// returns FACE_IN_PLANE if the face is fully in-plane (all verteces lie in the
// plane). returns POINT_IN_PLANE if the plane and face intersect at a single
// point. returns OUT_OF_PLANE if neither property holds.
//
// this function should only be called on faces that either intersect the plane
// or lie in it.
int inplane_status(const float z, const face* f) {
    edge *e0 = f->e;
    // check for in-plane conditions
    int verts_inplane = 0;
    int verts_above = 0;
    int verts_below = 0;

    do {
        vertex *v = e0->vert;
        if (v->loc[2] == z) {
            verts_inplane++;
        } else if (v->loc[2] > z) {
            verts_above++;
        } else {
            verts_below++;
        }
        e0 = e0->next;
    } while (e0 != f->e);

    // check for whole-tri-in-plane
    if (verts_above == 0 && verts_below == 0) {
        return FACE_IN_PLANE;

    }
    // check for single-point-in-plane
    if (verts_inplane == 1 && (verts_above == 0 || verts_below == 0)) {
        return POINT_IN_PLANE;
    }
    return OUT_OF_PLANE;
}

// returns the line segment across the face representing the line of
// intersection between the xy-plane at height z and the provided face.
// returns a zero-length line segment if face does not intersect plane
lineseg isect_tri_xy_plane(const float z, const face* f) {
    lineseg l;

    if (f->sides() != 3) {
        // TODO: triangularize!
        cout << "got a nontriangular mesh and I'm a stupid slicer." << endl;
        return l;
    }

    int points_found = 0;
    edge *e0 = f->e;
    do {
        // do this at the start so we can use continue within the loop
        e0 = e0->next;

        vertex *v = e0->next->vert,
               *v0 = e0->vert;
        float v_z = v->loc[2],
              v0_z = v0->loc[2];

        if (v_z == v0_z) {
            // check for edge-in-plane condition. since we know the whole
            // tri /isn't/ in-plane from the check above, we know that this
            // edge must be our levelset poly line segment
            if (v_z == z) {
                l.p1 = v0->loc;
                l.p2 = v->loc;
                points_found = 2;
                break;
            }
            continue;
        }

        float n = (v_z - z) / (v_z - v0_z);
        if (0 <= n && n <= 1) {
            Vector3f p = v->loc - n * (v->loc - v0->loc);
            if (points_found == 0) {
                l.p1 = p;
                points_found++;
            } else if (points_found == 1 && p != l.p1) {
                l.p2 = p;
                points_found++;
            }
        }
    } while (e0 != f->e);
    return l;
}

// assign faces to layer buckets. this modifies the levelset vector in-place.
void bucket_faces(
        const mesh &m, const bounds &b,
        const float layer_height, vector<levelset> &layers) {
    for (auto iter = m.faces.begin(); iter != m.faces.end(); iter++) {
        face* f = *iter;

        // find face bounds
        float z_min = INFINITY, z_max = -INFINITY;
        edge *e = f->e;
        vertex *v;
        do {
            v = e->vert;
            if (v->loc[2] > z_max) {
                z_max = v->loc[2];
            }
            if (v->loc[2] < z_min) {
                z_min = v->loc[2];
            }
            e = e->next;
        } while (e != f->e);

        // add face to appropriate buckets
        for (auto ls = layers.begin(); ls != layers.end(); ls++) {
            if (z_min <= ls->z && ls->z <= z_max) {
                ls->faces.push_back(f);
            } else if (ls->z > z_max) {
                // we're out of our good range, break
                break;
            }
        }
    }
}

// generates a list of line segments based on the intersection of the bucketed
// faces and the xy-plane at height ls.z
void find_line_segments(levelset &ls) {
    for (auto iter = ls.faces.begin(); iter != ls.faces.end(); iter++) {
        face* f = *iter;
        int inplane = inplane_status(ls.z, f);

        if (inplane == FACE_IN_PLANE) {
            ls.inplane.push_back(f);
            continue;
        } else if (inplane == POINT_IN_PLANE) {
            continue;
        }

        lineseg line = isect_tri_xy_plane(ls.z, f);
        if ((line.p1 - line.p2).norm() == 0) {
            cout << "warning: face doesn't intersect z = " << ls.z << endl;
            edge *e = f->e;
            do {
                vertex *v = e->vert;
                printf("  v[%lx]\t%f\t%f\t%f\n", (unsigned long) v,
                        v->loc[0], v->loc[1], v->loc[2]);
                e = e->next;
            } while (e != f->e);
        } else {
            ls.lines.push_back(line);
        }
    }
}

// creates a vertex in the vert_ids map and in the verts list. returns the ID to
// use for the next vertex (either next_id or next_id + 1)
int create_vertex(
        map<Vector3f, uint32_t, vector_comparitor> &vert_ids,
        Vector3f &loc, vector<Vector3f> &verts, int next_id) {
    if (vert_ids.find(loc) != vert_ids.end()) {
        return next_id;
    }
    vert_ids.insert(pair<Vector3f, uint32_t>(loc, next_id));
    verts.push_back(loc);
    return next_id + 1;
}

// converts an unsorted list of line segments to a list of ordered lists of
// verteces representing paths around the levelset.
void linesegs_to_vert_list(levelset &ls) {
    map<Vector3f, uint32_t, vector_comparitor> vert_ids;
    int next_id = 0;

    // build vert list
    for (auto iter = ls.lines.begin(); iter != ls.lines.end(); iter++) {
        next_id = create_vertex(vert_ids, iter->p1, ls.verteces, next_id);
        next_id = create_vertex(vert_ids, iter->p2, ls.verteces, next_id);
    }
    int n = ls.verteces.size();

    // build adjacency
    // TODO: handle double-edges
    vector<Triplet<uint8_t>> adjacents;
    for (auto iter = ls.lines.begin(); iter != ls.lines.end(); iter++) {
        auto i1 = vert_ids.find(iter->p1);
        if (i1 == vert_ids.end()) {
            cout << "warning: after building vert list, vert " <<
                iter->p1.transpose() << " is missing" << endl;
        }
        auto i2 = vert_ids.find(iter->p2);
        if (i2 == vert_ids.end()) {
            cout << "warning: after building vert list, vert " <<
                iter->p2.transpose() << " is missing" << endl;
        }
        adjacents.push_back(Triplet<uint8_t>(i1->second, i2->second, 1));
        adjacents.push_back(Triplet<uint8_t>(i2->second, i1->second, 1));
    }

    SparseMatrix<uint8_t> adj(n, n);
    adj.setFromTriplets(adjacents.begin(), adjacents.end());

    int vert = -1;
    vector<uint32_t> perimeter;
    vector<vector<uint32_t>*> perimeters;

    while (true) {
        if (vert == -1) {
            for (vert = 0; vert < adj.cols(); vert++) {
                for (int i = 0; i < adj.rows(); i++) {
                    if (adj.coeff(i, vert) > 0) {
                        goto next_vert;
                    }
                }
            }
            goto exit;
        } else {
            int next = -1;
            for (int r = 0; r < adj.rows(); r++) {
                if (adj.coeff(vert, r) > 0) {
                    adj.coeffRef(vert, r)--;
                    adj.coeffRef(r, vert)--;
                    next = r;
                    break;
                }
            }
            vert = next;
        }
next_vert:
        if (vert != -1) {
            perimeter.push_back(vert);
        } else if (perimeter.size() > 0) {
            ls.perimeters.push_back(perimeter);
            perimeter.clear();
        }
    }
exit:
    return;
}

void slice(const tooldef td, const mesh &m, vector<levelset> &levelsets) {
    levelsets.clear();

    bounds b = m.get_bounds();
    int level_count = ceil((b.max_z - b.min_z) / td.z_accuracy);
    for (int i = 0; i < level_count; i++) {
        levelset l;
        l.z = b.min_z + i * td.z_accuracy;
        levelsets.push_back(l);
    }
    levelset l;
    l.z = b.max_z;
    levelsets.push_back(l);

    bucket_faces(m, b, td.z_accuracy, levelsets);

    for (auto iter = levelsets.begin(); iter != levelsets.end(); iter++) {
        find_line_segments(*iter);
        linesegs_to_vert_list(*iter);
    }
}

lineseg::lineseg() {}

lineseg::lineseg(const lineseg &other) : p1(other.p1), p2(other.p2) {}

levelset::levelset() {}

levelset::levelset(const levelset &other) :
        verteces(other.verteces), perimeters(other.perimeters),
        inplane(other.inplane), z(other.z), faces(other.faces),
        lines(other.lines) {}

ostream& operator<< (ostream &out, const lineseg &l) {
    out << "(" << l.p1.transpose() << ",\t" << l.p2.transpose() << ")";
    return out;
}

ostream& operator<< (ostream &out, const levelset &ls) {
    out << "[verteces = " << ls.verteces.size()
        << " lines size " << ls.lines.size()
        << "]";
    return out;
}


// This file is part of the dune-gdt project:
//   https://github.com/dune-community/dune-gdt
// Copyright 2010-2018 dune-gdt developers and contributors. All rights reserved.
// License: Dual licensed as BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//      or  GPL-2.0+ (http://opensource.org/licenses/gpl-license)
//          with "runtime exception" (http://www.dune-project.org/license.html)
// Authors:
//   Felix Schindler (2015 - 2017)
//   Rene Milk       (2016 - 2018)
//   Tobias Leibner  (2018)

#ifndef DUNE_GDT_LOCAL_OPERATORS_INTEGRALS_HH
#define DUNE_GDT_LOCAL_OPERATORS_INTEGRALS_HH

#include <type_traits>

#include <boost/numeric/conversion/cast.hpp>

#include <dune/common/densematrix.hh>

#include <dune/geometry/quadraturerules.hh>

#include <dune/xt/common/matrix.hh>

#include <dune/xt/functions/interfaces.hh>

#include <dune/xt/la/container/common/matrix/dense.hh>

#include <dune/gdt/local/integrands/interfaces.hh>
#include <dune/gdt/type_traits.hh>

#include "interfaces.hh"

namespace Dune {
namespace GDT {

template <class BinaryEvaluationType,
          class TestBase,
          class AnsatzBase = TestBase,
          class Field = typename TestBase::RangeFieldType>
class LocalVolumeIntegralOperator : public LocalVolumeTwoFormInterface<TestBase, AnsatzBase, Field>
{
  static_assert(is_binary_volume_integrand<BinaryEvaluationType>::value, "");
  static_assert(std::is_same<typename TestBase::EntityType, typename AnsatzBase::EntityType>::value, "");
  static_assert(std::is_same<typename TestBase::DomainFieldType, typename AnsatzBase::DomainFieldType>::value, "");
  static_assert(TestBase::dimDomain == AnsatzBase::dimDomain, "");

  typedef LocalVolumeIntegralOperator<BinaryEvaluationType, TestBase, AnsatzBase, Field> ThisType;
  typedef LocalVolumeTwoFormInterface<TestBase, AnsatzBase, Field> BaseType;

  typedef typename TestBase::DomainFieldType D;
  static const size_t d = TestBase::dimDomain;

public:
  using typename BaseType::TestBaseType;
  using typename BaseType::AnsatzBaseType;
  using typename BaseType::FieldType;

  template <class... Args>
  explicit LocalVolumeIntegralOperator(Args&&... args)
    : integrand_(std::forward<Args>(args)...)
    , over_integrate_(0)
  {
  }

  template <class... Args>
  explicit LocalVolumeIntegralOperator(const int over_integrate, Args&&... args)
    : integrand_(std::forward<Args>(args)...)
    , over_integrate_(boost::numeric_cast<size_t>(over_integrate))
  {
  }

  template <class... Args>
  explicit LocalVolumeIntegralOperator(const size_t over_integrate, Args&&... args)
    : integrand_(std::forward<Args>(args)...)
    , over_integrate_(over_integrate)
  {
  }

  LocalVolumeIntegralOperator(const ThisType& other) = default;
  LocalVolumeIntegralOperator(ThisType&& source) = default;

  using BaseType::apply2;

  // copied from apply2 below
  // TODO: fix properly (use CommonDenseMatrix instead of DynamicMatrix everywhere in dune-gdt?)
  void apply2(const TestBaseType& test_base,
              const AnsatzBaseType& ansatz_base,
              XT::LA::CommonDenseMatrix<FieldType>& ret) const
  {
    const auto& entity = ansatz_base.entity();
    const auto local_functions = integrand_.localFunctions(entity);
    // create quadrature
    const size_t integrand_order = integrand_.order(local_functions, test_base, ansatz_base) + over_integrate_;
    const auto& quadrature = QuadratureRules<D, d>::rule(entity.type(), boost::numeric_cast<int>(integrand_order));
    // prepare storage
    const size_t rows = test_base.size();
    const size_t cols = ansatz_base.size();
    ret *= 0.0;
    assert(ret.rows() >= rows);
    assert(ret.cols() >= cols);
    DynamicMatrix<FieldType> integrand_eval(rows, cols, 0.); // \todo: make mutable member, after SMP refactor
    // loop over all quadrature points
    for (const auto& quadrature_point : quadrature) {
      const auto xx = quadrature_point.position();
      // integration factors
      const auto integration_factor = entity.geometry().integrationElement(xx);
      const auto quadrature_weight = quadrature_point.weight();
      // evaluate the integrand
      integrand_.evaluate(local_functions, test_base, ansatz_base, xx, integrand_eval);
      // compute integral
      ret.axpy(integration_factor * quadrature_weight, integrand_eval);
    } // loop over all quadrature points
  } // ... apply2(...)

  void
  apply2(const TestBaseType& test_base, const AnsatzBaseType& ansatz_base, DynamicMatrix<FieldType>& ret) const override
  {
    const auto& entity = ansatz_base.entity();
    const auto local_functions = integrand_.localFunctions(entity);
    // create quadrature
    const size_t integrand_order = integrand_.order(local_functions, test_base, ansatz_base) + over_integrate_;
    const auto& quadrature = QuadratureRules<D, d>::rule(entity.type(), boost::numeric_cast<int>(integrand_order));
    // prepare storage
    const size_t rows = test_base.size();
    const size_t cols = ansatz_base.size();
    ret *= 0.0;
    assert(ret.rows() >= rows);
    assert(ret.cols() >= cols);
    DynamicMatrix<FieldType> integrand_eval(rows, cols, 0.); // \todo: make mutable member, after SMP refactor
    // loop over all quadrature points
    for (const auto& quadrature_point : quadrature) {
      const auto xx = quadrature_point.position();
      // integration factors
      const auto integration_factor = entity.geometry().integrationElement(xx);
      const auto quadrature_weight = quadrature_point.weight();
      // evaluate the integrand
      integrand_.evaluate(local_functions, test_base, ansatz_base, xx, integrand_eval);
      // compute integral
      for (size_t ii = 0; ii < rows; ++ii) {
        const auto& integrand_eval_row = integrand_eval[ii];
        auto& ret_row = ret[ii];
        for (size_t jj = 0; jj < cols; ++jj)
          ret_row[jj] += integrand_eval_row[jj] * integration_factor * quadrature_weight;
      } // compute integral
    } // loop over all quadrature points
  } // ... apply2(...)

private:
  const BinaryEvaluationType integrand_;
  const size_t over_integrate_;
}; // class LocalVolumeIntegralOperator

template <class QuaternaryFaceIntegrandType,
          class TestBaseEntity,
          class Intersection,
          class AnsatzBaseEntity = TestBaseEntity,
          class TestBaseNeighbor = TestBaseEntity,
          class AnsatzBaseNeighbor = AnsatzBaseEntity,
          class Field = typename TestBaseEntity::RangeFieldType>
class LocalCouplingIntegralOperator : public LocalCouplingTwoFormInterface<TestBaseEntity,
                                                                           Intersection,
                                                                           AnsatzBaseEntity,
                                                                           TestBaseNeighbor,
                                                                           AnsatzBaseNeighbor,
                                                                           Field>
{
  static_assert(is_quaternary_face_integrand<QuaternaryFaceIntegrandType>::value, "");
  static_assert(std::is_same<typename TestBaseEntity::EntityType, typename AnsatzBaseEntity::EntityType>::value, "");
  static_assert(std::is_same<typename TestBaseEntity::EntityType, typename TestBaseNeighbor::EntityType>::value, "");
  static_assert(std::is_same<typename TestBaseEntity::EntityType, typename AnsatzBaseNeighbor::EntityType>::value, "");
  static_assert(
      std::is_same<typename TestBaseEntity::DomainFieldType, typename AnsatzBaseEntity::DomainFieldType>::value, "");
  static_assert(
      std::is_same<typename TestBaseEntity::DomainFieldType, typename TestBaseNeighbor::DomainFieldType>::value, "");
  static_assert(
      std::is_same<typename TestBaseEntity::DomainFieldType, typename AnsatzBaseNeighbor::DomainFieldType>::value, "");
  static_assert(TestBaseEntity::dimDomain == AnsatzBaseEntity::dimDomain, "");
  static_assert(TestBaseEntity::dimDomain == TestBaseNeighbor::dimDomain, "");
  static_assert(TestBaseEntity::dimDomain == AnsatzBaseNeighbor::dimDomain, "");

  typedef LocalCouplingIntegralOperator<QuaternaryFaceIntegrandType,
                                        TestBaseEntity,
                                        Intersection,
                                        AnsatzBaseEntity,
                                        TestBaseNeighbor,
                                        AnsatzBaseNeighbor,
                                        Field>
      ThisType;
  typedef LocalCouplingTwoFormInterface<TestBaseEntity,
                                        Intersection,
                                        AnsatzBaseEntity,
                                        TestBaseNeighbor,
                                        AnsatzBaseNeighbor,
                                        Field>
      BaseType;
  typedef typename TestBaseEntity::DomainFieldType D;
  static const size_t d = TestBaseEntity::dimDomain;

public:
  using typename BaseType::TestBaseEntityType;
  using typename BaseType::AnsatzBaseEntityType;
  using typename BaseType::TestBaseNeighborType;
  using typename BaseType::AnsatzBaseNeighborType;
  using typename BaseType::IntersectionType;
  using typename BaseType::FieldType;

  template <class... Args>
  explicit LocalCouplingIntegralOperator(Args&&... args)
    : integrand_(std::forward<Args>(args)...)
    , over_integrate_(0)
  {
  }

  template <class... Args>
  explicit LocalCouplingIntegralOperator(const int over_integrate, Args&&... args)
    : integrand_(std::forward<Args>(args)...)
    , over_integrate_(boost::numeric_cast<size_t>(over_integrate))
  {
  }

  template <class... Args>
  explicit LocalCouplingIntegralOperator(const size_t over_integrate, Args&&... args)
    : integrand_(std::forward<Args>(args)...)
    , over_integrate_(over_integrate)
  {
  }

  LocalCouplingIntegralOperator(const ThisType& other) = default;
  LocalCouplingIntegralOperator(ThisType&& source) = default;

  void apply2(const TestBaseEntityType& test_base_en,
              const AnsatzBaseEntityType& ansatz_base_en,
              const TestBaseNeighborType& test_base_ne,
              const AnsatzBaseNeighborType& ansatz_base_ne,
              const IntersectionType& intersection,
              DynamicMatrix<FieldType>& ret_en_en,
              DynamicMatrix<FieldType>& ret_ne_ne,
              DynamicMatrix<FieldType>& ret_en_ne,
              DynamicMatrix<FieldType>& ret_ne_en) const override
  {
    // local inducing function
    const auto& entity = test_base_en.entity();
    const auto& neighbor = test_base_ne.entity();
    const auto local_functions_en = integrand_.localFunctions(entity);
    const auto local_functions_ne = integrand_.localFunctions(neighbor);
    // quadrature
    const size_t integrand_order =
        integrand_.order(
            local_functions_en, local_functions_ne, test_base_en, ansatz_base_en, test_base_ne, ansatz_base_ne)
        + over_integrate_;
    const auto& quadrature =
        QuadratureRules<D, d - 1>::rule(intersection.type(), boost::numeric_cast<int>(integrand_order));
    // check matrices
    ret_en_en *= 0.0;
    ret_ne_ne *= 0.0;
    ret_en_ne *= 0.0;
    ret_ne_en *= 0.0;
    const size_t rows_en = test_base_en.size();
    const size_t cols_en = ansatz_base_en.size();
    const size_t rows_ne = test_base_ne.size();
    const size_t cols_ne = ansatz_base_ne.size();
    assert(ret_en_en.rows() >= rows_en);
    assert(ret_en_en.cols() >= cols_en);
    assert(ret_ne_ne.rows() >= rows_ne);
    assert(ret_ne_ne.cols() >= cols_ne);
    assert(ret_en_ne.rows() >= rows_en);
    assert(ret_en_ne.cols() >= cols_ne);
    assert(ret_ne_en.rows() >= rows_en);
    assert(ret_ne_en.cols() >= cols_en);
    // \todo: make mutable member, after SMP refactor
    DynamicMatrix<FieldType> integrand_eval_en_en(rows_en, cols_en, 0.);
    DynamicMatrix<FieldType> integrand_eval_ne_ne(rows_ne, cols_ne, 0.);
    DynamicMatrix<FieldType> integrand_eval_en_ne(rows_en, cols_ne, 0.);
    DynamicMatrix<FieldType> integrand_eval_ne_en(rows_ne, cols_en, 0.);
    // loop over all quadrature points
    for (const auto& quadrature_point : quadrature) {
      const auto xx = quadrature_point.position();
      const auto integration_factor = intersection.geometry().integrationElement(xx);
      const auto quadrature_weight = quadrature_point.weight();
      // evaluate local
      integrand_.evaluate(local_functions_en,
                          local_functions_ne,
                          test_base_en,
                          ansatz_base_en,
                          test_base_ne,
                          ansatz_base_ne,
                          intersection,
                          xx,
                          integrand_eval_en_en,
                          integrand_eval_ne_ne,
                          integrand_eval_en_ne,
                          integrand_eval_ne_en);
      // compute integrals
      // loop over all entity test basis functions
      for (size_t ii = 0; ii < rows_en; ++ii) {
        auto& ret_en_en_row = ret_en_en[ii];
        auto& ret_en_ne_row = ret_en_ne[ii];
        const auto& integrand_eval_en_en_row = integrand_eval_en_en[ii];
        const auto& integrand_eval_en_ne_row = integrand_eval_en_ne[ii];
        // loop over all entity ansatz basis functions
        for (size_t jj = 0; jj < cols_en; ++jj) {
          ret_en_en_row[jj] += integrand_eval_en_en_row[jj] * integration_factor * quadrature_weight;
        }
        // loop over all neighbor ansatz basis functions
        for (size_t jj = 0; jj < cols_ne; ++jj) {
          ret_en_ne_row[jj] += integrand_eval_en_ne_row[jj] * integration_factor * quadrature_weight;
        }
      }
      // loop over all neighbor test basis functions
      for (size_t ii = 0; ii < rows_ne; ++ii) {
        auto& ret_ne_ne_row = ret_ne_ne[ii];
        auto& ret_ne_en_row = ret_ne_en[ii];
        const auto& integrand_eval_ne_ne_row = integrand_eval_ne_ne[ii];
        const auto& integrand_eval_ne_en_row = integrand_eval_ne_en[ii];
        // loop over all neighbor ansatz basis functions
        for (size_t jj = 0; jj < cols_ne; ++jj) {
          ret_ne_ne_row[jj] += integrand_eval_ne_ne_row[jj] * integration_factor * quadrature_weight;
        }
        // loop over all entity ansatz basis functions
        for (size_t jj = 0; jj < cols_en; ++jj) {
          ret_ne_en_row[jj] += integrand_eval_ne_en_row[jj] * integration_factor * quadrature_weight;
        }
      }
    } // loop over all quadrature points
  } // void apply(...) const

private:
  const QuaternaryFaceIntegrandType integrand_;
  const size_t over_integrate_;
}; // class LocalCouplingIntegralOperator

template <class BinaryFaceIntegrandType,
          class TestBase,
          class Intersection,
          class AnsatzBase = TestBase,
          class Field = typename TestBase::RangeFieldType>
class LocalBoundaryIntegralOperator : public LocalBoundaryTwoFormInterface<TestBase, Intersection, AnsatzBase, Field>
{
  static_assert(is_binary_face_integrand<BinaryFaceIntegrandType>::value, "");
  static_assert(std::is_same<typename TestBase::EntityType, typename AnsatzBase::EntityType>::value, "");
  static_assert(std::is_same<typename TestBase::DomainFieldType, typename AnsatzBase::DomainFieldType>::value, "");
  static_assert(TestBase::dimDomain == AnsatzBase::dimDomain, "");

  typedef LocalBoundaryIntegralOperator<BinaryFaceIntegrandType, TestBase, Intersection, AnsatzBase, Field> ThisType;
  typedef LocalBoundaryTwoFormInterface<TestBase, Intersection, AnsatzBase, Field> BaseType;
  typedef typename TestBase::DomainFieldType D;
  static const size_t d = TestBase::dimDomain;

public:
  using typename BaseType::TestBaseType;
  using typename BaseType::AnsatzBaseType;
  using typename BaseType::IntersectionType;
  using typename BaseType::FieldType;

  template <class... Args>
  LocalBoundaryIntegralOperator(Args&&... args)
    : integrand_(std::forward<Args>(args)...)
    , over_integrate_(0)
  {
  }

  template <class... Args>
  LocalBoundaryIntegralOperator(const int over_integrate, Args&&... args)
    : integrand_(std::forward<Args>(args)...)
    , over_integrate_(boost::numeric_cast<size_t>(over_integrate))
  {
  }

  template <class... Args>
  LocalBoundaryIntegralOperator(const size_t over_integrate, Args&&... args)
    : integrand_(std::forward<Args>(args)...)
    , over_integrate_(over_integrate)
  {
  }

  void apply2(const TestBaseType& test_base,
              const AnsatzBaseType& ansatz_base,
              const IntersectionType& intersection,
              DynamicMatrix<FieldType>& ret) const override
  {
    // local inducing function
    const auto& entity = test_base.entity();
    const auto local_functions = integrand_.localFunctions(entity);
    // create quadrature
    const auto integrand_order = integrand_.order(local_functions, test_base, ansatz_base) + over_integrate_;
    const auto& quadrature =
        QuadratureRules<D, d - 1>::rule(intersection.type(), boost::numeric_cast<int>(integrand_order));
    // prepare storage
    ret *= 0.0;
    const size_t rows = test_base.size();
    const size_t cols = ansatz_base.size();
    assert(ret.rows() >= rows);
    assert(ret.cols() >= cols);
    DynamicMatrix<FieldType> integrand_eval(rows, cols, 0.); // \todo: make mutable member, after SMP refactor
    // loop over all quadrature points
    for (const auto& quadrature_point : quadrature) {
      const auto xx = quadrature_point.position();
      const auto integration_factor = intersection.geometry().integrationElement(xx);
      const auto quadrature_weight = quadrature_point.weight();
      // evaluate local
      integrand_.evaluate(local_functions, test_base, ansatz_base, intersection, xx, integrand_eval);
      // compute integral
      assert(integrand_eval.rows() >= rows);
      assert(integrand_eval.cols() >= cols);
      // loop over all test basis functions
      for (size_t ii = 0; ii < rows; ++ii) {
        auto& ret_row = ret[ii];
        const auto& integrand_eval_row = integrand_eval[ii];
        // loop over all ansatz basis functions
        for (size_t jj = 0; jj < cols; ++jj) {
          ret_row[jj] += integrand_eval_row[jj] * integration_factor * quadrature_weight;
        }
      }
    }
  } // ... apply(...)

private:
  const BinaryFaceIntegrandType integrand_;
  const size_t over_integrate_;
}; // class LocalBoundaryIntegralOperator

} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_LOCAL_OPERATORS_INTEGRALS_HH


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

// Standard includes
#include <fstream>
#include <string>

// Third party includes
#include <boost/format.hpp>

// VOTCA includes
#include <votca/tools/constants.h>

// Local VOTCA includes
#include <votca/xtp/staticsite.h>

namespace votca {
namespace xtp {

Eigen::Matrix3d StaticSite::CalculateCartesianMultipole() const {
  // We are transforming here just quadrupoles
  // const  Eigen::VectorXd& MP = _multipole;
  const Vector9d& MP = _Q;
  Eigen::Matrix3d theta = Eigen::Matrix3d::Zero();
  if (_rank > 1) {
    double sqr3 = std::sqrt(3);
    theta(0, 0) = 0.5 * (-MP(4) + sqr3 * MP(7));     // theta_xx
    theta(1, 1) = 0.5 * (-MP(4) - sqr3 * MP(7));     // theta_yy
    theta(2, 2) = MP(4);                             // theta_zz
    theta(0, 1) = theta(1, 0) = 0.5 * sqr3 * MP(8);  // theta_xy = theta_yx
    theta(0, 2) = theta(2, 0) = 0.5 * sqr3 * MP(5);  // theta_xz = theta_zx
    theta(1, 2) = theta(2, 1) = 0.5 * sqr3 * MP(6);  // theta_yz = theta_zy
  }
  return theta;
}

Eigen::VectorXd StaticSite::CalculateSphericalMultipole(
    const Eigen::Matrix3d& quad_cart) {
  Eigen::VectorXd quadrupole_polar = Eigen::VectorXd::Zero(5);
  const double sqr3 = std::sqrt(3);
  quadrupole_polar(0) = quad_cart(2, 2);
  quadrupole_polar(1) = (2. / sqr3) * quad_cart(0, 2);
  quadrupole_polar(2) = (2. / sqr3) * quad_cart(1, 2);
  quadrupole_polar(3) = (1. / sqr3) * (quad_cart(0, 0) - quad_cart(1, 1));
  quadrupole_polar(4) = (2. / sqr3) * quad_cart(0, 1);
  return quadrupole_polar;
}

void StaticSite::Rotate(const Eigen::Matrix3d& R,
                        const Eigen::Vector3d& refPos) {
  Eigen::Vector3d dir = _pos - refPos;
  dir = R * dir;
  _pos = refPos + dir;  // Rotated Position
  if (_rank > 0) {
    const Eigen::Vector3d temp = R * _Q.segment<3>(1);
    _Q.segment<3>(1) = temp;
  }
  if (_rank > 1) {
    Eigen::Matrix3d cartesianquad = CalculateCartesianMultipole();
    Eigen::Matrix3d rotated = R * cartesianquad * R.transpose();
    _Q.segment<5>(4) = CalculateSphericalMultipole(rotated);
  }
  return;
}

void StaticSite::Translate(const Eigen::VectorXd& shift) {
  _pos += shift;
  return;
}

std::string StaticSite::writepolarization() const {
  tools::Elements e;
  double default_pol = 1;  // default is alway 1A^3
  try {
    default_pol =
        e.getPolarizability(_element) * std::pow(tools::conv::nm2ang, 3);
  } catch (const std::runtime_error&) {
    ;
  }
  return (boost::format("     P %1$+1.7f\n") % default_pol).str();
}

std::string StaticSite::WriteMpsLine(std::string unit) const {
  double conv_pos = 1.;
  if (unit == "angstrom") {
    conv_pos = tools::conv::bohr2ang;
  } else if (unit == "bohr") {
    conv_pos = 1.;
  } else {
    throw std::runtime_error(
        " StaticSite::WriteMpsLine: Unit conversion not known");
  }
  std::string output = "";
  output += (boost::format(" %1$2s %2$+1.7f %3$+1.7f %4$+1.7f Rank %5$d\n") %
             _element % (_pos(0) * conv_pos) % (_pos(1) * conv_pos) %
             (_pos(2) * conv_pos) % _rank)
                .str();
  output += (boost::format("    %1$+1.7f\n") % getCharge()).str();
  if (_rank > 0) {
    // Dipole z x y
    output += (boost::format("    %1$+1.7f %2$+1.7f %3$+1.7f\n") % _Q(3) %
               _Q(1) % _Q(2))
                  .str();
    if (_rank > 1) {
      // Quadrupole 20 21c 21s 22c 22s
      output +=
          (boost::format("    %1$+1.7f %2$+1.7f %3$+1.7f %4$+1.7f %5$+1.7f\n") %
           _Q(4) % _Q(5) % _Q(6) % _Q(7) % _Q(8))
              .str();
    }
  }
  // Polarizability
  output += writepolarization();
  return output;
}

void StaticSite::SetupCptTable(CptTable& table) const {
  table.addCol(_id, "index", HOFFSET(data, id));
  table.addCol(_element, "type", HOFFSET(data, element));

  table.addCol(_pos[0], "posX", HOFFSET(data, posX));
  table.addCol(_pos[1], "posY", HOFFSET(data, posY));
  table.addCol(_pos[2], "posZ", HOFFSET(data, posZ));

  table.addCol(_rank, "rank", HOFFSET(data, rank));

  table.addCol(_Q[0], "Q00", HOFFSET(data, Q00));
  table.addCol(_Q[1], "Q11c", HOFFSET(data, Q11c));
  table.addCol(_Q[2], "Q11s", HOFFSET(data, Q11s));
  table.addCol(_Q[3], "Q10", HOFFSET(data, Q10));
  table.addCol(_Q[4], "Q20", HOFFSET(data, Q20));
  table.addCol(_Q[5], "Q21c", HOFFSET(data, Q21c));
  table.addCol(_Q[6], "Q21s", HOFFSET(data, Q21s));
  table.addCol(_Q[7], "Q22c", HOFFSET(data, Q22c));
  table.addCol(_Q[8], "Q22s", HOFFSET(data, Q22s));
}

void StaticSite::WriteData(data& d) const {
  d.id = _id;
  d.element = const_cast<char*>(_element.c_str());
  d.posX = _pos[0];
  d.posY = _pos[1];
  d.posZ = _pos[2];

  d.rank = _rank;

  d.Q00 = _Q[0];
  d.Q11c = _Q[1];
  d.Q11s = _Q[2];
  d.Q10 = _Q[3];
  d.Q20 = _Q[4];
  d.Q21c = _Q[5];
  d.Q21s = _Q[6];
  d.Q22c = _Q[7];
  d.Q22s = _Q[8];
}

void StaticSite::ReadData(const data& d) {
  _id = d.id;
  _element = std::string(d.element);
  free(d.element);
  _pos[0] = d.posX;
  _pos[1] = d.posY;
  _pos[2] = d.posZ;

  _rank = d.rank;

  _Q[0] = d.Q00;
  _Q[1] = d.Q11c;
  _Q[2] = d.Q11s;
  _Q[3] = d.Q10;
  _Q[4] = d.Q20;
  _Q[5] = d.Q21c;
  _Q[6] = d.Q21s;
  _Q[7] = d.Q22c;
  _Q[8] = d.Q22s;
}

}  // namespace xtp
}  // namespace votca


#include <stdio.h>
#include <cstdlib>
#include <ctime>
#include <fstream>
#include <dirent.h>
#include <iostream>
#include <sstream>
#include <string>

#include "stereo_sdf/SGMStereo.h"
#include "stereo_sdf/utils.h"

#include <Eigen/Dense>

// Headers from package stereo_utils.
#include <stereo_utils/stereo_utils.hpp>
#include <stereo_utils/Common.hpp>

using namespace std;

using JSON = nlohmann::json;
namespace su = stereo_utils;

/* -------------------------- SET THESE PARAMETERS --------------------------- */
/* stereo options */
#define STEREO_FULL_PIPELINE -1
#define STEREO_LEFT_ONLY      0

/* cost volume update options */
#define VOLUME_UPDATE_NAIVE   3
#define VOLUME_UPDATE_DIFFB   2
#define VOLUME_UPDATE_NEIGH   1
#define VOLUME_UPDATE_NONE   -1

/* image save flag */
#define SAVE_IMAGES           1

/* ground truth sampling option */
double SAMPLING_FRACTION   = 0.50;

/* disparity scaling factor (256 for KITTI) */
double SCALING_FACTOR      = 256.0;

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

/* ------------ Type definition for interfacing the JSON file. --------------- */

struct CaseDescription {
	std::string name    = "";   // Name of the test case.
	std::string fn0     = "";   // Filename of image Ref.
	std::string fn1     = "";   // Filename of image Tst.
	std::string fnD     = "";   // Filename of the true disparity map.
	std::string outDir  = "";   // Output directory.
	std::string fnQ     = "";   // Filename of the Q matrix.
	float qFactor       = 1.0f; // Scale factor of Q.
	float dOffs         = 0.0f; // Disparity offset. Non-zero for Middlebury dataset. Zero for other datasets.
    float distLimit     = 20.f; // The distance limit for point cloud generation.
    double gtSampleFrac = 0.5;  // Ground truth sampling option.

    SGMParams sgmParams = SGMParams();
};

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

void SemiGlobalMatching(const cv::Mat &leftImage,
                        const cv::Mat &rightImage,
                        cv::Mat &dispImage,
                        int STEREO_PIPELINE_MODE,
                        const std::string cameraParamFile,
                        cv::Mat depthImage,
                        cv::Mat weightImg,
                        int FUSE_FLAG,
                        const SGMParams &sgmParams)
{
    png::image<png::rgb_pixel> leftImageSGM, rightImageSGM;
    Utils utilities;
    utilities.convertCVMatToPNG(leftImage, leftImageSGM);
    utilities.convertCVMatToPNG(rightImage, rightImageSGM);
    size_t width = leftImageSGM.get_width();
    size_t height = leftImageSGM.get_height();
    if (width != rightImageSGM.get_width() ||
        height != rightImageSGM.get_height())
    {
        dispImage = cv::Mat1w();
        return;
    }
    float* dispImageFloat = (float*)malloc(width*height*sizeof(float));
    SGMStereo sgm;

    // Configure the SGM method.
    std::cout << "P1 = " << sgmParams.P1 << ", "
              << "P2 = " << sgmParams.P2 << ". \n";
    sgm.setSmoothnessCostParameters( sgmParams.P1, sgmParams.P2 );
    sgm.setDisparityTotal( sgmParams.total );

    cv::Mat leftImageGray;
    cv::cvtColor(leftImage, leftImageGray, CV_RGB2GRAY);
    sgm.compute(leftImageSGM,
                rightImageSGM,
                dispImageFloat,
                STEREO_PIPELINE_MODE,
                cameraParamFile,
                depthImage,
                FUSE_FLAG,
                leftImageGray,
                weightImg);
    dispImage = utilities.convertFloatToCVMat(width, height, dispImageFloat);
    free(dispImageFloat);
}

void displayMinMax(cv::Mat array)
{
    double min, max;
    cv::minMaxLoc(array, &min, &max);
    std::cout << "Minimum: " << min << " | Maximum: " << max << std::endl;
}

static void save_ply(
    const std::string &fn,
    const Eigen::MatrixXf &Q,
    const cv::Mat &dispFloat,
    const cv::Mat &color,
    float distLimit=20.f,
    float dOffs=0.f ) {

    cv::Mat dispFloat32;
    dispFloat.convertTo(dispFloat32, CV_32FC1);

    const bool flagFlip       = true; // Flip the point cloud so that the y-axis is upwards.
    const bool flagBinary     = true; // Write binary PLY files.
    const float distanceLimit = 20.f; // All points with depth greater than this value will be ignored.
    cv::Mat predDispOffs      = dispFloat32 + dOffs; // This is required by Middlebury dataset. dOffs will be zero for other dataset.
    su::write_ply_with_color(fn,
        predDispOffs, color,
        Q, flagFlip, distanceLimit, flagBinary);
}

static void process( const CaseDescription &cd ) {
    /* input and output directories */
    // std::string repo_dir = argv[1];
    // std::string left_image_uri = repo_dir + "imgs/stereo_left.png";
    // std::string right_image_uri = repo_dir + "imgs/stereo_right.png";
    // std::string left_depth_uri = repo_dir + "imgs/gt_disparity.png";
    // std::string save_dir = repo_dir + "results/";

    std::string left_image_uri  = cd.fn0;
    std::string right_image_uri = cd.fn1;
    std::string left_depth_uri  = cd.fnD;
    std::string save_dir        = cd.outDir + "/";

    su::test_directory(save_dir);

    Eigen::MatrixXf Q;
    if ( cd.fnQ != "" ) {
        su::read_matrix(cd.fnQ, 4, 4, " ", Q);

		// Update the Q matrix according to the scale factor.
		Q(0, 3) *= cd.qFactor;
		Q(1, 3) *= cd.qFactor;
		Q(2, 3) *= cd.qFactor;
    }

    Utils utilities;

    std::cout << "DATA DETAILS: " << std::endl;
    std::cout << "--- Left Image: " << left_image_uri << std::endl;
    std::cout << "--- Right Image: " << right_image_uri << std::endl;
    std::cout << "--- Disparity: (GT) " << left_depth_uri << std::endl;

    std::vector<int> compression_params;
    compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION);
    compression_params.push_back(0);

    cv::Mat left_image_clr = cv::imread(left_image_uri);
    cv::Mat right_image_clr = cv::imread(right_image_uri);

    cv::Mat_<double> disp_image = cv::imread(left_depth_uri,
                                             cv::IMREAD_ANYDEPTH);
    disp_image = disp_image / SCALING_FACTOR;
    std::cout << "{read status:} successfully read input images.." << std::endl;

    /* EVALUATION A: SEMI GLOBAL MATCHING */
    std::cout << "\n{EVALUATION A:} -- SEMI GLOBAL MATCHING -- " << std::endl;
    cv::Mat disparity_image;
    SemiGlobalMatching(left_image_clr,
                       right_image_clr,
                       disparity_image,
                       STEREO_LEFT_ONLY,
                       "no_params_needed",
                       cv::Mat(),
                       cv::Mat(),
                       VOLUME_UPDATE_NONE,
                       cd.sgmParams);
    if (SAVE_IMAGES) {
        std::string save_file_name = + "sgm_default.png";
        std::string save_url = save_dir + save_file_name;
        std::cout << "{SGM} saving image to: " << save_url << std::endl;
        cv::imwrite(save_url, disparity_image, compression_params);
    }

    cv::Mat_<double> disp_SGM = disparity_image / SCALING_FACTOR;
    /* evaluate SGM */
    cv::Mat_<double> error_image_sgm;
    double average_error_sgm;
    cv::Mat sample_mask_sgm = cv::Mat::zeros(disp_SGM.rows,
                                             disp_SGM.cols,
                                             CV_32FC1);
    utilities.calculateAccuracy(disp_SGM,
                                disp_image,
                                average_error_sgm,
                                error_image_sgm,
                                sample_mask_sgm);
    std::cout << "{SGM} avg error: " << average_error_sgm << std::endl;

    cv::Mat sample_mask;
    // utilities.generateRandomSamplingMask(disp_image,
    //                                      sample_mask,
    //                                      SAMPLING_FRACTION);
    utilities.generateRandomSamplingMask(disp_image,
                                         sample_mask,
                                         cd.gtSampleFrac);

    if (SAVE_IMAGES) {
        std::string save_file_name = "sparse_mask.png";
        std::string save_url = save_dir + save_file_name;
        std::cout << "{MASK} saving image to: " << save_url << std::endl;
        cv::imwrite(save_url, sample_mask, compression_params);
    }
    cv::Mat masked_depth;
    disp_image.copyTo(masked_depth, sample_mask);

    // Save PLY file.
    if ( cd.fnQ != "" ) {
        std::string plyFn = save_dir + "Cloud_SGM.ply";
        save_ply(plyFn, Q,
            disp_SGM, left_image_clr,
            cd.distLimit, cd.dOffs );
        std::cout << "Point cloud saved to " << plyFn << "\n";
    }

    /* EVALUATION B: USE SPARSE LIDAR POINTS FOR NAIVE FUSION */
    std::cout << "\n{EVALUATION B:} -- NAIVE LIDAR FUSION -- " << std::endl;
    cv::Mat disparity_image_sl_naive;
    SemiGlobalMatching(left_image_clr,
                       right_image_clr,
                       disparity_image_sl_naive,
                       STEREO_LEFT_ONLY,
                       "no_params_needed",
                       masked_depth,
                       cv::Mat(),
                       VOLUME_UPDATE_NAIVE,
                       cd.sgmParams);
    if (SAVE_IMAGES) {
        std::string save_file_name = "fuse_naive.png";
        std::string save_url = save_dir + save_file_name;
        std::cout << "{Naive Fusion} saving image to: " << save_url << std::endl;
        cv::imwrite(save_url, disparity_image_sl_naive, compression_params);
    }
    cv::Mat_<double> disp_NF = disparity_image_sl_naive / SCALING_FACTOR;
    /* evaluate naive fusion */
    cv::Mat_<double> error_image_nf;
    double average_error_nf;
    utilities.calculateAccuracy(disp_NF,
                                disp_image,
                                average_error_nf,
                                error_image_nf, sample_mask);
    std::cout << "{NAIVE FUSION} avg error: " << average_error_nf << std::endl;

    // Save PLY file.
    if ( cd.fnQ != "" ) {
        std::string plyFn = save_dir + "Cloud_Naive.ply";
        save_ply(plyFn, Q,
            disp_NF, left_image_clr,
            cd.distLimit, cd.dOffs );
        std::cout << "Point cloud saved to " << plyFn << "\n";
    }

    /*EVALUATION C: USE DIFFUSION BASED METHOD */
    std::cout << "\n{EVALUATION C:} -- DIFFUSION BASED -- " << std::endl;
    cv::Mat disparity_image_db;
    SemiGlobalMatching(left_image_clr,
                       right_image_clr,
                       disparity_image_db,
                       STEREO_LEFT_ONLY,
                       "no_params_needed",
                       masked_depth,
                       cv::Mat(),
                       VOLUME_UPDATE_DIFFB,
                       cd.sgmParams);
    if (SAVE_IMAGES) {
        std::string save_file_name = "fuse_diffusionbased.png";
        std::string save_url = save_dir + save_file_name;
        std::cout << "{DB} saving image to: " << save_url << std::endl;
        cv::imwrite(save_url, disparity_image_db, compression_params);
    }
    cv::Mat_<double> disp_DB = disparity_image_db / SCALING_FACTOR;
    /* evaluate diffusion based confidence propagation method */
    cv::Mat_<double> error_image_db;
    double average_error_db;
    utilities.calculateAccuracy(disp_DB,
                                disp_image,
                                average_error_db,
                                error_image_db,
                                sample_mask);
    std::cout << "{DB} avg error: " << average_error_db << std::endl;

    // Save PLY file.
    if ( cd.fnQ != "" ) {
        std::string plyFn = save_dir + "Cloud_Diffusion.ply";
        save_ply(plyFn, Q,
            disp_DB, left_image_clr,
            cd.distLimit, cd.dOffs );
        std::cout << "Point cloud saved to " << plyFn << "\n";
    }

    /*EVALUATION D: USE BASIC BILATERAL COST UPDATE */
    std::cout << "\n{EVALUATION D:} -- NEIGHBORHOOD SUPPORT -- " << std::endl;
    cv::Mat disparity_image_ns;
    SemiGlobalMatching(left_image_clr,
                       right_image_clr,
                       disparity_image_ns,
                       STEREO_LEFT_ONLY,
                       "no_params_needed",
                       masked_depth,
                       cv::Mat(),
                       VOLUME_UPDATE_NEIGH,
                       cd.sgmParams);
    if (SAVE_IMAGES) {
        std::string save_file_name = "fuse_neighborhoodsupport.png";
        std::string save_url = save_dir + save_file_name;
        std::cout << "{NS} saving image to: " << save_url << std::endl;
        cv::imwrite(save_url, disparity_image_ns, compression_params);
    }
    cv::Mat_<double> disp_NS = disparity_image_ns / SCALING_FACTOR;
    /* evaluate neighborhood support method */
    cv::Mat_<double> error_image_ns;
    double average_error_ns;
    utilities.calculateAccuracy(disp_NS,
                                disp_image,
                                average_error_ns,
                                error_image_ns,
                                sample_mask);
    std::cout << "{NS} avg error: " << average_error_ns << std::endl;

    // Save PLY file.
    if ( cd.fnQ != "" ) {
        std::string plyFn = save_dir + "Cloud_Neighbor.ply";
        save_ply(plyFn, Q,
            disp_NS, left_image_clr,
            cd.distLimit, cd.dOffs );
        std::cout << "Point cloud saved to " << plyFn << "\n";
    }
}

int main(int argc, char** argv)
{
    if ( argc < 2 ) {
		std::cerr << "Must specify the input JSON file. \n";
		throw std::runtime_error("Must specify the input JSON file. ");
	}

	// Read the JSON file.
	std::shared_ptr<JSON> pJSON = su::read_json(argv[1]);
	auto& cases = (*pJSON)["cases"];
	const int N = cases.size();

	// Process all the cases.
	for ( int i = 0; i < N; ++i ) {
        if ( true != cases[i]["enable"] )
            continue;

		auto cd = CaseDescription();
		cd.fn0  = cases[i]["fn0"]; // Filename of image 0.
		cd.fn1  = cases[i]["fn1"]; // Filename of image 1.

		// True disparity if exists.
		cd.fnD = cases[i]["fnD"];

		cd.name         = cases[i]["name"];         // Case name.
        cd.gtSampleFrac = cases[i]["gtSampleFrac"]; // The true data sample fraction.

        std::stringstream ss;
        std::string tempDir = cases[i]["outDir"]; // This strips the double quotes.
        ss << tempDir << "/" << cd.name << "_" << cd.gtSampleFrac;
        cd.outDir = ss.str(); // Output directory.

		// Q matrix if exists.
		if ( cases[i].find("fnQ") != cases[i].end() ) {
			cd.fnQ       = cases[i]["fnQ"];
			cd.qFactor   = cases[i]["QF"];
			cd.dOffs     = cases[i]["dOffs"];
            cd.distLimit = cases[i]["distLimit"];
		} else {
			cd.fnQ       = "";
			cd.qFactor   = 1.f;
			cd.dOffs     = 0.f;
            cd.distLimit = 20.f;
		}

        // SGM parameters.
        cd.sgmParams.P1    = cases[i]["sgm"]["P1"];
        cd.sgmParams.P2    = cases[i]["sgm"]["P2"];
        cd.sgmParams.total = cases[i]["sgm"]["total"];

		std::cout << "\n========== Procesing " << cd.name << "_" << cd.gtSampleFrac << ". ==========\n\n";
		process( cd );
	}

	std::cout << "SPS-Stereo done. \n";

    return 0;
}


// This file is part of DM-HEOM (https://github.com/noma/dm-heom)
//
// Copyright (c) 2015-2019 Matthias Noack, Zuse Institute Berlin
//
// Licensed under the 3-clause BSD License, see accompanying LICENSE,
// CONTRIBUTORS.md, and README.md for further information.

#include <exception>
#include <fstream>
#include <iostream>

#include <boost/format.hpp>
#include <noma/num/meta_stepper.hpp>

#include "heom/command_line.hpp"
#include "heom/common.hpp"
#include "heom/dipole_matrix.hpp"
#include "heom/handle_main_exception.hpp"
#include "heom/hierarchy_norm.hpp"
#include "heom/instance.hpp"
#include "heom/make_file_observer_list.hpp"
#include "heom/ocl_config.hpp"
#include "heom/ode.hpp"
#include "heom/population_dynamics_solver.hpp"
#include "heom/two_dimensional_spectra_config.hpp"
#include "heom/sites_to_states.hpp"

namespace bmt = ::heom::bmt;
namespace num = ::heom::num;
namespace ocl = ::heom::ocl;
using heom::int_t;
using heom::real_t;
using heom::complex_t;
using heom::real_format;
using heom::default_delimiter;
using heom::complex_matrix_t;

int main(int argc, char* argv[])
{
	// start runtime measurement
	bmt::timer app_timer;

	// output compile time configuration
	std::cout << "-------------------- Compile-Time Configuration ------------" << std::endl;
	heom::write_compile_config(std::cout);
	std::cout << "------------------------------------------------------------" << std::endl;

	std::exception_ptr eptr;
	try {
		// command line parsing
		heom::command_line command_line { argc, argv };

		std::cout << "Parsing OpenCL configuration from file: " << command_line.ocl_config_filename() << std::endl;
		heom::ocl_config ocl_config(command_line.ocl_config_filename());
		std::cout << "Parsing HEOM configuration from file: " << command_line.heom_config_filename() << std::endl;
		heom::two_dimensional_spectra_config heom_config(command_line.heom_config_filename());

		heom::hierarchy_graph complete_graph(heom_config.baths_number(), heom_config.baths_matsubaras(), heom_config.system_ado_depth());

		using stepper_t = num::meta_stepper; // uses solver_stepper_type from config
		using solver_t = heom::population_dynamics_solver<heom::ode, stepper_t, heom::hierarchy_norm>;

		const int_t steps_per_iteration = heom_config.program_observe_steps();
		const int_t t_1_iterations = heom_config.spectra_steps_t_1() / heom_config.program_observe_steps(); // not including zero
		const int_t t_3_iterations = heom_config.spectra_steps_t_3() / heom_config.program_observe_steps() + 1; // including 0 and max_steps 0 : steps_per_iteration : spectra_t_3_steps

		// prepare output file (so we run into possible file errors before starting the computation)
		auto& observation_filename = heom_config.observations().get().front().get().second; // filename of first specified observation, guaranteed to exist by config checks
		std::ofstream observation_file(observation_filename);
		if (observation_file.fail())
			throw std::runtime_error("Error: could not open output file: " + observation_filename);

		// output data structure for traces
		const int_t num_t_1_observations = t_1_iterations + 1;
		const int_t num_t_3_observations = t_3_iterations;

		// pre-allocate t_3 x t_1 sized 2d vector for accumulating the observations
		//std::vector<std::vector<heom::matrix_trace_observation_view>> observations(num_t_3_observations);
		std::vector<std::vector<complex_t>> observations(num_t_3_observations);
		for (auto& vec : observations) {
			vec.resize(num_t_1_observations, complex_t(0.0, 0.0));
		}

//		std::cout << "Coupling identity: " << heom::state_baths_coupling(heom_config.baths_coupling(), heom::sites_to_states_mode_t::identity) << std::endl;
//		std::cout << "Coupling with ground state: " << heom::state_baths_coupling(heom_config.baths_coupling(), heom::sites_to_states_mode_t::with_ground_state) << std::endl;
//		std::cout << "Coupling excited state absorption: " << heom::state_baths_coupling(heom_config.baths_coupling(), heom::sites_to_states_mode_t::excited_state_absorption) << std::endl;
//
//		std::cout << "Hamiltonian identity: " << heom::state_hamiltonian(heom_config.system_hamiltonian(), heom::sites_to_states_mode_t::identity) << std::endl;
//		std::cout << "Hamiltonian with ground state: " << heom::state_hamiltonian(heom_config.system_hamiltonian(), heom::sites_to_states_mode_t::with_ground_state) << std::endl;
//		std::cout << "Hamiltonian excited state absorption: " << heom::state_hamiltonian(heom_config.system_hamiltonian(), heom::sites_to_states_mode_t::excited_state_absorption) << std::endl;

		const auto& pathways = heom_config.spectra_pathways().get();

		// for progress estimate
		const size_t solver_runs = pathways.size() * heom_config.dipole_tensor_prefactors().size() * t_3_iterations;
		size_t solver_run = 0;

		// iterate over pathways
		for (size_t p = 0; p < pathways.size(); ++p)
		{
			DEBUG_ONLY( std::cout << "Pathway " << p << " is " << pathways.at(p) << std::endl; )
			const auto& pathway_spec = heom::spectra_pathway_to_specification.at(pathways[p]);
			const auto sts_mode = pathway_spec.sites_to_states_mode();

			heom::instance heom_instance(heom_config, sts_mode, complete_graph);
			heom_instance.set_hierarchy_top(); // NOTE: hierarchy top is initialised to zero with first element being complex_t(1.0,0.0) here

			// OpenCL range
			ocl::nd_range range{
				{}, // offset
				{1, static_cast<std::uint64_t>(heom_instance.matrices())}, // global size
				{1, 1} // local size
			};

			// create a solver from configuration and instance
			solver_t solver(ocl_config, range, heom_config, heom_instance);
			std::cout << "-------------------- OpenCL Runtime Configuration ----------" << std::endl;
			solver.write_ocl_runtime_config(std::cout);
			std::cout << "------------------------------------------------------------" << std::endl;

			for (size_t tensor_index = 0; tensor_index < heom_config.dipole_tensor_prefactors().size(); ++tensor_index) {
				for (int_t t_3_index = 0; t_3_index < t_3_iterations; ++t_3_index) {
					const int_t t_3 = t_3_index * steps_per_iteration;

					// re-init instance
					heom_instance.null_hierarchy();
					heom_instance.set_hierarchy_top();
					// re-init solver
					solver.reset();
					++solver_run; // for progress estimate

					// TODO: optimise: multiply top only on host-side (synchronise host/device!) => think through where possible
					auto get_dipole_matrix = [&](size_t spec_index) {
						return heom::get_dipole_matrix_for_pathway(pathway_spec, spec_index, heom_config.system_sites(), heom_config, tensor_index);
					};

					// lambda for generic dipole multiplication with plus/minus, from left/right
					auto dipole_mult = [&](size_t spec_index) {
						const auto dipole_matrix = get_dipole_matrix(spec_index);

						// multiple from left or right on hierarchy according to pathway specification
						if (pathway_spec.pathway()[spec_index].second ==
						    heom::pathway_dipole_side_t::left)
							solver.hierarchy_mmult_left(dipole_matrix.data());
						else // must be right
							solver.hierarchy_mmult_right(dipole_matrix.data());
					};

					// first dipole multiplication from specification
					dipole_mult(0);

					// propagate t_3 steps
					solver.step_forward(t_3);

					// second dipole multiplication from specification
					dipole_mult(1);

					// propagate delay steps
					solver.step_forward(heom_config.spectra_steps_t_delay());

					// third dipole multiplication from specification
					dipole_mult(2);

					// propagate additional t_1 steps and observe

					// setup observer with pre-scaled dipole matrix
					auto dipole_matrix = get_dipole_matrix(3);
					dipole_matrix.scale(heom::get_dipole_tensor_prefactor(pathway_spec, heom_config, tensor_index));
					heom::matrix_trace_observer<solver_t> trace_observer(complete_graph, solver, dipole_matrix.data()); // use last (fourth) polarization for trace, i.e. index 3

					// generate first observation with initial value
					observations[t_3_index][0] += trace_observer.observe_trace(0.0).trace();

					for (int_t t_1_index = 0; t_1_index < t_1_iterations; ++t_1_index) {
						// propagate
						solver.step_forward(steps_per_iteration);

						// after propagation, we are at:
						const auto current_step = (t_1_index + 1) * steps_per_iteration;
						const real_t current_time = current_step * heom_config.solver_step_size();

						// observe
						observations[t_3_index][t_1_index + 1] += trace_observer.observe_trace(current_time).trace();

						// update status
						if(!command_line.no_progress())
							heom::write_progress(current_step - 1, heom_config.spectra_steps_t_1(), solver_run, solver_runs, "Calculating two dimensional spectra: ", std::cout);
					} // t_1
				} // t_3
			} // tensor_index

			// write_complex_matrix(result_buffer_top, heom_instance.states(), std::cout);
			std::cout << "-------------------- Solver Runtime Summary ----------------" << std::endl;
			solver.write_runtime_summary(std::cout);
			std::cout << "------------------------------------------------------------" << std::endl;
		} // pathways

		// TODO: re-enable
		// average traces
//		for (auto& vec : observations)
//			for (auto& elem : vec)
//				elem = { elem.real() / num_dipole_matrices, elem.imag() / num_dipole_matrices };
//				//elem.avg(num_dipole_matrices);

		// write traces
		auto format = real_format;
		observation_file << "t_3" << default_delimiter << "t_1" << default_delimiter << "trace_real" << default_delimiter << "trace_imag" << std::endl;
		for (int_t t_3_index = 0; t_3_index < num_t_3_observations; ++t_3_index) {
			for (int_t t_1_index = 0; t_1_index < num_t_1_observations; ++t_1_index) {
				const auto& current_obs = observations[t_3_index][t_1_index];
				observation_file << format % (t_3_index * steps_per_iteration * heom_config.solver_step_size());
				observation_file << default_delimiter;
				observation_file << format % (t_1_index * steps_per_iteration * heom_config.solver_step_size());
				observation_file << default_delimiter;
				observation_file << format % current_obs.real();
				observation_file << default_delimiter;
				observation_file << format % current_obs.imag();
				observation_file << '\n';
			}
		}
		observation_file << std::flush; // tell the OS to write to disk

		std::cout << "-------------------- Memory Summary ------------------------" << std::endl;
//		std::cout << "local hierarchy buffer size:            "
//		          <<  boost::format("%11.2f MiB\n") % (heom_instance_thermal_state.size_hierarchy_byte() / 1024.0 / 1024.0);
//		std::cout << "local instance size:                    "
//		          <<  boost::format("%11.2f MiB\n") % (heom_instance_thermal_state.allocated_byte() / 1024.0 / 1024.0);
		std::cout << "max. heap via aligned::allocate(..):    "
		          << boost::format("%11.2f MiB") % (heom::memory::aligned::instance().allocated_byte_max() / 1024.0 / 1024.0)
		          << " ("
		          << boost::format("count: %6i") % heom::memory::aligned::instance().allocations_max()
		          << ')' << std::endl;
//		std::cout << "max. OpenCL buffer allocations:         "
//		          <<  boost::format("%11.2f MiB") % (bicgstab.ocl_helper().allocated_byte_max() / 1024.0 / 1024.0)
//		          << " ("
//		          << boost::format("count: %6i") % bicgstab.ocl_helper().allocations_max()
//		          << ')' << std::endl;
//		std::cout << "max. OpenCL buffer allocations:         "
//		          <<  boost::format("%11.2f MiB") % (solver.ocl_helper().allocated_byte_max() / 1024.0 / 1024.0)
//		          << " ("
//		          << boost::format("count: %6i") % solver.ocl_helper().allocations_max()
//		          << ')' << std::endl;
		std::cout << "------------------------------------------------------------" << std::endl;

	} catch (...) {
		eptr = std::current_exception();
	}
	int ret = heom::handle_main_exception(eptr);

	// print application runtime
	std::cout << "time in main():\t" << boost::format("%11.2f") % std::chrono::duration_cast<bmt::seconds>(bmt::duration(app_timer.elapsed())).count() << " s" << std::endl;

	return ret;
}


#include <iostream>
#include <boost/numeric/conversion/cast.hpp>

int main()
{
// this is fine
    const auto s1 = boost::numeric_cast<unsigned short>(42);
    std::cout << "boost::numeric_cast<unsigned short>(42) = " << s1 << "\n";

// this will throw
    try{
        const auto s2 = boost::numeric_cast<unsigned>(-1);
        (void)s2;
    }
    catch(const boost::numeric::bad_numeric_cast& e){
        std::cout << e.what() << "\n";
    }

// so will this
    try{
        const auto s3 = boost::numeric_cast<unsigned short>(100000);
        (void)s3;
    }
    catch(const boost::numeric::positive_overflow& e){
        std::cout << e.what() << "\n";
    }

// exception types:
    // boost::numeric::positive_overflow;
    // boost::numeric::negative_overflow;
    // boost::numeric::bad_numeric_cast;
}