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

#include "theia/sfm/global_pose_estimation/linear_position_estimator.h"

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <algorithm>
#include <ceres/rotation.h>
#include <glog/logging.h>
#include <memory>
#include <unordered_map>
#include <unordered_set>
#include <vector>

#include "spectra/include/SymEigsShiftSolver.h"

#include "theia/math/graph/triplet_extractor.h"
#include "theia/math/matrix/spectra_linear_operator.h"
#include "theia/sfm/find_common_tracks_in_views.h"
#include "theia/sfm/global_pose_estimation/compute_triplet_baseline_ratios.h"
#include "theia/sfm/reconstruction.h"
#include "theia/sfm/types.h"
#include "theia/sfm/view_triplet.h"
#include "theia/util/map_util.h"
#include "theia/util/threadpool.h"

namespace theia {

using Eigen::Matrix3d;
using Eigen::Vector3d;

namespace {

std::vector<ViewIdTriplet> GetLargetConnectedTripletGraph(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs) {
  static const int kLargestCCIndex = 0;

  // Get a list of all edges in the view graph.
  std::unordered_set<ViewIdPair> view_id_pairs;
  view_id_pairs.reserve(view_pairs.size());
  for (const auto& view_pair : view_pairs) {
    view_id_pairs.insert(view_pair.first);
  }

  // Extract connected triplets.
  TripletExtractor<ViewId> extractor;
  std::vector<std::vector<ViewIdTriplet> > triplets;
  CHECK(extractor.ExtractTriplets(view_id_pairs, &triplets));
  CHECK_GT(triplets.size(), 0);
  return triplets[kLargestCCIndex];
}

// Adds the constraint from the triplet to the symmetric matrix. Our standard
// constraint matrix A is a 3M x 3N matrix with M triplet constraints and N
// cameras. We seek to construct A^t * A directly. For each triplet constraint
// in our matrix A (i.e. a 3-row block), we can compute the corresponding
// entries in A^t * A with the following summation:
//
//   A^t * A += Row(i)^t * Row(i)
//
// for each triplet constraint i.
void AddTripletConstraintToSymmetricMatrix(
    const std::vector<Matrix3d>& constraints,
    const std::vector<int>& view_indices,
    std::unordered_map<std::pair<int, int>, double>* sparse_matrix_entries) {
  // Construct Row(i)^t * Row(i). If we denote the row as a block matrix:
  //
  //   Row(i) = [A | B | C]
  //
  // then we have:
  //
  //   Row(i)^t * Row(i) = [A | B | C]^t * [A | B | C]
  //                     = [ A^t * A  |  A^t * B  |  A^t * C]
  //                       [ B^t * A  |  B^t * B  |  B^t * C]
  //                       [ C^t * A  |  C^t * B  |  C^t * C]
  //
  // Since A^t * A is symmetric, we only store the upper triangular portion.
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      // Skip any block entries that correspond to the lower triangular portion
      // of the matrix.
      if (view_indices[i] > view_indices[j]) {
        continue;
      }

      // Compute the A^t * B, etc. matrix.
      const Eigen::Matrix3d symmetric_constraint =
          constraints[i].transpose() * constraints[j];

      // Add to the 3x3 block corresponding to (i, j)
      for (int r = 0; r < 3; r++) {
        for (int c = 0; c < 3; c++) {
          const std::pair<int, int> row_col(view_indices[i] + r,
                                            view_indices[j] + c);
          (*sparse_matrix_entries)[row_col] += symmetric_constraint(r, c);
        }
      }
    }
  }
}

inline Matrix3d AngleAxisToRotationMatrix(const Vector3d angle_axis) {
  const double angle = angle_axis.norm();
  const Eigen::AngleAxisd rotation_aa(angle, angle_axis / angle);
  return rotation_aa.toRotationMatrix();
}

// Returns true if the vector R1 * (c2 - c1) is in the same direction as t_12.
bool VectorsAreSameDirection(const Vector3d& position1,
                             const Vector3d& position2,
                             const Vector3d& rotation1,
                             const Vector3d& relative_position12) {
  const Vector3d global_relative_position =
      (position2 - position1).normalized();
  Vector3d rotated_relative_position;
  ceres::AngleAxisRotatePoint(rotation1.data(),
                              global_relative_position.data(),
                              rotated_relative_position.data());
  return rotated_relative_position.dot(relative_position12) > 0;
}

}  // namespace

LinearPositionEstimator::LinearPositionEstimator(
    const Options& options, const Reconstruction& reconstruction)
    : options_(options), reconstruction_(reconstruction) {
  CHECK_GT(options.num_threads, 0);
}

bool LinearPositionEstimator::EstimatePositions(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    const std::unordered_map<ViewId, Vector3d>& orientations,
    std::unordered_map<ViewId, Vector3d>* positions) {
  CHECK_NOTNULL(positions)->clear();
  view_pairs_ = &view_pairs;
  orientations_ = &orientations;

  // Extract triplets from the view pairs. As of now, we only consider the
  // largest connected triplet in the viewing graph.
  VLOG(2) << "Extracting triplets from the viewing graph.";
  triplets_ = GetLargetConnectedTripletGraph(view_pairs);

  VLOG(2) << "Determining baseline ratios within each triplet...";
  // Baselines where (x, y, z) corresponds to the baseline of the first,
  // second, and third view pair in the triplet.
  std::unique_ptr<ThreadPool> pool(new ThreadPool(options_.num_threads));
  baselines_.resize(triplets_.size());
  for (size_t i = 0; i < triplets_.size(); i++) {
    AddTripletConstraint(triplets_[i]);
    pool->Add(&LinearPositionEstimator::ComputeBaselineRatioForTriplet,
              this,
              triplets_[i],
              &baselines_[i]);
  }
  pool.reset(nullptr);

  VLOG(2) << "Building the constraint matrix...";
  // Create the linear system based on triplet constraints.
  Eigen::SparseMatrix<double> constraint_matrix;
  CreateLinearSystem(&constraint_matrix);

  // Solve for positions by examining the smallest eigenvalues. Since we have
  // set one position constant at the origin, we only need to solve for the
  // eigenvector corresponding to the smallest eigenvalue. This can be done
  // efficiently with inverse power iterations.
  VLOG(2) << "Solving for positions from the sparse eigenvalue problem...";
  SparseSymShiftSolveLLT op(constraint_matrix);
  Spectra::
      SymEigsShiftSolver<double, Spectra::LARGEST_MAGN, SparseSymShiftSolveLLT>
          eigs(&op, 1, 6, 0.0);
  eigs.init();
  eigs.compute();

  // Compute with power iterations.
  const Eigen::VectorXd solution = eigs.eigenvectors().col(0);

  // Add the solutions to the output. Set the position with an index of -1 to
  // be at the origin.
  for (const auto& view_index : linear_system_index_) {
    if (view_index.second < 0) {
      (*positions)[view_index.first].setZero();
    } else {
      (*positions)[view_index.first] =
          solution.segment<3>(view_index.second * 3);
    }
  }

  // Flip the sign of the positions if necessary.
  FlipSignOfPositionsIfNecessary(positions);

  return true;
}

// An alternative interface is to instead add triplets one by one to linear
// estimator. This allows for adding redundant observations of triplets, which
// may be useful if there are multiple estimates of the data.
void LinearPositionEstimator::AddTripletConstraint(
    const ViewIdTriplet& view_triplet) {
  num_triplets_for_view_[std::get<0>(view_triplet)] += 1;
  num_triplets_for_view_[std::get<1>(view_triplet)] += 1;
  num_triplets_for_view_[std::get<2>(view_triplet)] += 1;

  // Determine the order of the views in the linear system. We subtract 1 from
  // the linear system index so that the first position added to the system
  // will be set constant (index of -1 is intentionally not evaluated later).
  InsertIfNotPresent(&linear_system_index_,
                     std::get<0>(view_triplet),
                     linear_system_index_.size() - 1);
  InsertIfNotPresent(&linear_system_index_,
                     std::get<1>(view_triplet),
                     linear_system_index_.size() - 1);
  InsertIfNotPresent(&linear_system_index_,
                     std::get<2>(view_triplet),
                     linear_system_index_.size() - 1);
}

void LinearPositionEstimator::ComputeBaselineRatioForTriplet(
    const ViewIdTriplet& triplet, Vector3d* baseline) {
  baseline->setZero();

  const View& view1 = *reconstruction_.View(std::get<0>(triplet));
  const View& view2 = *reconstruction_.View(std::get<1>(triplet));
  const View& view3 = *reconstruction_.View(std::get<2>(triplet));

  // Find common tracks.
  const std::vector<ViewId> triplet_view_ids = {
      std::get<0>(triplet), std::get<1>(triplet), std::get<2>(triplet)};
  const std::vector<TrackId>& common_tracks =
      FindCommonTracksInViews(reconstruction_, triplet_view_ids);

  // Normalize all features.
  std::vector<Feature> feature1, feature2, feature3;
  feature1.reserve(common_tracks.size());
  feature2.reserve(common_tracks.size());
  feature3.reserve(common_tracks.size());
  for (const TrackId track_id : common_tracks) {
    feature1.emplace_back(GetNormalizedFeature(view1, track_id).point_);
    feature2.emplace_back(GetNormalizedFeature(view2, track_id).point_);
    feature3.emplace_back(GetNormalizedFeature(view3, track_id).point_);
  }

  // Get the baseline ratios.
  ViewTriplet view_triplet;
  view_triplet.view_ids[0] = std::get<0>(triplet);
  view_triplet.view_ids[1] = std::get<1>(triplet);
  view_triplet.view_ids[2] = std::get<2>(triplet);
  view_triplet.info_one_two = FindOrDieNoPrint(
      *view_pairs_,
      ViewIdPair(view_triplet.view_ids[0], view_triplet.view_ids[1]));
  view_triplet.info_one_three = FindOrDieNoPrint(
      *view_pairs_,
      ViewIdPair(view_triplet.view_ids[0], view_triplet.view_ids[2]));
  view_triplet.info_two_three = FindOrDieNoPrint(
      *view_pairs_,
      ViewIdPair(view_triplet.view_ids[1], view_triplet.view_ids[2]));

  ComputeTripletBaselineRatios(
      view_triplet, feature1, feature2, feature3, baseline);
}

// Sets up the linear system with the constraints that each triplet adds.
void LinearPositionEstimator::CreateLinearSystem(
    Eigen::SparseMatrix<double>* constraint_matrix) {
  const int num_views = num_triplets_for_view_.size();

  std::unordered_map<std::pair<int, int>, double> sparse_matrix_entries;
  sparse_matrix_entries.reserve(27 * num_triplets_for_view_.size());
  for (int i = 0; i < triplets_.size(); i++) {
    const ViewId& view_id1 = std::get<0>(triplets_[i]);
    const ViewId& view_id2 = std::get<1>(triplets_[i]);
    const ViewId& view_id3 = std::get<2>(triplets_[i]);
    AddTripletConstraintToSparseMatrix(
        view_id1, view_id2, view_id3, baselines_[i], &sparse_matrix_entries);
  }

  // Set the sparse matrix from the container of the accumulated entries.
  std::vector<Eigen::Triplet<double> > triplet_list;
  triplet_list.reserve(sparse_matrix_entries.size());
  for (const auto& sparse_matrix_entry : sparse_matrix_entries) {
    // Skip this entry if the indices are invalid. This only occurs when we
    // encounter a constraint with the constant camera (which has a view index
    // of -1).
    if (sparse_matrix_entry.first.first < 0 ||
        sparse_matrix_entry.first.second < 0) {
      continue;
    }
    triplet_list.emplace_back(sparse_matrix_entry.first.first,
                              sparse_matrix_entry.first.second,
                              sparse_matrix_entry.second);
  }

  // We construct the constraint matrix A^t * A directly, which is an
  // N - 1 x N - 1 matrix where N is the number of cameras (and 3 entries per
  // camera, corresponding to the camera position entries).
  constraint_matrix->resize((num_views - 1) * 3, (num_views - 1) * 3);
  constraint_matrix->setFromTriplets(triplet_list.begin(), triplet_list.end());
}

void LinearPositionEstimator::ComputeRotatedRelativeTranslationRotations(
    const ViewId view_id0,
    const ViewId view_id1,
    const ViewId view_id2,
    Eigen::Matrix3d* r012,
    Eigen::Matrix3d* r201,
    Eigen::Matrix3d* r120) {
  // Relative camera positions.
  const Eigen::Vector3d& orientation0_aa =
      FindOrDieNoPrint(*orientations_, view_id0);
  const Eigen::Vector3d& orientation1_aa =
      FindOrDieNoPrint(*orientations_, view_id1);
  const Matrix3d orientation0 = AngleAxisToRotationMatrix(orientation0_aa);
  const Matrix3d orientation1 = AngleAxisToRotationMatrix(orientation1_aa);
  const Vector3d t01 =
      -orientation0.transpose() *
      FindOrDieNoPrint(*view_pairs_, ViewIdPair(view_id0, view_id1)).position_2;
  const Vector3d t02 =
      -orientation0.transpose() *
      FindOrDieNoPrint(*view_pairs_, ViewIdPair(view_id0, view_id2)).position_2;
  const Vector3d t12 =
      -orientation1.transpose() *
      FindOrDieNoPrint(*view_pairs_, ViewIdPair(view_id1, view_id2)).position_2;

  // Rotations between the translation vectors.
  *r012 = Eigen::Quaterniond::FromTwoVectors(t12, -t01).toRotationMatrix();
  *r201 = Eigen::Quaterniond::FromTwoVectors(t01, t02).toRotationMatrix();
  *r120 = Eigen::Quaterniond::FromTwoVectors(-t02, -t12).toRotationMatrix();
}

// Adds a triplet constraint to the linear system. The weight of the constraint
// (w), the global orientations, baseline (ratios), and view triplet information
// are needed to form the constraint.
void LinearPositionEstimator::AddTripletConstraintToSparseMatrix(
    const ViewId view_id0,
    const ViewId view_id1,
    const ViewId view_id2,
    const Eigen::Vector3d& baselines,
    std::unordered_map<std::pair<int, int>, double>* sparse_matrix_entries) {
  // Weight each term by the inverse of the # of triplet that the nodes
  // participate in.
  const double w =
      1.0 / std::sqrt(std::min({num_triplets_for_view_[view_id0],
                                num_triplets_for_view_[view_id1],
                                num_triplets_for_view_[view_id2]}));

  // Get the index of each camera in the sparse matrix.
  const std::vector<int> view_indices = {
      static_cast<int>(3 * FindOrDie(linear_system_index_, view_id0)),
      static_cast<int>(3 * FindOrDie(linear_system_index_, view_id1)),
      static_cast<int>(3 * FindOrDie(linear_system_index_, view_id2))};

  // Compute the rotations between relative translations.
  Eigen::Matrix3d r012, r201, r120;
  ComputeRotatedRelativeTranslationRotations(
      view_id0, view_id1, view_id2, &r012, &r201, &r120);

  // Baselines ratios.
  const double s_012 = baselines[0] / baselines[2];
  const double s_201 = baselines[1] / baselines[0];
  const double s_120 = baselines[2] / baselines[1];

  // Assume that t01 is perfect and solve for c2.
  std::vector<Eigen::Matrix3d> constraints(3);
  constraints[0] =
      (-s_201 * r201 + r012.transpose() / s_012 + Matrix3d::Identity()) * w;
  constraints[1] =
      (s_201 * r201 - r012.transpose() / s_012 + Matrix3d::Identity()) * w;
  constraints[2] = -2.0 * w * Matrix3d::Identity();
  AddTripletConstraintToSymmetricMatrix(
      constraints, view_indices, sparse_matrix_entries);

  // Assume t02 is perfect and solve for c1.
  constraints[0] =
      (-r201.transpose() / s_201 + s_120 * r120 + Matrix3d::Identity()) * w;
  constraints[1] = -2.0 * w * Matrix3d::Identity();
  constraints[2] =
      (r201.transpose() / s_201 - s_120 * r120 + Matrix3d::Identity()) * w;
  AddTripletConstraintToSymmetricMatrix(
      constraints, view_indices, sparse_matrix_entries);

  // Assume t12 is perfect and solve for c0.
  constraints[0] = -2.0 * w * Matrix3d::Identity();
  constraints[1] =
      (-s_012 * r012 + r120.transpose() / s_120 + Matrix3d::Identity()) * w;
  constraints[2] =
      (s_012 * r012 - r120.transpose() / s_120 + Matrix3d::Identity()) * w;
  AddTripletConstraintToSymmetricMatrix(
      constraints, view_indices, sparse_matrix_entries);
}

Feature LinearPositionEstimator::GetNormalizedFeature(const View& view,
                                                      const TrackId track_id) {
  Feature feature = *view.GetFeature(track_id);
  const Camera& camera = view.Camera();
  Eigen::Vector3d ray = camera.PixelToNormalizedCoordinates(feature.point_);
  Feature normalized_Feature(ray.hnormalized());
  // todo normalized covariance?
  return normalized_Feature;
}

void LinearPositionEstimator::FlipSignOfPositionsIfNecessary(
    std::unordered_map<ViewId, Vector3d>* positions) {
  // If this value is below zero, then we should flip the sign.
  int correct_sign_votes = 0;
  for (const auto& view_pair : *view_pairs_) {
    // Only count the votes for edges where both positions were successfully
    // estimated.
    const Vector3d* position1 = FindOrNull(*positions, view_pair.first.first);
    const Vector3d* position2 = FindOrNull(*positions, view_pair.first.second);
    if (position1 == nullptr || position2 == nullptr) {
      continue;
    }

    // Check the relative translation of views 1 and 2 in the triplet.
    if (VectorsAreSameDirection(
            *position1,
            *position2,
            FindOrDieNoPrint(*orientations_, view_pair.first.first),
            view_pair.second.position_2)) {
      correct_sign_votes += 1;
    } else {
      correct_sign_votes -= 1;
    }
  }

  // If the sign of the votes is below zero, we must flip the sign of all
  // position estimates.
  if (correct_sign_votes < 0) {
    const int num_correct_votes =
        (view_pairs_->size() + correct_sign_votes) / 2;
    VLOG(2) << "Sign of the positions was incorrect: " << num_correct_votes
            << " of " << view_pairs_->size()
            << " relative translations had the correct sign. "
               "Flipping the sign of the camera positions.";
    for (auto& position : *positions) {
      position.second *= -1.0;
    }
  }
}

std::unordered_map<ViewId, Eigen::Vector3d>
LinearPositionEstimator::EstimatePositionsWrapper(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    const std::unordered_map<ViewId, Eigen::Vector3d>& orientation) {
  std::unordered_map<ViewId, Eigen::Vector3d> positions;
  EstimatePositions(view_pairs, orientation, &positions);
  return positions;
}

}  // namespace theia


/******************************************************************************
 * Copyright 2017 Baidu Robotic Vision Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
#ifndef __DEVELOPMENT_DEBUG_MODE__
#define __FEATURE_UTILS_NO_DEBUG__
#endif
#include "feature_utils.h"
#include "patch_score.h"

#include "cameras/PinholeCamera.hpp"
#include "cameras/RadialTangentialDistortion.hpp"
#include "cameras/RadialTangentialDistortion8.hpp"

#include <boost/lexical_cast.hpp>
#ifndef __FEATURE_UTILS_NO_DEBUG__
#include <opencv2/highgui.hpp>
#endif

// define this MACROS to enable profing and verifying align2D_NEON
#undef _ALIGN2D_TIMING_AND_VERIFYING
#ifdef _ALIGN2D_TIMING_AND_VERIFYING
#include <vio/timing/ProfilingTimer.hpp>
using vio::timing::MicrosecondStopwatch;
MicrosecondStopwatch timer_align2d("align2d", 0);
MicrosecondStopwatch timer_align2d_neon("align2d neon", 0);
float align2d_total_time = 0.f;
float align2d_total_time_neon = 0.f;
#endif

namespace XP {

using Eigen::Vector2i;
using Eigen::Vector2f;
using Eigen::Vector3f;
using Eigen::Matrix2f;
using Eigen::Matrix3f;
using Eigen::Matrix4f;

// The triangulated point at the current frame coordinate can be represented as:
// R_cur_ref * f_ref * d_ref + t_cur_ref =  f_cur * d_cur,
// where d_ref and d_cur are scales, respetively
// We can organize the equation into a linear system to solve d in the form of
// Ax = b:
// [R_cur_ref * f_ref  f_cur] * [d_ref; - d_cur] = - t_cur_ref
//   A = [R_cur_ref * f_ref f_cur],  3 x 2
//   x = [d_ref; -d_cur],  2 x 1
//   b = - t_cur_ref,  3 x 1
// x = (AtA)^-1 * At * b
//
// The returned depth is based on the reference frame
bool depthFromTriangulation(const Matrix3f& R_cur_ref,
                            const Vector3f& t_cur_ref, const Vector3f& f_ref,
                            const Vector3f& f_cur, float* depth) {
  Eigen::Matrix<float, 3, 2> A;
  A << R_cur_ref * f_ref, f_cur;
  const Matrix2f AtA = A.transpose() * A;
  if (AtA.determinant() < 1e-6) {
    // TODO(mingyu): figure the right threshold for float
    *depth = 1000;  // a very far point
    return false;
  }
  const Vector2f depth2 = -AtA.inverse() * A.transpose() * t_cur_ref;
  *depth = fabs(depth2[0]);
  return true;
}

void DirectMatcher::createPatchFromPatchWithBorder() {
  uint8_t* ref_patch_ptr = patch_;
  for (int y = 1; y < patch_size_ + 1; ++y, ref_patch_ptr += patch_size_) {
    uint8_t* ref_patch_border_ptr =
        patch_with_border_ + y * (patch_size_ + 2) + 1;
    memcpy(ref_patch_ptr, ref_patch_border_ptr, patch_size_);
    /*
    for (int x = 0; x < patch_size_; ++x) {
      ref_patch_ptr[x] = ref_patch_border_ptr[x];
    }
    */
  }
}

bool DirectMatcher::findMatchDirect(
    const vio::cameras::CameraBase& cam_ref,
    const vio::cameras::CameraBase& cam_cur, const Vector2f& px_ref,
    const Vector3f& f_ref, const Matrix3f& R_cur_ref, const Vector3f& t_cur_ref,
    const int level_ref, const float depth_ref,
    const std::vector<cv::Mat>& pyrs_ref, const std::vector<cv::Mat>& pyrs_cur,
    const bool edgelet_feature, Vector2f* px_cur) {
  CHECK_NEAR(f_ref[2], 1.f, 1e-6);
  CHECK_EQ(pyrs_ref.size(), pyrs_cur.size());

  // TODO(mingyu): check return boolean of getWarpMatrixAffine
  // warp affine
  warp::getWarpMatrixAffine(cam_ref, cam_cur, px_ref, f_ref, depth_ref,
                            R_cur_ref, t_cur_ref, level_ref, &A_cur_ref_);
  const int max_level = pyrs_ref.size() - 1;
  const int search_level = warp::getBestSearchLevel(A_cur_ref_, max_level);

  // TODO(mingyu): check return boolean of warpAffine
  warp::warpAffine(A_cur_ref_, pyrs_ref[level_ref], px_ref, level_ref,
                   search_level, halfpatch_size_ + 1, patch_with_border_);
  createPatchFromPatchWithBorder();

  // px_cur should be set
  Vector2f px_scaled = *px_cur / (1 << search_level);

  bool success = false;
  if (edgelet_feature) {
    // TODO(mingyu): currently not used until we further refine the feature type
    //               with gradient direction.
    //               Fast features do contain edgelet features.
    /*
    Vector2f dir_cur(A_cur_ref_ * ref_ftr_->grad);
    dir_cur.normalize();
    success = align::align1D(pyrs_cur[search_level],
                             dir_cur,
                             patch_with_border_,
                             patch_,
                             options_.max_iter,
                             &px_scaled,
                             &h_inv_);
    */
    LOG(ERROR) << "findMatchDirect for edgelet feature is NOT rimplemented yet";
    success = false;
  } else {
#ifndef _ALIGN2D_TIMING_AND_VERIFYING
#ifndef __ARM_NEON__
    success = align::align2D(pyrs_cur[search_level], patch_with_border_, patch_,
                             options_.max_iter, &px_scaled);
#else
    success = align::align2D_NEON(pyrs_cur[search_level], patch_with_border_,
                                  patch_, options_.max_iter, &px_scaled);
#endif  // __ARM_NEON__
#else
// timing and verifying code.
#ifdef __ARM_NEON__
    Vector2f px_scaled_neon = px_scaled;
#endif  // __ARM_NEON__
    timer_align2d.start();
    success = align::align2D(pyrs_cur[search_level], patch_with_border_, patch_,
                             options_.max_iter, &px_scaled);
    timer_align2d.stop();
    align2d_total_time = timer_align2d.stop();

#ifdef __ARM_NEON__
    timer_align2d_neon.start();
    bool success_neon =
        align::align2D_NEON(pyrs_cur[search_level], patch_with_border_, patch_,
                            options_.max_iter, &px_scaled_neon);
    timer_align2d_neon.stop();
    align2d_total_time_neon = timer_align2d_neon.elapse_ms();
    CHECK_EQ(success, success_neon);
    if (success_neon) {
      CHECK_NEAR(px_scaled[0], px_scaled_neon[0], 0.05);
      CHECK_NEAR(px_scaled[1], px_scaled_neon[1], 0.05);
      std::cout << "[NEON : NON-NEON --->"
                << "[" << align2d_total_time_neon << " : " << align2d_total_time
                << "]" << std::endl;
    }
#endif  // __ARM_NEON__
#endif  // _ALIGN2D_TIMING_AND_VERIFYING
  }
  *px_cur = px_scaled * (1 << search_level);
  return success;
}

bool DirectMatcher::findEpipolarMatchDirect(
    const vio::cameras::CameraBase& cam_ref,
    const vio::cameras::CameraBase& cam_cur, const Vector2f& px_ref,
    const Vector3f& f_ref, const Matrix3f& R_cur_ref, const Vector3f& t_cur_ref,
    const int level_ref, const float d_estimate, const float d_min,
    const float d_max, const std::vector<cv::Mat>& pyrs_ref,
    const std::vector<cv::Mat>& pyrs_cur, const cv::Mat_<uchar>& mask_cur,
    const bool edgelet_feature, Vector2f* px_cur, float* depth, int* level_cur,
    cv::Mat* dbg_cur) {
  CHECK_NEAR(f_ref[2], 1.f, 1e-6);
  CHECK_EQ(pyrs_ref.size(), pyrs_cur.size());

  // Compute start and end of epipolar line in old_kf for match search, on unit
  // plane!
  // i.e., A & B are the first two elements of unit rays.
  // We will search from far to near
  Vector3f ray_A, ray_B;
  Vector2f px_A, px_B;
  ray_B = R_cur_ref * (f_ref * d_max) + t_cur_ref;  // far
  ray_B /= ray_B(2);
  if (vio::cameras::CameraBase::ProjectionStatus::Successful !=
      cam_cur.project(ray_B, &px_B)) {
#ifndef __FEATURE_UTILS_NO_DEBUG__
    VLOG(2) << "ray_A (far) cannot be reprojected in cam_cur";
#endif
    return false;
  }

  bool invalid_ray_A = true;
  for (float d = d_min; d < d_estimate; d *= 10) {
    ray_A = R_cur_ref * (f_ref * d) + t_cur_ref;  // near
    ray_A /= ray_A(2);
    if (vio::cameras::CameraBase::ProjectionStatus::Successful ==
        cam_cur.project(ray_A, &px_A)) {
      invalid_ray_A = false;
      break;
    }
  }
  if (invalid_ray_A) {
#ifndef __FEATURE_UTILS_NO_DEBUG__
    VLOG(2) << "ray_B (near) cannot be reprojected in cam_cur "
            << " d_min = " << d_min << " d_estimate = " << d_estimate;
#endif
    return false;
  }

  // Compute warp affine matrix
  if (!warp::getWarpMatrixAffine(cam_ref, cam_cur, px_ref, f_ref, d_estimate,
                                 R_cur_ref, t_cur_ref, level_ref,
                                 &A_cur_ref_)) {
    LOG(WARNING) << "warp::getWarpMatrixAffine fails";
    return false;
  }
#ifndef __FEATURE_UTILS_NO_DEBUG__
  VLOG(2) << "A_cur_ref_ =\n" << A_cur_ref_;
#endif

  const int max_level = pyrs_ref.size() - 1;
  const int search_level = warp::getBestSearchLevel(A_cur_ref_, max_level);
  epi_dir_ = ray_A.head<2>() - ray_B.head<2>();  // far to near, B to A
  epi_length_ = (px_A - px_B).norm() / (1 << search_level);

  // feature pre-selection
  if (edgelet_feature) {
    /*
    const Vector2f grad_cur = (A_cur_ref_ * ref_ftr.grad).normalized();
    const float cosangle = fabs(grad_cur.dot(epi_dir_.normalized()));
    if (cosangle < options_.epi_search_edgelet_max_angle) {
      return false;
    }
    */
    LOG(ERROR)
        << "findEpipolarMatchDirect for edgelet feature is NOT implemented yet";
    return false;
  }

  if (!warp::warpAffine(A_cur_ref_, pyrs_ref[level_ref], px_ref, level_ref,
                        search_level, halfpatch_size_ + 1,
                        patch_with_border_)) {
    return false;
  }
  createPatchFromPatchWithBorder();
#ifndef __FEATURE_UTILS_NO_DEBUG__
  VLOG(2) << " search_level = " << search_level
          << " epi_length_ = " << epi_length_;
#endif
  if (epi_length_ < options_.max_epi_length_optim) {
    // The epipolar search line is short enough (< 2 pixels)
    // to perform direct alignment
    *px_cur = (px_A + px_B) * 0.5f;
    Vector2f px_scaled(*px_cur / (1 << search_level));
    bool success;
    if (options_.align_1d) {
      Vector2f direction = (px_A - px_B).normalized();
      success =
          align::align1D(pyrs_cur[search_level], direction, patch_with_border_,
                         patch_, options_.max_iter, &px_scaled, &h_inv_);
    } else {
#ifndef _ALIGN2D_TIMING_AND_VERIFYING
#ifndef __ARM_NEON__
      success = align::align2D(pyrs_cur[search_level], patch_with_border_,
                               patch_, options_.max_iter, &px_scaled);
#else
      success = align::align2D_NEON(pyrs_cur[search_level], patch_with_border_,
                                    patch_, options_.max_iter, &px_scaled);
#endif  // __ARM_NEON__
#else
// verifying and timing code
#ifdef __ARM_NEON__
      Vector2f px_scaled_neon = px_scaled;
#endif  // __ARM_NEON__
      timer_align2d.start();
      success = align::align2D(pyrs_cur[search_level], patch_with_border_,
                               patch_, options_.max_iter, &px_scaled);
      timer_align2d.stop();
      align2d_total_time = timer_align2d.elapse_ms();
#ifdef __ARM_NEON__
      timer_align2d_neon.start();
      bool success_neon =
          align::align2D_NEON(pyrs_cur[search_level], patch_with_border_,
                              patch_, options_.max_iter, &px_scaled_neon);
      timer_align2d_neon.stop();
      align2d_total_time_neon = timer_align2d_neon.elapse_ms();
      CHECK_EQ(success, success_neon);
      if (success_neon) {
        CHECK_NEAR(px_scaled[0], px_scaled_neon[0], 0.05);
        CHECK_NEAR(px_scaled[1], px_scaled_neon[1], 0.05);
        std::cout << "[NEON : NON-NEON --->"
                  << "[" << align2d_total_time_neon << " : "
                  << align2d_total_time << "]" << std::endl;
      }
#endif  // __ARM_NEON__
#endif  // _ALIGN2D_TIMING_AND_VERIFYING
    }

    if (success) {
      *px_cur = px_scaled * (1 << search_level);
      Vector3f f_cur;
      if (cam_cur.backProject(*px_cur, &f_cur)) {
        CHECK_NEAR(f_cur[2], 1.f, 1e-6);
        if (!depthFromTriangulation(R_cur_ref, t_cur_ref, f_ref, f_cur,
                                    depth)) {
          LOG(WARNING) << "depthFromTriangulation fails, set depth to d_max";
          *depth = d_max;
        }
      }
    }

    if (dbg_cur != nullptr) {
      if (success) {
        // green: subpix alignment is good
        cv::circle(*dbg_cur, cv::Point2f((*px_cur)(0), (*px_cur)(1)), 2,
                   cv::Scalar(0, 255, 0));
      } else {
        // red: subpix alignment fails
        cv::circle(*dbg_cur, cv::Point2f((*px_cur)(0), (*px_cur)(1)), 2,
                   cv::Scalar(0, 0, 255));
      }
    }
    return success;
  }

  // Determine the steps to Search along the epipolar line
  // [NOTE] The epipolar line can be curvy, so we slightly increase it
  //        to roughly have one step per pixel (heuristically).
  size_t n_steps = epi_length_ / 0.7;
  Vector3f step;
  step << epi_dir_ / n_steps, 0;
  if (n_steps > options_.max_epi_search_steps) {
    LOG(ERROR) << "Skip epipolar search: evaluations = " << n_steps
               << "epi length (px) = " << epi_length_;
    return false;
  }

  // Search along the epipolar line (on unit plane) with patch matching
  // for matching, precompute sum and sum2 of warped reference patch
  // [heuristic] The ssd from patch mean difference can be up to 50% of the
  // resulting ssd.
  //  ssd = zmssd + N * (a_bar - b_bar)^2
  typedef patch_score::ZMSSD<halfpatch_size_> PatchScore;
  PatchScore patch_score(patch_);
  int zmssd_best = PatchScore::threshold();
  int ssd_corr = PatchScore::threshold() * 2;
  Vector3f ray_best;
  Vector3f ray = ray_B;
  Eigen::Vector2i last_checked_pxi(0, 0);
  const int search_img_rows = pyrs_cur[search_level].rows;
  const int search_img_cols = pyrs_cur[search_level].cols;
  ++n_steps;
  for (size_t i = 0; i < n_steps; ++i, ray += step) {
    Vector2f px;
    if (vio::cameras::CameraBase::ProjectionStatus::Successful !=
        cam_cur.project(ray, &px)) {
      // We have already checked the valid projection of starting and ending
      // rays.  However,
      // under very rare circumstance, cam_cur.project may still fail:
      // close to zero denominator for radial tangential 8 distortion:
      // 1 + k4 * r^2 + k5 * r^4 + k6 * r^6 < 1e-6
      continue;
    }
    Vector2i pxi(px[0] / (1 << search_level) + 0.5,
                 px[1] / (1 << search_level) + 0.5);  // round to closest int
    if (pxi == last_checked_pxi) {
      continue;
    }
    last_checked_pxi = pxi;

    // check if the patch is full within the new frame
    if (pxi[0] >= halfpatch_size_ &&
        pxi[0] < search_img_cols - halfpatch_size_ &&
        pxi[1] >= halfpatch_size_ &&
        pxi[1] < search_img_rows - halfpatch_size_ &&
        mask_cur(pxi(1), pxi(0)) > 0) {
      // TODO(mingyu): Interpolation instead?
      uint8_t* cur_patch_ptr = pyrs_cur[search_level].data +
                               (pxi[1] - halfpatch_size_) * search_img_cols +
                               (pxi[0] - halfpatch_size_);
      int ssd, zmssd;
      patch_score.computeScore(cur_patch_ptr, search_img_cols, &zmssd, &ssd);
      if (zmssd < zmssd_best) {
        // We store the best zmssd and its corresponding ssd score.  Usually,
        // zmssd and ssd have good correlation if the *matching* is reasonable.
        zmssd_best = zmssd;
        ssd_corr = ssd;
        ray_best = ray;
      }
#ifndef __FEATURE_UTILS_NO_DEBUG__
      VLOG(2) << "search pxi=[" << pxi[0] << ", " << pxi[1]
              << "] zmssd = " << zmssd << " ssd = " << ssd;
#endif
      if (dbg_cur != nullptr) {
        dbg_cur->at<cv::Vec3b>(pxi[1], pxi[0]) = cv::Vec3b(255, 0, 0);
      }
    } else {
      // The patch contains out of bound pixels
      continue;
    }
  }

  VLOG(2) << "zmssd_best = " << zmssd_best << " ssd_corr = " << ssd_corr
          << " zmssd / ssd = " << static_cast<float>(zmssd_best) / ssd_corr;
  if (zmssd_best < PatchScore::threshold()) {
    cam_cur.project(ray_best, px_cur);
    if (options_.subpix_refinement) {
      Vector2f px_scaled(*px_cur / (1 << search_level));
      bool success;
      if (options_.align_1d) {
        Vector2f direction = (px_A - px_B).normalized();
        success = align::align1D(pyrs_cur[search_level], direction,
                                 patch_with_border_, patch_, options_.max_iter,
                                 &px_scaled, &h_inv_);
      } else {
#ifndef _ALIGN2D_TIMING_AND_VERIFYING
#ifndef __ARM_NEON__
        success = align::align2D(pyrs_cur[search_level], patch_with_border_,
                                 patch_, options_.max_iter, &px_scaled);
#else
        success =
            align::align2D_NEON(pyrs_cur[search_level], patch_with_border_,
                                patch_, options_.max_iter, &px_scaled);
#endif  // __ARM_NEON__
#else
#ifdef __ARM_NEON__
        Vector2f px_scaled_neon = px_scaled;
#endif  // __ARM_NEON__
        timer_align2d.start();
        success = align::align2D(pyrs_cur[search_level], patch_with_border_,
                                 patch_, options_.max_iter, &px_scaled);
        timer_align2d.stop();
        align2d_total_time = timer_align2d.elapse_ms();
#ifdef __ARM_NEON__
        timer_align2d_neon.start();
        bool success_neon =
            align::align2D_NEON(pyrs_cur[search_level], patch_with_border_,
                                patch_, options_.max_iter, &px_scaled_neon);
        timer_align2d_neon.stop();
        align2d_total_time_neon = timer_align2d_neon.elapse_ms();
        CHECK_EQ(success, success_neon);
        if (success_neon) {
          CHECK_NEAR(px_scaled[0], px_scaled_neon[0], 0.05);
          CHECK_NEAR(px_scaled[1], px_scaled_neon[1], 0.05);
          std::cout << "[NEON : NON-NEON --->"
                    << "[" << align2d_total_time_neon << " : "
                    << align2d_total_time << "]" << std::endl;
        }
#endif  // __ARM_NEON__
#endif  // _ALIGN2D_TIMING_AND_VERIFYING
      }

      if (success) {
        *px_cur = px_scaled * (1 << search_level);
        Vector3f f_cur;
        if (cam_cur.backProject(*px_cur, &f_cur)) {
          CHECK_NEAR(f_cur[2], 1.f, 1e-6);
          if (!depthFromTriangulation(R_cur_ref, t_cur_ref, f_ref, f_cur,
                                      depth)) {
            LOG(WARNING) << "depthFromTriangulation fails, set depth to d_max";
            *depth = d_max;
          }
        }
      }

      if (dbg_cur != nullptr) {
        if (success) {
          // green: subpix alignment is good
          cv::circle(*dbg_cur, cv::Point2f((*px_cur)(0), (*px_cur)(1)), 2,
                     cv::Scalar(0, 255, 0));
        } else {
          // red: subpix alignment fails
          cv::circle(*dbg_cur, cv::Point2f((*px_cur)(0), (*px_cur)(1)), 2,
                     cv::Scalar(0, 0, 255));
        }
      }
      return success;
    } else {
      // No subpix refinement
      CHECK_NEAR(ray_best[2], 1.f, 1e-6);
      if (!depthFromTriangulation(R_cur_ref, t_cur_ref, f_ref, ray_best,
                                  depth)) {
        LOG(WARNING) << "depthFromTriangulation fails, set depth to d_max";
        *depth = d_max;
      }
      return true;
    }
  }

// No patch qualifiess a match
#ifndef __FEATURE_UTILS_NO_DEBUG__
  VLOG(1) << "No matching patch found for this feature";
#endif
  return false;
}

// Prepare all the shared variabls for the whole tracking pipeline
ImgFeaturePropagator::ImgFeaturePropagator(
    const Eigen::Matrix3f& cur_camK, const Eigen::Matrix3f& ref_camK,
    const cv::Mat_<float>& cur_cv_dist_coeff,
    const cv::Mat_<float>& ref_cv_dist_coeff, const cv::Mat_<uchar>& cur_mask,
    int feat_det_pyramid_level, float min_feature_distance_over_baseline_ratio,
    float max_feature_distance_over_baseline_ratio)
    : mask_cur_(cur_mask),
      min_feature_distance_over_baseline_ratio_(
          min_feature_distance_over_baseline_ratio),
      max_feature_distance_over_baseline_ratio_(
          max_feature_distance_over_baseline_ratio),
      feat_det_pyramid_level_(feat_det_pyramid_level) {
  CHECK_GT(mask_cur_.rows, 0);
  CHECK_GT(mask_cur_.cols, 0);
  if (cur_cv_dist_coeff.rows == 8) {
    cam_cur_.reset(new vio::cameras::PinholeCamera<
                   vio::cameras::RadialTangentialDistortion8>(
        mask_cur_.cols, mask_cur_.rows, cur_camK(0, 0),  // focalLength[0],
        cur_camK(1, 1),                                  // focalLength[1],
        cur_camK(0, 2),                                  // principalPoint[0],
        cur_camK(1, 2),                                  // principalPoint[1],
        vio::cameras::RadialTangentialDistortion8(
            cur_cv_dist_coeff(0), cur_cv_dist_coeff(1), cur_cv_dist_coeff(2),
            cur_cv_dist_coeff(3), cur_cv_dist_coeff(4), cur_cv_dist_coeff(5),
            cur_cv_dist_coeff(6), cur_cv_dist_coeff(7))));
  } else if (cur_cv_dist_coeff.rows == 4) {
    cam_cur_.reset(new vio::cameras::PinholeCamera<
                   vio::cameras::RadialTangentialDistortion>(
        mask_cur_.cols, mask_cur_.rows, cur_camK(0, 0),  // focalLength[0],
        cur_camK(1, 1),                                  // focalLength[1],
        cur_camK(0, 2),                                  // principalPoint[0],
        cur_camK(1, 2),                                  // principalPoint[1],
        vio::cameras::RadialTangentialDistortion(
            cur_cv_dist_coeff(0), cur_cv_dist_coeff(1), cur_cv_dist_coeff(2),
            cur_cv_dist_coeff(3))));
  } else {
    LOG(FATAL) << "Dist model unsupported for cam_cur_";
  }
  if (ref_cv_dist_coeff.rows == 8) {
    cam_ref_.reset(new vio::cameras::PinholeCamera<
                   vio::cameras::RadialTangentialDistortion8>(
        mask_cur_.cols, mask_cur_.rows, ref_camK(0, 0),  // focalLength[0],
        ref_camK(1, 1),                                  // focalLength[1],
        ref_camK(0, 2),                                  // principalPoint[0],
        ref_camK(1, 2),                                  // principalPoint[1],
        vio::cameras::RadialTangentialDistortion8(
            ref_cv_dist_coeff(0), ref_cv_dist_coeff(1), ref_cv_dist_coeff(2),
            ref_cv_dist_coeff(3), ref_cv_dist_coeff(4), ref_cv_dist_coeff(5),
            ref_cv_dist_coeff(6), ref_cv_dist_coeff(7))));
  } else if (ref_cv_dist_coeff.rows == 4) {
    cam_ref_.reset(new vio::cameras::PinholeCamera<
                   vio::cameras::RadialTangentialDistortion>(
        mask_cur_.cols, mask_cur_.rows, ref_camK(0, 0),  // focalLength[0],
        ref_camK(1, 1),                                  // focalLength[1],
        ref_camK(0, 2),                                  // principalPoint[0],
        ref_camK(1, 2),                                  // principalPoint[1],
        vio::cameras::RadialTangentialDistortion(
            ref_cv_dist_coeff(0), ref_cv_dist_coeff(1), ref_cv_dist_coeff(2),
            ref_cv_dist_coeff(3))));
  } else {
    LOG(FATAL) << "Dist model unsupported for cam_ref_";
  }
}

bool ImgFeaturePropagator::PropagateFeatures(
    const cv::Mat& cur_img,
    const cv::Mat& ref_img,  // TODO(mingyu): store image pyramids
    const std::vector<cv::KeyPoint>& ref_keypoints, const Matrix4f& T_ref_cur,
    std::vector<cv::KeyPoint>* cur_keypoints, cv::Mat* cur_orb_features,
    const bool draw_debug) {
  cur_keypoints->clear();
  cur_keypoints->reserve(ref_keypoints.size());

  R_cur_ref_ = T_ref_cur.topLeftCorner<3, 3>().transpose();
  t_cur_ref_ = -R_cur_ref_ * T_ref_cur.topRightCorner<3, 1>();

  // TODO(mingyu): Make shared variables of DirectMatcher into member variables
  //               instead of passing as input arguments

  // Heuristically determine the d_max, d_min, and d_estimate based on the
  // baseline,
  // d_min = baseline x 3
  // d_max = baseline x 3000
  // inv_d_estimate is the average of inv_d_min and inv_d_max
  const float baseline = t_cur_ref_.norm();
  const float d_min = baseline * min_feature_distance_over_baseline_ratio_;
  const float d_max = baseline * max_feature_distance_over_baseline_ratio_;
  const float d_estimate = 2.f / (1.f / d_min + 1.f / d_max);

  // TODO(mingyu): feed the backend results back for d_estimate if available
  // TODO(mingyu): feed the pyramids in directly
  std::vector<cv::Mat> pyrs_cur(feat_det_pyramid_level_);
  std::vector<cv::Mat> pyrs_ref(feat_det_pyramid_level_);
  pyrs_cur[0] = cur_img;
  pyrs_ref[0] = ref_img;
  for (int pyr_lv = 1; pyr_lv < feat_det_pyramid_level_; ++pyr_lv) {
    pyrs_cur[pyr_lv] = fast_pyra_down(pyrs_cur[pyr_lv - 1]);
    pyrs_ref[pyr_lv] = fast_pyra_down(pyrs_ref[pyr_lv - 1]);
  }

  for (const cv::KeyPoint& ref_kp : ref_keypoints) {
    int level_cur = 0;
    float depth = -1.f;
    Vector2f px_cur, px_ref(ref_kp.pt.x, ref_kp.pt.y);
    Vector3f f_ref;
    if (!cam_ref_->backProject(px_ref, &f_ref)) {
      continue;
    }

    // Visualization (re-draw for every keypoint)
    if (draw_debug) {
      dbg_img_.create(mask_cur_.rows * 2, mask_cur_.cols, CV_8UC3);
      dbg_ref_ = dbg_img_(cv::Rect(0, 0, mask_cur_.cols, mask_cur_.rows));
      dbg_cur_ =
          dbg_img_(cv::Rect(0, mask_cur_.rows, mask_cur_.cols, mask_cur_.rows));
      dbg_cur_ptr_ = &dbg_cur_;
      cv::cvtColor(ref_img, dbg_ref_, CV_GRAY2RGB);
      cv::cvtColor(cur_img, dbg_cur_, CV_GRAY2RGB);
      cv::circle(dbg_ref_, ref_kp.pt, 2, cv::Scalar(0, 255, 0));
      cv::putText(dbg_ref_, boost::lexical_cast<std::string>(ref_kp.class_id),
                  ref_kp.pt, cv::FONT_HERSHEY_SIMPLEX, 0.5,
                  cv::Scalar(0, 255, 0), 1);
      for (int i = 0; i < mask_cur_.rows; ++i) {
        for (int j = 0; j < mask_cur_.cols; ++j) {
          if (mask_cur_(i, j) == 0x00) {
            dbg_cur_.at<cv::Vec3b>(i, j)[0] = 0;
            dbg_cur_.at<cv::Vec3b>(i, j)[1] = 0;
          }
        }
      }
    }
#ifndef __FEATURE_UTILS_NO_DEBUG__
    VLOG(1) << "findEpipolarMatchDirect for of_id = " << ref_kp.class_id;
#endif
    if (!direct_matcher_.findEpipolarMatchDirect(
            *cam_ref_, *cam_cur_, px_ref, f_ref, R_cur_ref_, t_cur_ref_,
            ref_kp.octave,  // double check here
            d_estimate, d_min, d_max, pyrs_ref, pyrs_cur, mask_cur_,
            false, /*edgelet_feature, not supported yet*/
            &px_cur, &depth, &level_cur, dbg_cur_ptr_)) {
#ifndef __FEATURE_UTILS_NO_DEBUG__
      if (draw_debug) {
        cv::imwrite("/tmp/per_feat_dbg/det_" +
                        boost::lexical_cast<std::string>(ref_kp.class_id) +
                        ".png",
                    dbg_img_);
      }
      VLOG(1) << "findEpipolarMatchDirect fails for of_id: " << ref_kp.class_id;
#endif
      continue;
    }
#ifndef __FEATURE_UTILS_NO_DEBUG__
    if (draw_debug) {
      cv::imwrite("/tmp/per_feat_dbg/det_" +
                      boost::lexical_cast<std::string>(ref_kp.class_id) +
                      ".png",
                  dbg_img_);
      VLOG(1) << "findEpipolarMatchDirect succeeds for of_id: "
              << ref_kp.class_id;
    }
#endif

    // check boundary condition (set to 20 pixels for computing ORB features)
    // [NOTE] Returned px_cur should be within mask_cur_ already
    const int orb_desc_margin = 20;
    if (px_cur(0) < orb_desc_margin ||
        px_cur(0) > mask_cur_.cols - orb_desc_margin ||
        px_cur(1) < orb_desc_margin ||
        px_cur(1) > mask_cur_.rows - orb_desc_margin) {
      continue;
    }

    cv::KeyPoint cur_kp = ref_kp;
    cur_kp.octave = level_cur;
    cur_kp.pt.x = px_cur(0);
    cur_kp.pt.y = px_cur(1);
    cur_keypoints->push_back(cur_kp);
  }

  if (cur_orb_features != nullptr && cur_keypoints->size() > 0) {
    cur_orb_features->create(cur_keypoints->size(), 32, CV_8U);
#ifdef __ARM_NEON__
    ORBextractor::computeDescriptorsN512(cur_img, *cur_keypoints,
                                         cur_orb_features);
#else
    ORBextractor::computeDescriptors(cur_img, *cur_keypoints, cur_orb_features);
#endif
  }
  return true;
}

}  // namespace XP


/*
 * This file is part of the Interpolated Polyline (https://github.com/fzi-forschungszentrum-informatik/P3IV),
 * copyright by FZI Forschungszentrum Informatik, licensed under the BSD-3 license (see LICENSE file in main directory)
 */

#pragma once
#include <iostream>
#include <Eigen/Core>

namespace util_probability {

template <typename T, int DistributionDim>
struct NormalDistributionSequence {
    using Mean = Eigen::Matrix<T, Eigen::Dynamic, DistributionDim>;
    using Covariance = Eigen::Matrix<T, Eigen::Dynamic, DistributionDim>;

    NormalDistributionSequence() = default;

    NormalDistributionSequence(const Eigen::Ref<const Mean>& mean_, const Eigen::Ref<const Covariance>& covariance_)
            : _mean{mean_}, _covariance{covariance_} {
    }

    NormalDistributionSequence(const Eigen::Ref<const Mean>& mean_, Eigen::Matrix<T, Eigen::Dynamic, 1>& variance_)
            : _mean{mean_}, _covariance{Covariance::Zero()} {

        for (size_t i = 0; i < variance_.size(); i++) {
            _covariance(i, i) = variance_(i);
        }
    }

    NormalDistributionSequence(std::vector<T> mean_, std::vector<T> covariance_) {
        _mean = Eigen::Map<Mean, Eigen::Unaligned>(mean_.data(), mean_.size() / DistributionDim, DistributionDim);
        _covariance = Eigen::Map<Covariance, Eigen::Unaligned>(
            covariance_.data(), covariance_.size() / DistributionDim, DistributionDim);
    };

    size_t dimension() const {
        return static_cast<size_t>(_mean.size() / DistributionDim);
    }

    void setMean(std::vector<T> mean_) {
        _mean = Eigen::Map<Mean, Eigen::Unaligned>(mean_.data(), mean_.size() / DistributionDim, DistributionDim);
    }

    void setCovariance(std::vector<T> covariance_) {
        _covariance = Eigen::Map<Mean, Eigen::Unaligned>(
            covariance_.data(), covariance_.size() / DistributionDim, DistributionDim);
    }

    Mean mean() const {
        return _mean;
    }

    Covariance covariance() const {
        return _covariance;
    }

    std::vector<T> meanVec() {
        std::vector<T> mean(_mean.data(), _mean.data() + _mean.rows() * _mean.cols());
        return mean;
    }

    T covariance(size_t r, size_t c) const {
        return _covariance(r, c);
    }

protected:
    Mean _mean;
    Covariance _covariance;
};

template <typename T>
struct UnivariateNormalDistributionSequence : NormalDistributionSequence<T, 1> {
    using Mean = Eigen::Matrix<T, Eigen::Dynamic, 1>;
    using Covariance = Eigen::Matrix<T, Eigen::Dynamic, 1>;

    using NormalDistributionSequence<T, 1>::NormalDistributionSequence;

    using NormalDistributionSequence<T, 1>::mean;
    using NormalDistributionSequence<T, 1>::covariance;

    T mean(size_t e) const {
        return this->_mean(e, 0);
    }

    T covariance(size_t e) const {
        return this->_covariance(e, 0);
    }
};

template <typename T>
struct BivariateNormalDistributionSequence : NormalDistributionSequence<T, 2> {
    using Mean = Eigen::Matrix<T, Eigen::Dynamic, 2>;
    using Covariance = Eigen::Matrix<T, Eigen::Dynamic, 4>;

    using NormalDistributionSequence<T, 2>::NormalDistributionSequence;

    BivariateNormalDistributionSequence(const Eigen::Ref<const Mean>& mean_,
                                        Eigen::Matrix<T, Eigen::Dynamic, 1>& variance_)
            : NormalDistributionSequence<T, 2>(mean_, Covariance::Zero()) {

        for (size_t i = 0; i < this->dimension(); i++) {
            this->_covariance(i, 0) = variance_(i);
            this->_covariance(i, 3) = variance_(i);
        }
    }

    using NormalDistributionSequence<T, 2>::mean;
    using NormalDistributionSequence<T, 2>::covariance;

    T mean(size_t e, size_t c) const {
        return this->_mean(e, c);
    }

    T covariance(size_t e, size_t c) const {
        return this->_covariance(e, c);
    }

    virtual Eigen::Matrix<T, Eigen::Dynamic, 2> variance() const {
        Eigen::Matrix<T, Eigen::Dynamic, 2> variance;
        for (size_t i = 0; i < this->dimension(); i++) {
            variance(i, 0) = this->_covariance(i, 0);
            variance(i, 1) = this->_covariance(i, 3);
        }
        return variance;
    }
};

} // namespace util_probability


#include "DarkART/Wavefunctions_Initial.hpp"

#include <cmath>
#include <complex>
#include <fstream>
#include <stdlib.h>

#include <boost/math/quadrature/gauss_kronrod.hpp>

#include "libphysica/Integration.hpp"
#include "libphysica/Natural_Units.hpp"
#include "libphysica/Utilities.hpp"

#include "DarkART/Special_Functions.hpp"
#include "DarkART/version.hpp"

namespace DarkART
{
using namespace std::complex_literals;
using namespace libphysica::natural_units;
using namespace boost::math::quadrature;
using boost::math::factorial;

double a0 = Bohr_Radius;
double au = 27.211386245988 * eV;

// 1. Initial state: Roothaan-Hartree-Fock Ground-State Atomic Wave Functions

void Initial_Electron_State::Import_RHF_Coefficients()
{
	std::string filepath = TOP_LEVEL_DIR "data/" + Orbital_Name() + ".txt";
	if(libphysica::File_Exists(filepath) == false)
	{
		std::cerr << "Error in Initial_Electron_State::Import_RHF_Coefficients(): Coefficient table for " << Orbital_Name() << " does not exist." << std::endl;
		std::exit(EXIT_FAILURE);
	}

	std::ifstream f;
	f.open(filepath);
	if(f.is_open())
	{
		f >> binding_energy;
		binding_energy *= au;
		double C, Z;
		unsigned int nin;
		while(f >> nin >> Z >> C)
		{
			n_lj.push_back(nin);
			Z_lj.push_back(Z);
			C_nlj.push_back(C);
		}
		f.close();
	}
	Z_eff = sqrt(-2.0 * binding_energy / au) * n;
}

void Initial_Electron_State::Check_Normalization()
{
	double norm = Normalization();
	if(std::fabs(1.0 - norm) > 1.0e-4)
	{
		std::cout << "Error in Initial_Electron_State(): Normalization of " << element_name << " = " << norm << " != 1.0" << std::endl;
		std::exit(EXIT_FAILURE);
	}
}

Initial_Electron_State::Initial_Electron_State()
: element_name("none"), n(0), l(0), binding_energy(0.0), Z_eff(0.0)
{
}

Initial_Electron_State::Initial_Electron_State(const std::string& element, int N, int L)
: element_name(element), n(N), l(L)
{
	Import_RHF_Coefficients();
	Check_Normalization();
}

Initial_Electron_State::Initial_Electron_State(const std::string& element, std::string shell_name)
: element_name(element)
{
	n = shell_name[0] - '0';
	for(l = 0; l < l_orbital_names.size(); l++)
		if(shell_name[1] == l_orbital_names[l][0])
			break;
	Import_RHF_Coefficients();
	Check_Normalization();
}

std::string Initial_Electron_State::Orbital_Name() const
{
	return element_name + "_" + std::to_string(n) + l_orbital_names[l];
}

double Initial_Electron_State::Radial_Wavefunction(double r) const
{
	double R_nl = 0.0;
	for(unsigned int j = 0; j < C_nlj.size(); j++)
		R_nl += C_nlj[j] * std::pow(2.0 * Z_lj[j], n_lj[j] + 0.5) / sqrt(factorial<double>(2.0 * n_lj[j])) * std::pow(r / a0, n_lj[j] - 1.0) * std::exp(-Z_lj[j] * r / a0);

	return std::pow(a0, -1.5) * R_nl;
}

double Initial_Electron_State::Radial_Wavefunction_Derivative(double r) const
{
	double dR_dr = 0.0;
	for(unsigned int j = 0; j < C_nlj.size(); j++)
		dR_dr += C_nlj[j] * std::pow(2.0 * Z_lj[j], n_lj[j] + 0.5) / sqrt(factorial<double>(2.0 * n_lj[j])) * ((n_lj[j] - 1.0) / a0 * std::pow(r / a0, n_lj[j] - 2.0) - Z_lj[j] / a0 * std::pow(r / a0, n_lj[j] - 1.0)) * std::exp(-Z_lj[j] * r / a0);

	return std::pow(a0, -1.5) * dR_dr;
}

double Initial_Electron_State::Normalization() const
{
	std::function<double(double)> integrand = [this](double r) {
		double R = Radial_Wavefunction(r);
		return r * r * R * R;
	};
	// Integrate with Gauss Legendre
	return libphysica::Integrate_Gauss_Legendre(integrand, 0.0, 50.0 * Bohr_Radius, 1000);
}

double Initial_Electron_State::Radial_Integral(double r) const
{
	std::function<double(double)> integrand = [this](double rprime) {
		double R = Radial_Wavefunction(rprime);
		return rprime * rprime * R * R;
	};
	return gauss_kronrod<double, 31>::integrate(integrand, 0.0, r, 5, 1e-9);
}

void Initial_Electron_State::Print_Summary(unsigned int mpi_rank) const
{
	if(mpi_rank == 0)
	{
		std::cout << SEPARATOR
				  << Orbital_Name() << " - Summary" << std::endl
				  << std::endl
				  << "Binding energy [eV]:\t" << In_Units(binding_energy, eV) << std::endl
				  << "Z_effective:\t\t" << Z_eff << std::endl
				  << std::endl
				  << "n_lj\tZ_lj\tC_nlj" << std::endl;
		for(unsigned int i = 0; i < C_nlj.size(); i++)
			std::cout << n_lj[i] << "\t" << Z_lj[i] << "\t" << C_nlj[i] << std::endl;
	}
}

}	// namespace DarkART


#include <boost/math/distributions/normal.hpp>

#include <iostream>

namespace bmath = boost::math;

inline double normal_cdf(const double x, const double mean, const double sd) {
    if (sd == 0.0) {return 0.0;}
    return bmath::cdf(bmath::normal(mean, sd), x);
}

inline double normal_ccdf(const double x, const double mean, const double sd) {
    if (sd == 0.0) {return 0.0;}
    return bmath::cdf(bmath::complement(bmath::normal(mean, sd), x));
}

int main() {
    std::cout << normal_cdf(0.0, 0.0, 1.0) << std::endl;
    std::cout << normal_cdf(0.0, 0.0, 1.0) << std::endl;
    return 0;
}


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>
#include <filesystem>
#include <string>

using namespace std::string_literals;

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

#include "miMaS/field.h"
#include "miMaS/complex_field.h"
#include "miMaS/weno.h"
#include "miMaS/fft.h"
#include "miMaS/array_view.h"
#include "miMaS/poisson.h"
#include "miMaS/rk.h"
#include "miMaS/config.h"
#include "miMaS/signal_handler.h"
#include "miMaS/iteration.h"

namespace o2 {
  template < typename _T , std::size_t NumDimsV >
  auto
  trp_v ( field<_T,NumDimsV> const & u , ublas::vector<_T> const& E )
  {
    field<_T,NumDimsV> trp(tools::array_view<const std::size_t>(u.shape(),NumDimsV+1));

    { auto k=0, km1=trp.size(0)-1;
      for ( auto i=0 ; i<trp.size(1) ; ++i ) {
        trp[k][i] = ( E(i)*(u[k+1][i]-u[km1][i])/(2.*u.step.dv) );
      }
    }
    for ( auto k=1 ; k<trp.size(0)-1 ; ++k ) {
      for ( auto i=0 ; i<trp.size(1) ; ++i ) {
        trp[k][i] = ( E(i)*(u[k+1][i]-u[k-1][i])/(2.*u.step.dv) );
      }
    }
    { auto k=trp.size(0)-1, kp1=0;
      for ( auto i=0 ; i<trp.size(1) ; ++i ) {
        trp[k][i] = ( E(i)*(u[kp1][i]-u[k-1][i])/(2.*u.step.dv) );
      }
    }

    return trp;
  }
}

struct iter_s {
  std::size_t iter;
  double dt;
  double current_time;
  double Lhfh;
  double LE;
};

#define save(data,dir,suffix,x_y) {\
  std::stringstream filename; filename << #data << "_" << suffix << ".dat"; \
  std::ofstream of( dir / filename.str() );\
  std::transform( data.begin() , data.end() , std::ostream_iterator<std::string>(of,"\n") , x_y );\
  of.close();\
}

namespace math = boost::math::constants;
const std::complex<double> & I = std::complex<double>(0.,1.);

#define SQ(X) ((X)*(X))
#define Xi(i) (i*fh.step.dx+fh.range.x_min)
#define Vk(k) (k*fh.step.dv+fh.range.v_min)

auto
maxwellian ( double rho , double u , double T ) {
  return [=](double x,double v){ return rho/(std::sqrt(2.*math::pi<double>()*T))*std::exp( -0.5*SQ(v-u)/T ); };
}

int
main(int argc, char const *argv[])
{
  const double sigma = 1.733;

  std::filesystem::path p("config.init");
  if ( argc > 1 ) {
    p = argv[1];
  }
  auto c = config(p);
  c.name = "vhll";

  std::filesystem::create_directories(c.output_dir);
  std::cout << c << std::endl;
  std::ofstream oconfig( c.output_dir / "config.init" );
  oconfig << c << std::endl;
  oconfig.close();

/* --------------------------------------------------------------- */
  field<double,1> fh(boost::extents[c.Nv][c.Nx]);
  complex_field<double,1> hfh(boost::extents[c.Nv][c.Nx]);

  fh.range.v_min = -8.; fh.range.v_max = 8.;
  fh.step.dv = (fh.range.v_max-fh.range.v_min)/c.Nv;

  double Kx = 0.5;
  fh.range.x_min = 0.; fh.range.x_max = 2./Kx*math::pi<double>();
  fh.step.dx = (fh.range.x_max-fh.range.x_min)/c.Nx;

  ublas::vector<double> v (c.Nv,0.);
  for ( std::size_t k=0 ; k<c.Nv ; ++k ) { v[k] = Vk(k); }

  ublas::vector<double> kx(c.Nx);
  {
    double l = fh.range.len_x();
    for ( auto i=0 ; i<c.Nx/2 ; ++i ) { kx[i]      = 2.*math::pi<double>()*i/l; }
    for ( int i=-c.Nx/2 ; i<0 ; ++i ) { kx[c.Nx+i] = 2.*math::pi<double>()*i/l; }
  }

  auto tb_M1    = maxwellian( 0.5*c.alpha ,  c.ui , 1.   ) , tb_M2  = maxwellian( 0.5*c.alpha , -c.ui , 1.  );

  for (field<double,2>::size_type k=0 ; k<fh.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<fh.size(1) ; ++i ) {
      // tb
      fh[k][i] = ( tb_M1(Xi(i),Vk(k)) + tb_M2(Xi(i),Vk(k)) )*(1. + 0.01*std::cos(Kx*Xi(i)));
      //fh[k][i] = ( tb_M1(Xi(i),Vk(k)) + tb_M2(Xi(i),Vk(k)) )*(1.);
    }
    fft::fft(&(fh[k][0]),&(fh[k][c.Nx-1])+1,&(hfh[k][0]));
  }
  fh.write( c.output_dir / "init_vhll.dat" );

  iteration::iteration<double> iter;
  iter.iter = 0;
  iter.current_time = 0.;
  iter.dt = 0.5*fh.step.dv;

  ublas::vector<double> uc(c.Nx,0.);
  ublas::vector<double> E (c.Nx,0.);

  std::vector<double> ee;   ee.reserve(int(std::ceil(c.Tf/iter.dt))+1);
  std::vector<double> Emax; Emax.reserve(int(std::ceil(c.Tf/iter.dt))+1);
  std::vector<double> H;    H.reserve(int(std::ceil(c.Tf/iter.dt))+1);

  std::vector<double> times; times.reserve(int(std::ceil(c.Tf/iter.dt))+1);
  std::vector<iteration::iteration<double>> iterations;         iterations.reserve(int(std::ceil(c.Tf/iter.dt))+1);
  std::vector<iteration::iteration<double>> success_iterations; success_iterations.reserve(int(std::ceil(c.Tf/iter.dt))+1);

  auto save_data = [&] ( std::string && suffix ) {
    suffix = c.name + suffix;

    // save iterations informations
    auto writer_iter = [] ( auto const & it ) {
      std::stringstream ss; ss << it;
      return ss.str();
    };
    c << monitoring::data( "iterations_"+suffix+".dat"         , iterations         , writer_iter );
    c << monitoring::data( "success_iterations_"+suffix+".dat" , success_iterations , writer_iter );

    // save temporel data
    auto dt_y = [&,count=0] (auto const& y) mutable {
      std::stringstream ss; ss<<times[count++]<<" "<<y;
      return ss.str();
    };
    c << monitoring::data( "ee_"+suffix+".dat"   , ee   , dt_y );
    c << monitoring::data( "Emax_"+suffix+".dat" , Emax , dt_y );
    c << monitoring::data( "H_"+suffix+".dat"    , H    , dt_y );

    // save distribution function
    for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
    fh.write( c.output_dir / ("vp_"+suffix+".dat") );

  };

  signal_handler::signal_handler<SIGINT,SIGILL>::handler( [&]( int signal ) -> void {
    std::cerr << "\n\033[41;97m ** End of execution after signal " << signal << " ** \033[0m\n";
    std::cerr << "\033[36msave data...\033[0m\n";

    save_data("_SIGINT");
  });

  const double rho_c = 1.-c.alpha;
  const double sqrt_rho_c = std::sqrt(rho_c);
  {
    poisson<double> poisson_solver(c.Nx,fh.range.len_x());
    ublas::vector<double> rho(c.Nx,0.);
    rho = fh.density(); // compute density from init data
    for ( auto i=0 ; i<c.Nx ; ++i ) { rho[i] += (1.-c.alpha); } // add (1-alpha) for cold particules
    E = poisson_solver(rho);

    Emax.push_back( std::abs(*std::max_element( E.begin() , E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );
    double electric_energy = 0.;
    for ( const auto & ei : E ) { electric_energy += ei*ei*fh.step.dx; }
    ee.push_back( std::sqrt(electric_energy) );

    double total_energy = energy(fh,E);
    total_energy += 0.; // sum(rho_c*u_c*u_c) = 0 because u_c = 0 at time 0
    H.push_back( total_energy );
    times.push_back( 0. );
  }

  // initialize memory for all temporary variables
  ublas::vector<double> J(c.Nx);
  fft::spectrum_ d(c.Nx);
  field<double,1> Edvf(tools::array_view<const std::size_t>(fh.shape(),2));
  ublas::vector<double> uc1(c.Nx) , uc2(c.Nx) , uc3(c.Nx) , uc4(c.Nx) , uc5(c.Nx) , uc6(c.Nx) , uc7(c.Nx),
                        E1(c.Nx)  , E2(c.Nx)  , E3(c.Nx)  , E4(c.Nx)  , E5(c.Nx)  , E6(c.Nx)  , E7(c.Nx);
  complex_field<double,1> hfh1(boost::extents[c.Nv][c.Nx]),hfh2(boost::extents[c.Nv][c.Nx]),hfh3(boost::extents[c.Nv][c.Nx]),hfh4(boost::extents[c.Nv][c.Nx]),hfh5(boost::extents[c.Nv][c.Nx]),hfh6(boost::extents[c.Nv][c.Nx]),hfh7(boost::extents[c.Nv][c.Nx]);

  while (  iter.current_time < c.Tf ) {
    std::cout << "\r" << iteration::time(iter) << std::flush;

/////////////////////////////////////////////////////////////////////
/**
/////////////////////////////////////////////////////////////////////
    // RK(3,3) classical

    // STAGE 1
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E);

      double c1 = std::cos(dt*sqrt_rho_c), s1 = std::sin(dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc1[i] =  uc[i]*c1 + E[i]*s1/sqrt_rho_c - dt*J[i]*s1/sqrt_rho_c;
        E1[i]  = -uc[i]*s1*sqrt_rho_c + E[i]*c1 - dt*J[i]*c1;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh1[k][i] = hfh[k][i]*std::exp(-I*kx[i]*Vk(k)*dt) - dt*d[i]*std::exp(-I*kx[i]*Vk(k)*dt);
        }
      }
    } // end stage 1

    // STAGE 2
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh1[k].begin(),hfh1[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E1);

      double c2 = std::cos(0.5*dt*sqrt_rho_c), s2 = std::sin(0.5*dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc2[i] =  0.75*uc[i]*c2 + 0.75*E[i]*s2/sqrt_rho_c + 0.25*uc1[i]*c2 - 0.25*E1[i]*s2/sqrt_rho_c + 0.25*dt*J[i]*s2/sqrt_rho_c;
        E2[i]  = -0.75*uc[i]*s2*sqrt_rho_c + 0.75*E[i]*c2 + 0.25*uc1[i]*s2*sqrt_rho_c + 0.25*E1[i]*c2 - 0.25*dt*J[i]*c2;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh2[k][i] = 0.75*hfh[k][i]*std::exp(-I*kx[i]*Vk(k)*0.5*dt) + 0.25*hfh1[k][i]*std::exp(I*kx[i]*Vk(k)*0.5*dt) - 0.25*dt*d[i]*std::exp(I*kx[i]*Vk(k)*0.5*dt);
        }
      }
    } // end stage 2

    // STAGE 3
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh2[k].begin(),hfh2[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E2);

      double c1 = std::cos(dt*sqrt_rho_c), s1 = std::sin(dt*sqrt_rho_c);
      double c2 = std::cos(0.5*dt*sqrt_rho_c), s2 = std::sin(0.5*dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        double tmp_uc =  (1./3.)*uc[i]*c1 + (1./3.)*E[i]*s1/sqrt_rho_c + (2./3.)*uc2[i]*c2 + (2./3.)*E2[i]*s2/sqrt_rho_c - (2./3.)*dt*J[i]*s2/sqrt_rho_c;
        E[i]          = -(1./3.)*uc[i]*s1*sqrt_rho_c + (1./3.)*E[i]*c1 - (2./3.)*uc2[i]*s2*sqrt_rho_c + (2./3.)*E2[i]*c2 - (2./3.)*dt*J[i]*c2;
        uc[i] = tmp_uc;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh[k][i] = (1./3.)*hfh[k][i]*std::exp(-I*kx[i]*Vk(k)*dt) + (2./3.)*hfh2[k][i]*std::exp(-I*kx[i]*Vk(k)*0.5*dt) - (2./3.)*dt*d[i]*std::exp(-I*kx[i]*Vk(k)*0.5*dt);
        }
      }
    } // end stage 3

/////////////////////////////////////////////////////////////////////
**
/////////////////////////////////////////////////////////////////////
    // RK(3,3) NSSP

    // STAGE 1
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E);

      double c49 = std::cos((4./9.)*dt*sqrt_rho_c), s49 = std::sin((4./9.)*dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc1[i] =  uc[i]*c49 - E[i]*s49/sqrt_rho_c - (4./9.)*dt*J[i]*s49/sqrt_rho_c;
        E1[i]  =  uc[i]*s49*sqrt_rho_c + E[i]*c49 + (4./9.)*dt*J[i]*c49;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh1[k][i] = hfh[k][i]*std::exp((4./9.)*I*kx[i]*Vk(k)*dt) + (4./9.)*dt*d[i]*std::exp((4./9.)*I*kx[i]*Vk(k)*dt);
        }
      }
    } // end stage 1

    // STAGE 2
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh1[k].begin(),hfh1[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E1);

      double c23  = std::cos((2./3.)*dt*sqrt_rho_c)  , s23  = std::sin((2./3.)*dt*sqrt_rho_c) ,
             c109 = std::cos((10./9.)*dt*sqrt_rho_c) , s109 = std::sin((10./9.)*dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc2[i] = (29./8.)*(  uc[i]*c23 + E[i]*s23/sqrt_rho_c ) - (21./8.)*(  uc1[i]*c109 + E1[i]*s109/sqrt_rho_c ) + 0.5*dt*J[i]*s109/sqrt_rho_c;
        E2[i]  = (29./8.)*( -uc[i]*s23*sqrt_rho_c + E[i]*c23 ) - (21./8.)*( -uc1[i]*s109*sqrt_rho_c + E1[i]*c109 ) + 0.5*dt*J[i]*c109;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh2[k][i] = (29./8.)*hfh[k][i]*std::exp(-(2./3.)*I*kx[i]*Vk(k)*dt) - (21./8.)*hfh1[k][i]*std::exp(-(10./9.)*I*kx[i]*Vk(k)*dt) + 0.5*dt*d[i]*std::exp(-(10./9.)*I*kx[i]*Vk(k)*dt);
        }
      }
    } // end stage 2

    // STAGE 3
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh2[k].begin(),hfh2[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E2);

      double c1   = std::cos(dt*sqrt_rho_c)          , s1   = std::sin(dt*sqrt_rho_c)         ,
             c13  = std::cos((1./3.)*dt*sqrt_rho_c)  , s13  = std::sin((1./3.)*dt*sqrt_rho_c) ,
             c139 = std::cos((13./9.)*dt*sqrt_rho_c) , s139 = std::sin((13./9.)*dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        double tmp_uc =  (25./16.)*(  uc[i]*c1 + E[i]*s1/sqrt_rho_c ) - (9./16.)*(  uc1[i]*c139 + E1[i]*s139/sqrt_rho_c ) - (3./4.)*dt*J[i]*s13/sqrt_rho_c;
        E[i]          =  (25./16.)*( -uc[i]*s1*sqrt_rho_c + E[i]*c1 ) - (9./16.)*( -uc1[i]*s139*sqrt_rho_c + E1[i]*c139 ) - (3./4.)*dt*J[i]*c13;
        uc[i] = tmp_uc;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh[k][i] = (25./16.)*hfh[k][i]*std::exp(-I*kx[i]*Vk(k)*dt) - (9./16.)*hfh1[k][i]*std::exp(-(13./9.)*I*kx[i]*Vk(k)*dt) - (3./4.)*dt*d[i]*std::exp(-(1./3.)*I*kx[i]*Vk(k)*dt);
        }
      }
    } // end stage 3

/////////////////////////////////////////////////////////////////////
**/
/////////////////////////////////////////////////////////////////////

    // DP4(3)
    // STAGE 1
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E);

      double c05 = std::cos(0.5*iter.dt*sqrt_rho_c), s05 = std::sin(0.5*iter.dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc1[i] =  uc[i]*c05 + E[i]*s05/sqrt_rho_c - 0.5*iter.dt*J[i]*s05/sqrt_rho_c;
        E1[i]  = -uc[i]*s05*sqrt_rho_c + E[i]*c05 - 0.5*iter.dt*J[i]*c05;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh1[k][i] = hfh[k][i]*std::exp(-0.5*I*kx[i]*Vk(k)*iter.dt) - 0.5*iter.dt*d[i]*std::exp(-0.5*I*kx[i]*Vk(k)*iter.dt);
        }
      }
    } // end stage 1

    // STAGE 2
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh1[k].begin(),hfh1[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E1);

      double c05 = std::cos(0.5*iter.dt*sqrt_rho_c), s05 = std::sin(0.5*iter.dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc2[i] =  uc[i]*c05 + E[i]*s05/sqrt_rho_c;
        E2[i]  = -uc[i]*s05*sqrt_rho_c + E[i]*c05 - 0.5*iter.dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh2[k][i] = hfh[k][i]*std::exp(-0.5*I*kx[i]*Vk(k)*iter.dt) - 0.5*iter.dt*d[i];
        }
      }
    } // end stage 2

    // STAGE 3
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh2[k].begin(),hfh2[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E2);

      double c1  = std::cos(iter.dt*sqrt_rho_c)     , s1  = std::sin(iter.dt*sqrt_rho_c)    ,
             c05 = std::cos(0.5*iter.dt*sqrt_rho_c) , s05 = std::sin(0.5*iter.dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc3[i] =  uc[i]*c1 + E[i]*s1/sqrt_rho_c - iter.dt*J[i]*s05/sqrt_rho_c;
        E3[i]  = -uc[i]*s1*sqrt_rho_c + E[i]*c1 - iter.dt*J[i]*c05;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh3[k][i] = hfh[k][i]*std::exp(-I*kx[i]*Vk(k)*iter.dt) - iter.dt*d[i]*std::exp(-0.5*I*kx[i]*Vk(k)*iter.dt);
        }
      }
    } // end stage 3

    // STAGE 4
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh3[k].begin(),hfh3[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E3);

      double c1  = std::cos(iter.dt*sqrt_rho_c)     , s1  = std::sin(iter.dt*sqrt_rho_c)    ,
             c05 = std::cos(0.5*iter.dt*sqrt_rho_c) , s05 = std::sin(0.5*iter.dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc4[i] = -(1./3.)*(  uc[i]*c1 + E[i]*s1/sqrt_rho_c ) + (1./3.)*(  uc1[i]*c05 + E1[i]*s05/sqrt_rho_c ) + (2./3.)*(  uc2[i]*c05 + E2[i]*s05/sqrt_rho_c ) + uc3[i]/3.;
        E4[i]  = -(1./3.)*( -uc[i]*s1*sqrt_rho_c + E[i]*c1 ) + (1./3.)*( -uc1[i]*s05*sqrt_rho_c + E1[i]*c05 ) + (2./3.)*( -uc2[i]*s05*sqrt_rho_c + E2[i]*c05 ) + E3[i]/3. - (1./6.)*iter.dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh4[k][i] = -(1./3.)*hfh[k][i]*std::exp(-I*kx[i]*Vk(k)*iter.dt) + (1./3.)*hfh1[k][i]*std::exp(-0.5*I*kx[i]*Vk(k)*iter.dt) + (2./3.)*hfh2[k][i]*std::exp(-0.5*I*kx[i]*Vk(k)*iter.dt) + hfh3[k][i]/3. - (1./6.)*iter.dt*d[i];
        }
      }
    } // end stage 4

/*
    // STAGE 5
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh4[k].begin(),hfh4[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E4);

      double c1  = std::cos(iter.dt*sqrt_rho_c)     , s1  = std::sin(iter.dt*sqrt_rho_c)    ,
             c05 = std::cos(0.5*iter.dt*sqrt_rho_c) , s05 = std::sin(0.5*iter.dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc5[i] = -(1./5.)*(  uc[i]*c1 + E[i]*s1/sqrt_rho_c ) + (1./5.)*(  uc1[i]*c05 + E1[i]*s05/sqrt_rho_c ) + (2./5.)*(  uc2[i]*c05 + E2[i]*s05/sqrt_rho_c ) + uc3[i]/5. + (2./5.)*uc4[i];
        E5[i]  = -(1./5.)*( -uc[i]*s1*sqrt_rho_c + E[i]*c1 ) + (1./5.)*( -uc1[i]*s05*sqrt_rho_c + E1[i]*c05 ) + (2./5.)*( -uc2[i]*s05*sqrt_rho_c + E2[i]*c05 ) + E3[i]/5. + (2./5.)*E4[i] - (1./10.)*iter.dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh5[k][i] = -(1./5.)*hfh[k][i]*std::exp(-I*kx[i]*Vk(k)*iter.dt) + (1./5.)*hfh1[k][i]*std::exp(-0.5*I*kx[i]*Vk(k)*iter.dt) + (2./5.)*hfh2[k][i]*std::exp(-0.5*I*kx[i]*Vk(k)*iter.dt) + hfh3[k][i]/5. + (2./5.)*hfh4[k][i] - 0.1*iter.dt*d[i];
        }
      }
    } // end stage 5
*/

/////////////////////////////////////////////////////////////////////
/**
/////////////////////////////////////////////////////////////////////

    //DP5
    // STAGE 1
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E);

      double c02 = std::cos(0.2*dt*sqrt_rho_c) , s02 = std::sin(0.2*dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc1[i] =  uc[i]*c02 + E[i]*s02/sqrt_rho_c - 0.2*dt*J[i]*s02/sqrt_rho_c;
        E1[i]  = -uc[i]*s02*sqrt_rho_c + E[i]*c02 - 0.2*dt*J[i]*c02;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh1[k][i] = hfh[k][i]*std::exp(-0.2*I*kx[i]*Vk(k)*dt) - 0.2*dt*d[i]*std::exp(-0.2*I*kx[i]*Vk(k)*dt);
        }
      }
    } // end stage 1

    // STAGE 2
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh1[k].begin(),hfh1[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E1);

      double c01 = std::cos(0.1*sqrt_rho_c*dt) , s01 = std::sin(0.1*sqrt_rho_c*dt),
             c03 = std::cos(0.3*sqrt_rho_c*dt) , s03 = std::sin(0.3*sqrt_rho_c*dt);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc2[i] = 0.625*(  uc[i]*c03 + E[i]*s03/sqrt_rho_c ) + 0.375*(  uc1[i]*c01 + E1[i]*s01/sqrt_rho_c ) - 0.225*dt*J[i]*s01/sqrt_rho_c;
        E2[i]  = 0.625*( -uc[i]*s03*sqrt_rho_c + E[i]*c03 ) + 0.375*( -uc1[i]*sqrt_rho_c*s01 + E1[i]*c01 ) - 0.225*dt*J[i]*c01;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh2[k][i] = 0.625*hfh[k][i]*std::exp(-0.3*I*kx[i]*Vk(k)*dt) + 0.375*hfh1[k][i]*std::exp(-0.1*I*kx[i]*Vk(k)*dt) - 0.225*dt*d[i]*std::exp(-0.1*I*kx[i]*Vk(k));
        }
      }
    } // end stage 2

    // STAGE 3
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh2[k].begin(),hfh2[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E2);

      double c08 = std::cos(0.8*dt*sqrt_rho_c) , s08 = std::sin(0.8*dt*sqrt_rho_c) ,
             c06 = std::cos(0.6*dt*sqrt_rho_c) , s06 = std::sin(0.6*dt*sqrt_rho_c) ,
             c05 = std::cos(0.5*dt*sqrt_rho_c) , s05 = std::sin(0.5*dt*sqrt_rho_c) ;
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc3[i] = (175./27.)*(  uc[i]*c08 + E[i]*s08/sqrt_rho_c ) + (100./9.)*(  uc1[i]*c06 + E1[i]*s06/sqrt_rho_c ) - (448./27.)*(  uc2[i]*c05 + E2[i]*s05/sqrt_rho_c ) - (32./9.)*dt*J[i]*s05/sqrt_rho_c;
        E3[i]  = (175./27.)*( -uc[i]*s08*sqrt_rho_c + E[i]*c08 ) + (100./9.)*( -uc1[i]*s06*sqrt_rho_c + E1[i]*c06 ) - (448./27.)*( -uc2[i]*s05*sqrt_rho_c + E2[i]*c05 ) - (32./9.)*dt*J[i]*c05;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh3[k][i] = (175./27.)*hfh[k][i]*std::exp(-0.8*I*kx[i]*Vk(k)*dt) + (100./9.)*hfh1[k][i]*std::exp(-0.6*I*kx[i]*Vk(k)*dt) - (448./27.)*hfh2[k][i]*std::exp(-0.5*I*kx[i]*Vk(k)*dt) - (32./9.)*dt*d[i]*std::exp(-0.5*I*kx[i]*Vk(k));
        }
      }
    } // end stage 3

    // STAGE 4
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh3[k].begin(),hfh3[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E3);

      double c89    = std::cos((8./9.)*sqrt_rho_c*dt)    , s89    = std::sin((8./9.)*sqrt_rho_c*dt)    ,
             c3145  = std::cos((31./45.)*sqrt_rho_c*dt)  , s3145  = std::sin((31./45.)*sqrt_rho_c*dt)  ,
             c5390  = std::cos((53./90.)*sqrt_rho_c*dt)  , s5390  = std::sin((53./90.)*sqrt_rho_c*dt)  ,
             c44827 = std::cos((448./27.)*sqrt_rho_c*dt) , s44827 = std::sin((448./27.)*sqrt_rho_c*dt) ,
             c445   = std::cos((4./45.)*sqrt_rho_c*dt)   , s445   = std::sin((4./45.)*sqrt_rho_c*dt)   ;
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc4[i] = (3551./6561.)*(  uc[i]*c89 + E[i]*s89/sqrt_rho_c ) + (7420./2187.)*(  uc1[i]*c3145 + E1[i]*s3145/sqrt_rho_c ) - (37376./6561.)*(  uc2[i]*c5390 + E2[i]*s5390/sqrt_rho_c ) + (2014./729.)*(  uc3[i]*c445 + E3[i]*s445/sqrt_rho_c ) + (212./729.)*dt*J[i]*s445/sqrt_rho_c;
        E4[i]  = (3551./6561.)*( -uc[i]*s89*sqrt_rho_c + E[i]*c89 ) + (7420./2187.)*( -uc1[i]*s3145*sqrt_rho_c + E1[i]*c3145 ) - (37376./6561.)*( -uc2[i]*s5390*sqrt_rho_c + E2[i]*c5390 ) + (2014./729.)*( -uc3[i]*s445*sqrt_rho_c + E3[i]*c445 ) + (212./729.)*dt*J[i]*c445;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh4[k][i] = (3551./6561.)*hfh[k][i]*std::exp(-(8./9.)*I*kx[i]*Vk(k)*dt) + (7420./2187.)*hfh1[k][i]*std::exp(-(31./45.)*I*kx[i]*Vk(k)*dt) - (37376./6561.)*hfh2[k][i]*std::exp(-(53./90.)*I*kx[i]*Vk(k)*dt) + (2014./729.)*hfh3[k][i]*std::exp(-(4./45.)*I*kx[i]*Vk(k)*dt) + (212./729.)*dt*d[i]*std::exp(-(4./45.)*kx[i]*Vk(k)*dt);
        }
      }
    } // end stage 4

    // STAGE 5
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh4[k].begin(),hfh4[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E4);

      double c1  = std::cos(sqrt_rho_c*dt)         , s1  = std::sin(sqrt_rho_c*dt)         ,
             c08 = std::cos(0.8*sqrt_rho_c*dt)     , s08 = std::sin(0.8*sqrt_rho_c*dt)     ,
             c07 = std::cos(0.7*sqrt_rho_c*dt)     , s07 = std::sin(0.7*sqrt_rho_c*dt)     ,
             c02 = std::cos(0.2*sqrt_rho_c*dt)     , s02 = std::sin(0.2*sqrt_rho_c*dt)     ,
             c19 = std::cos((1./9.)*sqrt_rho_c*dt) , s19 = std::sin((1./9.)*sqrt_rho_c*dt) ;
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc5[i] = (313397./335808.)*(  uc[i]*c1 + E[i]*s1/sqrt_rho_c ) + (424025./55968.)*(  uc1[i]*c08 + E1[i]*s08/sqrt_rho_c ) - (61400./5247.)*(  uc2[i]*c07 + E2[i]*s07/sqrt_rho_c ) + (96075./18656.)*(  uc3[i]*c02 + E3[i]*s02/sqrt_rho_c ) - (35721./37312.)*(  uc4[i]*c19 + E4[i]*s19/sqrt_rho_c ) + (5103./18656.)*dt*J[i]*s19/sqrt_rho_c;
        E5[i]  = (313397./335808.)*( -uc[i]*s1*sqrt_rho_c + E[i]*c1 ) + (424025./55968.)*( -uc1[i]*s08*sqrt_rho_c + E1[i]*c08 ) - (61400./5247.)*( -uc2[i]*s07*sqrt_rho_c + E2[i]*c07 ) + (96075./18656.)*( -uc3[i]*s02*sqrt_rho_c + E3[i]*c02 ) - (35721./37312.)*( -uc4[i]*s19*sqrt_rho_c + E4[i]*c19 ) + (5103./18656.)*dt*J[i]*c19;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh5[k][i] = (313397./335808.)*hfh[k][i]*std::exp(-I*kx[i]*Vk(k)*dt) + (424025./55968.)*hfh1[k][i]*std::exp(-0.8*I*kx[i]*Vk(k)*dt) - (61400./5247.)*hfh2[k][i]*std::exp(-0.7*I*kx[i]*Vk(k)*dt) + (96075./18656.)*hfh3[k][i]*std::exp(-0.2*I*kx[i]*Vk(k)*dt) - (35721./37312.)*hfh4[k][i]*std::exp(-(1./9.)*I*kx[i]*Vk(k)*dt) + (5103./18656.)*dt*d[i]*std::exp(-(1./9.)*I*kx[i]*Vk(k)*dt);
        }
      }
    } // end stage 5

    // STAGE 6
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh5[k].begin(),hfh5[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E5);

      double c1  = std::cos(sqrt_rho_c*dt)         , s1  = std::sin(sqrt_rho_c*dt)         ,
             c08 = std::cos(0.8*sqrt_rho_c*dt)     , s08 = std::sin(0.8*sqrt_rho_c*dt)     ,
             c07 = std::cos(0.7*sqrt_rho_c*dt)     , s07 = std::sin(0.7*sqrt_rho_c*dt)     ,
             c02 = std::cos(0.2*sqrt_rho_c*dt)     , s02 = std::sin(0.2*sqrt_rho_c*dt)     ,
             c19 = std::cos((1./9.)*sqrt_rho_c*dt) , s19 = std::sin((1./9.)*sqrt_rho_c*dt) ;
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc6[i] = -(563./3456.)*(  uc[i]*c1 + E[i]*s1/sqrt_rho_c ) - (575./252.)*(  uc1[i]*c08 + E1[i]*s08/sqrt_rho_c ) + (31400./10017.)*(  uc2[i]*c07 + E2[i]*s07/sqrt_rho_c ) + (325./1344.)*(  uc3[i]*c02 + E3[i]*s02/sqrt_rho_c ) - (7533./6784.)*(  uc4[i]*c19 + E4[i]*s19/sqrt_rho_c ) + (33./28.)*uc5[i];
        E6[i]  = -(563./3456.)*( -uc[i]*s1*sqrt_rho_c + E[i]*c1 ) - (575./252.)*( -uc1[i]*s08*sqrt_rho_c + E1[i]*c08 ) + (31400./10017.)*( -uc2[i]*s07*sqrt_rho_c + E2[i]*c07 ) + (325./1344.)*( -uc3[i]*s02*sqrt_rho_c + E3[i]*c02 ) - (7533./6784.)*( -uc4[i]*s19*sqrt_rho_c + E4[i]*c19 ) + (33./28.)*E5[i] - (11./84.)*dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh6[k][i] = -(563./3456.)*hfh[k][i]*std::exp(-I*kx[i]*Vk(k)*dt) - (575./252.)*hfh1[k][i]*std::exp(-0.8*I*kx[i]*Vk(k)*dt) + (31400./10017.)*hfh2[k][i]*std::exp(-0.7*I*kx[i]*Vk(k)*dt) + (325./1344.)*hfh3[k][i]*std::exp(-0.2*I*kx[i]*Vk(k)*dt) - (7533./6784.)*hfh4[k][i]*std::exp(-(1./9.)*I*kx[i]*Vk(k)*dt) + (33./28.)*hfh5[k][i] - (11./84.)*dt*d[i];
        }
      }
    } // end stage 6

    // STAGE 7
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh6[k].begin(),hfh6[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E6);

      double c1  = std::cos(sqrt_rho_c*dt)         , s1  = std::sin(sqrt_rho_c*dt)         ,
             c08 = std::cos(0.8*sqrt_rho_c*dt)     , s08 = std::sin(0.8*sqrt_rho_c*dt)     ,
             c07 = std::cos(0.7*sqrt_rho_c*dt)     , s07 = std::sin(0.7*sqrt_rho_c*dt)     ,
             c02 = std::cos(0.2*sqrt_rho_c*dt)     , s02 = std::sin(0.2*sqrt_rho_c*dt)     ,
             c19 = std::cos((1./9.)*sqrt_rho_c*dt) , s19 = std::sin((1./9.)*sqrt_rho_c*dt) ;
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc7[i] = (8813./172800.)*(  uc[i]*c1 + E[i]*s1/sqrt_rho_c ) + (41./180.)*(  uc1[i]*c08 + E1[i]*s08/sqrt_rho_c ) - (4294./10017.)*(  uc2[i]*c07 + E2[i]*s07/sqrt_rho_c ) + (263./384.)*(  uc3[i]*c02 + E3[i]*s02/sqrt_rho_c ) - (137781./339200.)*(  uc4[i]*c19 + E4[i]*s19/sqrt_rho_c ) + (803./4200)*uc5[i] + (17./25.)*uc6[i];
        E7[i]  = (8813./172800.)*( -uc[i]*s1*sqrt_rho_c + E[i]*c1 ) + (41./180.)*( -uc1[i]*s08*sqrt_rho_c + E1[i]*c08 ) - (4294./10017.)*( -uc2[i]*s07*sqrt_rho_c + E2[i]*c07 ) + (263./384.)*( -uc3[i]*s02*sqrt_rho_c + E3[i]*c02 ) - (137781./339200.)*( -uc4[i]*s19*sqrt_rho_c + E4[i]*c19 ) + (803./4200)*E5[i] + (17./25.)*E6[i] - (1./40.)*dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh7[k][i] = (8813./172800.)*hfh[k][i]*std::exp(-I*kx[i]*Vk(k)*dt) + (41./180.)*hfh1[k][i]*std::exp(-0.8*I*kx[i]*Vk(k)*dt) - (4294./10017.)*hfh2[k][i]*std::exp(-0.7*I*kx[i]*Vk(k)*dt) + (263./384.)*hfh3[k][i]*std::exp(-0.2*I*kx[i]*Vk(k)*dt) - (137781./339200.)*hfh4[k][i]*std::exp(-(1./9.)*I*kx[i]*Vk(k)*dt) + (803./4200)*hfh5[k][i] + (17./25.)*hfh6[k][i] - (1./40.)*dt*d[i];
        }
      }
    } // end stage 7
/////////////////////////////////////////////////////////////////////
/**/
/////////////////////////////////////////////////////////////////////
    // end of time loop

    // CHECK local error for compute next time step
    iter.E_error(E5,E4,fh.step.dx);
    iter.hfh_error(hfh5,hfh4,fh.step.dx*fh.step.dv);
    std::cout << " -- " << iteration::error(iter) << std::flush;

    iter.success = std::abs(iter.error() - c.tol) <= c.tol;
    //if ( iter.success ) // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    {
      iter.success = true;
      // SAVE TIME STEP
      std::copy( uc4.begin()  , uc4.end()  , uc.begin()  );
      std::copy( E4.begin()   , E4.end()   , E.begin()   );
      std::copy( hfh4.begin() , hfh4.end() , hfh.begin() );

      // MONITORING

      Emax.push_back( std::abs(*std::max_element( E.begin() , E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );
      double electric_energy = std::sqrt(std::accumulate(
        E.begin() , E.end() , 0. ,
        [&] ( double partial_sum , double ei ) {
          return partial_sum + ei*ei*fh.step.dx;
        }
      ));
      ee.push_back( electric_energy );

      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
      double total_energy = energy(fh,E);
      {
        auto rhoh = fh.density();
        fft::spectrum_ hrhoh(c.Nx); hrhoh.fft(&rhoh[0]);
        fft::spectrum_ hE(c.Nx); hE.fft(&E[0]);
        fft::spectrum_ hrhoc(c.Nx);
        hrhoc[0] = I*kx[0]*hE[0] - hrhoh[0] + 1.;
        for ( auto i=1 ; i<c.Nx ; ++i ) {
          hrhoc[i] = I*kx[i]*hE[i] - hrhoh[i];
        }
        ublas::vector<double> rhoc (c.Nx,0.); hrhoc.ifft(&rhoc[0]);

        for ( auto i=0 ; i<c.Nx ; ++i ) {
          total_energy += rhoc[i]*uc[i]*uc[i];
        }
      }
      H.push_back( total_energy );

      // increment time
      iter.current_time += iter.dt;
      times.push_back( iter.current_time );
      success_iterations.push_back( iter );
    }
    iterations.push_back( iter );

    ++iter.iter;

    //iter.dt = std::pow( c.tol/iter.Lhfh , 0.25 )*iter.dt;// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    if ( iter.current_time+iter.dt > c.Tf ) { iter.dt = c.Tf - iter.current_time; }
  } // while (  i_t*dt < Tf )
  std::cout << "\r" << time(iter) << std::endl;

  save_data("");

  auto dx_y = [&,count=0](auto const& y) mutable {
    std::stringstream ss; ss<< fh.step.dx*(count++) <<" "<<y;
    return ss.str();
  };
  c << monitoring::data( "E_"+c.name+".dat" , E , dx_y );

  ublas::vector<double> rho  (c.Nx,0.);
  ublas::vector<double> rhoc (c.Nx,0.);
  {
    auto rhoh = fh.density();
    fft::spectrum_ hrhoh(c.Nx); hrhoh.fft(&rhoh[0]);
    fft::spectrum_ hE(c.Nx); hE.fft(&E[0]);
    fft::spectrum_ hrhoc(c.Nx);
    hrhoc[0] = I*kx[0]*hE[0] - hrhoh[0] + 1.;
    for ( auto i=1 ; i<c.Nx ; ++i ) {
      hrhoc[i] = I*kx[i]*hE[i] - hrhoh[i];
    }
    hrhoc.ifft(&rhoc[0]);
    rho = rhoc + rhoh;
  }

  c << monitoring::data( "rho_"+c.name+".dat" , rho , dx_y );
  c << monitoring::data( "uc_"+c.name+".dat"  , uc  , dx_y );

  auto Jh = fh.courant();
  ublas::vector<double> Jc (c.Nx,0.), Jtot (c.Nx,0.);
  for ( auto i=0 ; i<c.Nx ; ++i ) {
    Jc[i] = rhoc[i]*uc[i];
  }
  Jtot = Jc + Jh;
  c << monitoring::data( "J_"+c.name+".dat" , Jtot , dx_y );

  return 0;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_DETAIL_SCALAR_F_INVTRIG_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SCALAR_F_INVTRIG_HPP_INCLUDED

#include <boost/simd/function/horn.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/mhalf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/pio_2.hpp>
#include <boost/simd/detail/constant/pio_2lo.hpp>
#include <boost/simd/constant/pio_3.hpp>
#include <boost/simd/constant/pio_4.hpp>
#include <boost/simd/detail/constant/pio_4lo.hpp>
#include <boost/simd/constant/tan_3pio_8.hpp>
#include <boost/simd/constant/tanpio_8.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/twopio_3.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/sqrt.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bd =  boost::dispatch;
    namespace bs =  boost::simd;

   template < class A0,
             class unit_tag,
             class style,
             class base_A0 = bd::scalar_of_t<A0>
  >
  struct invtrig_base{};

  template < class A0 >
  struct invtrig_base<A0,tag::radian_tag,tag::not_simd_type, float>
  {
    static BOOST_FORCEINLINE A0 asin(A0 a0) BOOST_NOEXCEPT
    {
      A0 sign, x, z;
      x = bs::abs(a0);
      sign = bitofsign(a0);
      if ((x < Constant<A0,0x38d1b717>())) return a0; //1.0e-4
      if ((x >  One<A0>())) return Nan<A0>();
      auto bx_larger_05    = (x > Half<A0>());
      if (bx_larger_05)
      {
        z = Half<A0>()*oneminus(x);
        x =  sqrt(z);
      }
      else
      {
        z = sqr(x);
      }
      A0 z1 = horn<A0,
        0x3e2aaae4,
        0x3d9980f6,
        0x3d3a3ec7,
        0x3cc617e3,
        0x3d2cb352
        > (z);
      z1 = fma(z1, z*x, x);
      if(bx_larger_05)
      {
        z1 = z1+z1;
        z1 = Pio_2<A0>()-z1;
      }
      return bitwise_xor(z1, sign);
    }

    static BOOST_FORCEINLINE A0 acos(const  A0& a0) BOOST_NOEXCEPT
    {
      if (a0 < Mhalf<A0>())
        return Pi<A0>()-asin( sqrt(inc(a0)*Half<A0>()))*Two<A0>();
      else if (a0 > Half<A0>())
        return asin( sqrt(oneminus(a0)*Half<A0>()))*Two<A0>();
      return (Pio_2<A0>()-asin(a0));
    }

    static BOOST_FORCEINLINE A0 atan(A0 a0) BOOST_NOEXCEPT
    {
      A0 x  = abs(a0);
      return bitwise_xor(kernel_atan(x, rec(x)), bitofsign(a0));
    }

    static BOOST_FORCEINLINE A0 acot(A0 a0) BOOST_NOEXCEPT
    {
      A0 x  = abs(a0);
      return bitwise_xor(kernel_atan(rec(x), x), bitofsign(a0));
    }

    static BOOST_FORCEINLINE A0 kernel_atan(A0 x, A0 recx) BOOST_NOEXCEPT
    {
      if (is_eqz(x))  return Zero<A0>();
      if (x == Inf<A0>())  return Pio_2<A0>();
      A0 y = 0.0;
      A0 more = Zero<A0>();
      if( x > Tan_3pio_8<A0>())
      {
        y = Pio_2<A0>();
        more = Pio_2lo<A0>();
        x = -recx;
      }
      else if( x > Tanpio_8<A0>())
      {
        y = Pio_4<A0>();
        more =  Pio_4lo<A0>();
        x = dec(x)/inc(x);
      }
      A0 z = sqr(x);
      A0 z1 = horn<A0
        , 0xbeaaaa2aul  // -3.3333293e-01
        , 0x3e4c925ful  //  1.9991724e-01
        , 0xbe0e1b85ul  // -1.4031009e-01
        , 0x3da4f0d1ul  //  8.5460119e-02
        > (z);
    z1 = fma(x, z1*z, x);

      return y+(z1+more);
    }
  };
}
} }
#endif