// Copyright (C) 2012  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <dlib/svm_threaded.h>
#include <dlib/data_io.h>

#include "tester.h"

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.graph_cuts");

    template <
        typename graph_type,
        typename samples_type,
        typename labels_type
        >
    void make_data(
        samples_type& samples,
        labels_type& labels
    )
    {
        //samples.clear();
        //labels.clear();

        std::vector<bool> label;
        graph_type g;

    // ---------------------------
        g.set_number_of_nodes(4);
        label.resize(g.number_of_nodes());
        g.node(0).data = 0, 0, 1; label[0] = true;
        g.node(1).data = 0, 0, 1; label[1] = true;
        g.node(2).data = 0, 1, 0; label[2] = false;
        g.node(3).data = 0, 1, 0; label[3] = false;

        g.add_edge(0,1);
        g.add_edge(1,2);
        g.add_edge(2,3);
        g.add_edge(3,0);

        edge(g,0,1) = 1, 1;
        edge(g,1,2) = 1, 1;
        edge(g,2,3) = 1, 1;
        edge(g,3,0) = 1, 1;
        samples.push_back(g);
        labels.push_back(label);
    // ---------------------------

        g.clear();
        g.set_number_of_nodes(4);
        label.resize(g.number_of_nodes());
        g.node(0).data = 0, 0, 1; label[0] = true;
        g.node(1).data = 0, 0, 0; label[1] = true;
        g.node(2).data = 0, 1, 0; label[2] = false;
        g.node(3).data = 0, 0, 0; label[3] = false;

        g.add_edge(0,1);
        g.add_edge(1,2);
        g.add_edge(2,3);
        g.add_edge(3,0);

        edge(g,0,1) = 1, 0;
        edge(g,1,2) = 0, 1;
        edge(g,2,3) = 1, 0;
        edge(g,3,0) = 0, 1;
        samples.push_back(g);
        labels.push_back(label);
    // ---------------------------

        g.clear();
        g.set_number_of_nodes(4);
        label.resize(g.number_of_nodes());
        g.node(0).data = 0, 1, 0; label[0] = false;
        g.node(1).data = 0, 1, 0; label[1] = false;
        g.node(2).data = 0, 1, 0; label[2] = false;
        g.node(3).data = 0, 0, 0; label[3] = false;

        g.add_edge(0,1);
        g.add_edge(1,2);
        g.add_edge(2,3);
        g.add_edge(3,0);

        edge(g,0,1) = 1, 0;
        edge(g,1,2) = 0, 1;
        edge(g,2,3) = 1, 0;
        edge(g,3,0) = 0, 1;
        samples.push_back(g);
        labels.push_back(label);
    // ---------------------------
    }

    template <
        typename graph_type,
        typename samples_type,
        typename labels_type
        >
    void make_data_sparse(
        samples_type& samples,
        labels_type& labels
    )
    {
        //samples.clear();
        //labels.clear();

        std::vector<bool> label;
        graph_type g;
        typename graph_type::edge_type v;

    // ---------------------------
        g.set_number_of_nodes(4);
        label.resize(g.number_of_nodes());
        g.node(0).data[2] = 1; label[0] = true;
        g.node(1).data[2] = 1; label[1] = true;
        g.node(2).data[1] = 1; label[2] = false;
        g.node(3).data[1] = 1; label[3] = false;

        g.add_edge(0,1);
        g.add_edge(1,2);
        g.add_edge(2,3);
        g.add_edge(3,0);
        g.add_edge(3,1);

        v[0] = 1; v[1] = 1;
        edge(g,0,1) = v;
        edge(g,1,2) = v;
        edge(g,2,3) = v;
        edge(g,3,0) = v;
        samples.push_back(g);
        labels.push_back(label);
    // ---------------------------

        g.clear();
        g.set_number_of_nodes(5);
        label.resize(g.number_of_nodes());
        g.node(0).data[2] = 1; label[0] = true;
        g.node(1).data[0] = 0; label[1] = true;
        g.node(2).data[1] = 1; label[2] = false;
        g.node(3).data[0] = 0; label[3] = false;
                               label[4] = true;

        g.add_edge(0,1);
        g.add_edge(1,4);
        g.add_edge(1,2);
        g.add_edge(2,3);
        g.add_edge(3,0);

        edge(g,0,1)[0] = 1;
        edge(g,1,4)[0] = 1;
        edge(g,1,2)[1] = 1;
        edge(g,2,3)[0] = 1;
        edge(g,3,0)[1] = 1;
        samples.push_back(g);
        labels.push_back(label);
    // ---------------------------

        g.clear();
        g.set_number_of_nodes(4);
        label.resize(g.number_of_nodes());
        g.node(0).data[1] = 1; label[0] = false;
        g.node(1).data[1] = 1; label[1] = false;
        g.node(2).data[1] = 1; label[2] = false;
        g.node(3).data[1] = 0; label[3] = false;

        g.add_edge(0,1);
        g.add_edge(1,2);
        g.add_edge(2,3);
        g.add_edge(3,0);

        edge(g,0,1)[0] = 1;
        edge(g,1,2)[1] = 1;
        edge(g,2,3)[0] = 1;
        edge(g,3,0)[1] = 1;
        samples.push_back(g);
        labels.push_back(label);
    // ---------------------------
    }

    template <
        typename graph_type,
        typename samples_type,
        typename labels_type
        >
    void make_data2(
        samples_type& samples,
        labels_type& labels
    )
    {
        //samples.clear();
        //labels.clear();

        std::vector<bool> label;
        graph_type g;

    // ---------------------------
        g.set_number_of_nodes(4);
        label.resize(g.number_of_nodes());
        g.node(0).data = 0, 0, 1; label[0] = true;
        g.node(1).data = 0, 0, 1; label[1] = true;
        g.node(2).data = 0, 1, 0; label[2] = false;
        g.node(3).data = 0, 1, 0; label[3] = false;

        g.add_edge(0,1);
        g.add_edge(1,2);
        g.add_edge(2,3);
        g.add_edge(3,0);

        edge(g,0,1) = 1, 1;
        edge(g,1,2) = 1, 1;
        edge(g,2,3) = 1, 1;
        edge(g,3,0) = 1, 1;
        samples.push_back(g);
        labels.push_back(label);
    // ---------------------------
    }

    template <
        typename graph_type,
        typename samples_type,
        typename labels_type
        >
    void make_data2_sparse(
        samples_type& samples,
        labels_type& labels
    )
    {
        //samples.clear();
        //labels.clear();

        std::vector<bool> label;
        graph_type g;
        typename graph_type::edge_type v;

    // ---------------------------
        g.set_number_of_nodes(4);
        label.resize(g.number_of_nodes());
        g.node(0).data[2] = 1; label[0] = true;
        g.node(1).data[2] = 1; label[1] = true;
        g.node(2).data[1] = 1; label[2] = false;
        g.node(3).data[1] = 1; label[3] = false;

        g.add_edge(0,1);
        g.add_edge(1,2);
        g.add_edge(2,3);
        g.add_edge(3,0);

        v[0] = 1; v[1] = 1;
        edge(g,0,1) = v;
        edge(g,1,2) = v;
        edge(g,2,3) = v;
        edge(g,3,0) = v;
        samples.push_back(g);
        labels.push_back(label);
    // ---------------------------

    }

    template <
        typename node_vector_type,
        typename edge_vector_type,
        typename vector_type,
        typename graph_type
        >
    void test1(
        const dlib::array<graph_type>& samples,
        const std::vector<std::vector<bool> >& labels
    )
    {
        dlog << LINFO << "begin test1()";

        structural_graph_labeling_trainer<vector_type> trainer;
        //trainer.be_verbose();
        trainer.set_epsilon(1e-12);
        graph_labeler<vector_type> labeler = trainer.train(samples, labels);

        // test serialization code for the labeler.
        std::ostringstream sout;
        serialize(labeler, sout);
        std::istringstream sin(sout.str());
        labeler = graph_labeler<vector_type>();
        deserialize(labeler, sin);

        std::vector<bool> temp;
        for (unsigned long k = 0; k < samples.size(); ++k)
        {
            temp = labeler(samples[k]);
            for (unsigned long i = 0; i < temp.size(); ++i)
            {
                const bool true_label = (labels[k][i] != 0);
                const bool pred_label = (temp[i] != 0);
                DLIB_TEST(true_label == pred_label);
            }
        }

        matrix<double> cv;

        cv = test_graph_labeling_function(labeler, samples, labels);
        DLIB_TEST(sum(cv) == 2);
        cv = cross_validate_graph_labeling_trainer(trainer, samples, labels, 4);
        DLIB_TEST(sum(cv) == 2);

        dlog << LINFO << "edge weights: " << trans(sparse_to_dense(labeler.get_edge_weights()));
        dlog << LINFO << "node weights: " << trans(sparse_to_dense(labeler.get_node_weights()));
    }

    class graph_labeling_tester : public tester
    {
    public:
        graph_labeling_tester (
        ) :
            tester ("test_graph_labeling",
                    "Runs tests on the graph labeling component.")
        {}

        void perform_test (
        )
        {
            print_spinner();
            // test with dense vectors
            {
                typedef matrix<double,3,1> node_vector_type;
                typedef matrix<double,2,1> edge_vector_type;
                typedef matrix<double,0,1> vector_type;
                typedef dlib::graph<node_vector_type, edge_vector_type>::kernel_1a_c graph_type;

                dlib::array<graph_type> samples;
                std::vector<std::vector<bool> > labels;

                make_data<graph_type>(samples, labels);
                make_data<graph_type>(samples, labels);
                make_data<graph_type>(samples, labels);
                make_data<graph_type>(samples, labels);

                test1<node_vector_type,edge_vector_type,vector_type>(samples, labels);
            }
            print_spinner();
            // test with dense vectors and sparse vectors together
            {
                typedef matrix<double,3,1> node_vector_type;
                typedef matrix<double,2,1> edge_vector_type;
                typedef std::map<unsigned long,double> vector_type;
                typedef dlib::graph<node_vector_type, edge_vector_type>::kernel_1a_c graph_type;

                dlib::array<graph_type> samples;
                std::vector<std::vector<bool> > labels;

                make_data<graph_type>(samples, labels);
                make_data<graph_type>(samples, labels);
                make_data<graph_type>(samples, labels);
                make_data<graph_type>(samples, labels);

                test1<node_vector_type,edge_vector_type,vector_type>(samples, labels);
            }
            print_spinner();
            // test with sparse vectors
            {
                typedef std::vector<std::pair<unsigned long,double> > vector_type;
                typedef std::map<unsigned long, double> edge_vector_type;
                typedef std::map<unsigned long, double> node_vector_type;
                typedef dlib::graph<node_vector_type, edge_vector_type>::kernel_1a_c graph_type;

                dlib::array<graph_type> samples;
                std::vector<std::vector<bool> > labels;

                make_data_sparse<graph_type>(samples, labels);
                make_data_sparse<graph_type>(samples, labels);
                make_data_sparse<graph_type>(samples, labels);
                make_data_sparse<graph_type>(samples, labels);

                test1<node_vector_type,edge_vector_type,vector_type>(samples, labels);
            }

            print_spinner();
            // test with dense vectors
            {
                typedef matrix<double,3,1> node_vector_type;
                typedef matrix<double,2,1> edge_vector_type;
                typedef matrix<double,0,1> vector_type;
                typedef dlib::graph<node_vector_type, edge_vector_type>::kernel_1a_c graph_type;

                dlib::array<graph_type> samples;
                std::vector<std::vector<bool> > labels;

                make_data2<graph_type>(samples, labels);
                make_data2<graph_type>(samples, labels);
                make_data2<graph_type>(samples, labels);
                make_data2<graph_type>(samples, labels);

                test1<node_vector_type,edge_vector_type,vector_type>(samples, labels);
            }
            print_spinner();
            // test with sparse vectors
            {
                typedef std::vector<std::pair<unsigned long,double> > vector_type;
                typedef std::map<unsigned long, double> edge_vector_type;
                typedef std::map<unsigned long, double> node_vector_type;
                typedef dlib::graph<node_vector_type, edge_vector_type>::kernel_1a_c graph_type;

                dlib::array<graph_type> samples;
                std::vector<std::vector<bool> > labels;

                make_data2_sparse<graph_type>(samples, labels);
                make_data2_sparse<graph_type>(samples, labels);
                make_data2_sparse<graph_type>(samples, labels);
                make_data2_sparse<graph_type>(samples, labels);

                test1<node_vector_type,edge_vector_type,vector_type>(samples, labels);
            }
            print_spinner();
            // test with sparse vectors and dense mix
            {
                typedef matrix<double,0,1> vector_type;
                typedef std::map<unsigned long, double> edge_vector_type;
                typedef std::map<unsigned long, double> node_vector_type;
                typedef dlib::graph<node_vector_type, edge_vector_type>::kernel_1a_c graph_type;

                dlib::array<graph_type> samples;
                std::vector<std::vector<bool> > labels;

                make_data2_sparse<graph_type>(samples, labels);
                make_data2_sparse<graph_type>(samples, labels);
                make_data2_sparse<graph_type>(samples, labels);
                make_data2_sparse<graph_type>(samples, labels);

                test1<node_vector_type,edge_vector_type,vector_type>(samples, labels);
            }
        }
    } a;

}


// 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/least_unsquared_deviation_position_estimator.h"

#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <Eigen/Core>
#include <unordered_map>

#include "theia/sfm/global_pose_estimation/pairwise_translation_and_scale_error.h"
#include "theia/sfm/types.h"
#include "theia/util/map_util.h"
#include "theia/util/util.h"

namespace theia {
namespace {

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

Vector3d GetRotatedTranslation(const Vector3d& rotation_angle_axis,
                               const Vector3d& translation) {
  Matrix3d rotation;
  ceres::AngleAxisToRotationMatrix(
      rotation_angle_axis.data(),
      ceres::ColumnMajorAdapter3x3(rotation.data()));
  return rotation.transpose() * translation;
}

}  // namespace

LeastUnsquaredDeviationPositionEstimator::
    LeastUnsquaredDeviationPositionEstimator(
        const LeastUnsquaredDeviationPositionEstimator::Options& options)
    : options_(options) {
  CHECK_GT(options_.num_threads, 0);
}

bool LeastUnsquaredDeviationPositionEstimator::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);
  view_pairs_ = &view_pairs;

  // Initialize scale and weight terms.
  scales_.reserve(view_pairs_->size());
  weights_.reserve(view_pairs_->size());
  for (const auto& view_pair : *view_pairs_) {
    scales_[view_pair.first] = 10.0;
    weights_[view_pair.first] = 1.0;
  }

  if (options_.initialize_random_positions) {
    // Initialize positions to be random.
    InitializeRandomPositions(orientations, positions);
  }

  ceres::Solver::Summary summary;
  for (int i = 0; i < options_.max_num_reweighted_iterations; i++) {
    problem_.reset(new ceres::Problem());

    // Add the (weighted) constraints to the problem.
    AddCameraToCameraConstraints(orientations, positions);

    // Set one camera to be at the origin to remove the ambiguity of the origin.
    positions->begin()->second.setZero();
    problem_->SetParameterBlockConstant(positions->begin()->second.data());

    // Set the solver options.
    solver_options_.num_threads = options_.num_threads;
    solver_options_.num_linear_solver_threads = options_.num_threads;
    solver_options_.max_num_iterations = options_.max_num_iterations;
    solver_options_.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;

    ceres::Solve(solver_options_, problem_.get(), &summary);
    LOG(INFO) << "Iteration " << i + 1
              << " of the Iterative Reweighted Least Squares\n"
              << summary.FullReport();

    const double convergence_criterion =
        (summary.initial_cost - summary.final_cost) / summary.initial_cost;
    if (convergence_criterion < options_.convergence_criterion) {
      break;
    }

    ComputeWeights(orientations, *positions);
  }

  return summary.IsSolutionUsable();
}

void LeastUnsquaredDeviationPositionEstimator::InitializeRandomPositions(
    const std::unordered_map<ViewId, Vector3d>& orientations,
    std::unordered_map<ViewId, Vector3d>* positions) {
  std::unordered_set<ViewId> constrained_positions;
  constrained_positions.reserve(orientations.size());
  for (const auto& view_pair : *view_pairs_) {
    constrained_positions.insert(view_pair.first.first);
    constrained_positions.insert(view_pair.first.second);
  }

  positions->reserve(orientations.size());
  for (const auto& orientation : orientations) {
    if (ContainsKey(constrained_positions, orientation.first)) {
      (*positions)[orientation.first] = 100.0 * Vector3d::Random();
    }
  }
}

void LeastUnsquaredDeviationPositionEstimator::AddCameraToCameraConstraints(
    const std::unordered_map<ViewId, Vector3d>& orientations,
    std::unordered_map<ViewId, Vector3d>* positions) {
  // Add the camera to camera constraints.
  for (const auto& view_pair : *view_pairs_) {
    const ViewId view_id1 = view_pair.first.first;
    const ViewId view_id2 = view_pair.first.second;
    Vector3d* position1 = FindOrNull(*positions, view_id1);
    Vector3d* position2 = FindOrNull(*positions, view_id2);

    // Do not add this view pair if one or both of the positions do not exist.
    if (position1 == nullptr || position2 == nullptr) {
      continue;
    }

    // Rotate the relative translation so that it is aligned to the global
    // orientation frame.
    const Vector3d translation_direction = GetRotatedTranslation(
        FindOrDie(orientations, view_id1), view_pair.second.position_2);

    ceres::CostFunction* cost_function =
        PairwiseTranslationAndScaleError::Create(translation_direction);

    double* scale = FindOrNull(scales_, view_pair.first);
    const double weight = FindOrDieNoPrint(weights_, view_pair.first);
    problem_->AddResidualBlock(
        cost_function,
        new ceres::ScaledLoss(NULL, weight, ceres::TAKE_OWNERSHIP),
        position1->data(),
        position2->data(),
        scale);
    problem_->SetParameterLowerBound(scale, 0, 1.0);
  }

  VLOG(2) << problem_->NumResidualBlocks() << " camera to camera constraints "
                                              "were added to the position "
                                              "estimation problem.";
}

void LeastUnsquaredDeviationPositionEstimator::ComputeWeights(
    const std::unordered_map<ViewId, Vector3d>& orientations,
    const std::unordered_map<ViewId, Vector3d>& positions) {
  const double gamma = 1e-8;
  // Add the camera to camera constraints.
  for (const auto& view_pair : *view_pairs_) {
    const ViewId view_id1 = view_pair.first.first;
    const ViewId view_id2 = view_pair.first.second;
    const Vector3d* position1 = FindOrNull(positions, view_id1);
    const Vector3d* position2 = FindOrNull(positions, view_id2);

    // Do not add this view pair if one or both of the positions do not exist.
    if (position1 == nullptr || position2 == nullptr) {
      continue;
    }

    // Rotate the relative translation so that it is aligned to the global
    // orientation frame.
    const Vector3d translation_direction = GetRotatedTranslation(
        FindOrDie(orientations, view_id1), view_pair.second.position_2);
    const double scale = FindOrDieNoPrint(scales_, view_pair.first);
    const double error =
        (*position2 - *position1 - scale * translation_direction).squaredNorm();
    weights_[view_pair.first] = std::sqrt(1.0 / (error + gamma));
  }
}

}  // namespace theia


/**
 * \ file AllPassReverbFilter.cpp
 */

#include <ATK/Reverberation/AllPassReverbFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#include <gtest/gtest.h>

#include <boost/math/constants/constants.hpp>

constexpr gsl::index PROCESSSIZE = 1024*64;

TEST(AllPassReverbFilter, sinus_delay_test)
{
  ATK::AllPassReverbFilter<double> filter(128);
  filter.set_delay(10);
  ASSERT_EQ(filter.get_delay(), 10);
}

TEST(AllPassReverbFilter, sinus_delay_range_test)
{
  ATK::AllPassReverbFilter<double> filter(128);
  ASSERT_THROW(filter.set_delay(0), ATK::RuntimeError);
}

TEST(AllPassReverbFilter, sinus_delay_range2_test)
{
  ATK::AllPassReverbFilter<double> filter(128);
  ASSERT_THROW(filter.set_delay(128), ATK::RuntimeError);
}

TEST(AllPassReverbFilter, sinus_feedback_test)
{
  ATK::AllPassReverbFilter<double> filter(128);
  filter.set_feedback(0.5);
  ASSERT_EQ(filter.get_feedback(), 0.5);
}

TEST(AllPassReverbFilter, sinus_feedback_range_test)
{
  ATK::AllPassReverbFilter<double> filter(128);
  ASSERT_THROW(filter.set_feedback(1), ATK::RuntimeError);
}

TEST(AllPassReverbFilter, sinus_feedback_range2_test)
{
  ATK::AllPassReverbFilter<double> filter(128);
  ASSERT_THROW(filter.set_feedback(-1), ATK::RuntimeError);
}

namespace
{
  void compare(double v1, double v2)
  {
    if(std::abs(v1) < 1e-5 || std::abs(v2) < 1e-5)
    {
      ASSERT_NEAR(v1, 0, static_cast<double>(1e-5));
      ASSERT_NEAR(v2, 0, static_cast<double>(1e-5));
    }
    else
    {
      ASSERT_NEAR(v1, v2, 0.001);
    }
  }
}

TEST(AllPassReverbFilter, sinus_line100_delay50_test)
{
  std::vector<double> data(PROCESSSIZE);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<double>() * (i+1.)/48000 * 1000);
  }
  std::vector<double> ref(PROCESSSIZE);
  for(int64_t i = 50; i < PROCESSSIZE; ++i)
  {
    ref[i] = data[i - 50];
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<double> outdata(PROCESSSIZE);

  ATK::AllPassReverbFilter<double> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(50);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    compare(outdata[i], ref[i]);
  }
}

TEST(AllPassReverbFilter, sinus_line25_delay24_feedback_0_5_test)
{
  std::vector<double> data(PROCESSSIZE);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<double>() * (i+1.)/48000 * 1000);
  }
  std::vector<double> ref(PROCESSSIZE);
  for(int64_t i = 0; i < 24; ++i)
  {
    ref[i] = -0.5*data[i];
  }
  for(int64_t i = 24; i < PROCESSSIZE; ++i)
  {
    ref[i] = 0.5 * (ref[i - 24] - data[i]) + data[i - 24];
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<double> outdata(PROCESSSIZE);

  ATK::AllPassReverbFilter<double> filter(25);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(24);
  filter.set_feedback(0.5);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    compare(outdata[i], ref[i]);
  }
}

TEST(AllPassReverbFilter, sinus_line25_delay24_feedback_1_test)
{
  std::vector<double> data(PROCESSSIZE);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<double>() * (i+1.)/48000 * 1000);
  }
  std::vector<double> ref(PROCESSSIZE);
  for(int64_t i = 0; i < 24; ++i)
  {
    ref[i] = -data[i];
  }
  for(int64_t i = 24; i < PROCESSSIZE; ++i)
  {
    ref[i] = (ref[i - 24] - data[i]) + data[i - 24];
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<double> outdata(PROCESSSIZE);

  ATK::AllPassReverbFilter<double> filter(25);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(24);
  filter.set_feedback(.999999);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    compare(outdata[i], ref[i]);
  }
}


#include <iostream>
#include <iomanip>
#include <vector>
#include <array>
#include <limits>
#include <algorithm>
#include <boost/numeric/odeint.hpp>

#include <GeographicLib/Math.hpp>
#include <GeographicLib/Utility.hpp>
#include <GeographicLib/NormalGravity.hpp>

using namespace GeographicLib;
namespace ode = boost::numeric::odeint;
typedef Math::real real;
typedef std::array<real, 2> point;

// Initialize with the end points of a line
class LineDistance {
  point _a, _b;
  real _l, _nx, _ny;
public:
  LineDistance(const point& a, const point& b);
  real Distance(const point& p) const;
  // negative on left of line, pos on right
  real Displacement(const point& p) const;
};

// Use Ramer-Douglas-Peucker to simplify a polyline
class LineSimplifier {
  // Simplify range [a,b]
  static void InternalSimplify(std::vector<point>& p, unsigned a, unsigned b,
                               real thresh);
public:
  static void Simplify(std::vector<point>& p, real thresh);
};

inline LineDistance::LineDistance(const point& a, const point& b)
  : _a(a), _b(b) {
  using std::hypot;
  real dx = _b[0] - _a[0], dy = _b[1] - _a[1];
  _l = hypot(dx, dy);
  if (_l > std::numeric_limits<real>::epsilon()) {
  _nx = dx / _l; _ny = dy / _l;
  } else {
    _l = 0; _nx = 1; _ny = 0;
  }
}

inline real LineDistance::Distance(const point& p) const {
  using std::abs; using std::hypot;
  real x = p[0] - _a[0], y = p[1] - _a[1];
  if (_l != 0) {
    real X = x * _nx + y * _ny, Y = abs(x * _ny - y * _nx);
    X = X < 0 ? -X : (X > _l ? X - _l : 0);
    return X != 0 ? hypot(X, Y) : Y;
  } else
    return hypot(x, y);
}

inline real LineDistance::Displacement(const point& p) const {
  real x = p[0] - _a[0], y = p[1] - _a[1];
  return x * _ny - y * _nx;
}

inline void LineSimplifier::Simplify(std::vector<point>& p, real thresh) {
  using std::isnan;
  unsigned n = p.size();
  InternalSimplify(p, 0, n-1, thresh);
  unsigned i = 0, j = 0;
  while (i < n) {               // Squeeze out nans
    if (!isnan(p[i][0])) { p[j] = p[i]; ++j; }
    ++i;
  }
  p.resize(j);
}

void LineSimplifier::InternalSimplify(std::vector<point>& p,
                                      unsigned a, unsigned b,
                                      real thresh) {
  if (b - a + 1 <= 2) return;
  // Assume b > a+1, thresh > 0
  real maxdist = -1;
  unsigned maxind = (a + b) / 2;         // initial value isn't used
  LineDistance dist(p[a], p[b]);
  for (unsigned i = a + 1; i <= b - 1; ++i) {
    real d = dist.Distance(p[i]);
    if (d > maxdist) { maxdist = d; maxind = i; }
  }
  if (maxdist > thresh) {
    InternalSimplify(p, a, maxind, thresh);
    InternalSimplify(p, maxind, b, thresh);
  } else {
    for (unsigned i = a+1; i <= b-1; ++i)
      p[i][0] = Math::NaN();
  }
}

class PointTest {
  real _xmin, _xmax, _ymin, _ymax;
public:
  PointTest(real xmin, real xmax, real ymin, real ymax)
    : _xmin(xmin)
    , _xmax(xmax)
    , _ymin(ymin)
    , _ymax(ymax) {}
  bool operator() (const point &p) const {
    return p[0] >= _xmin && p[0] <= _xmax && p[1] >= _ymin && p[1] <= _ymax;
  }
};

class GravityInt {
  const NormalGravity& _grav;
  unsigned _dir;
public:
  GravityInt(const NormalGravity& grav, unsigned dir)
    : _grav(grav)
    , _dir(dir & 3u)
  {}
  void operator() (const point& p, point &d, const real /*t*/) const {
    real Y = 0, gX, gY, gZ;
    _grav.U(p[0], Y, p[1], gX, gY, gZ);
    Math::norm(gX, gZ);
    switch (_dir) {
    case  0: d[0] =  gX; d[1] =  gZ; break; // parallel to g
    case  1: d[0] = -gZ; d[1] =  gX; break; // counterclockwise 90d from g
    case  2: d[0] = -gX; d[1] = -gZ; break; // antiparallel to g
    case  3:
    default: d[0] =  gZ; d[1] = -gX; break; // clockwise 90d from g
    }
  }
};

class GravityFollow {
public:
  static void follow(const GravityInt& sys, const point& p0, real t0, real ds,
                     std::vector<point>& points, const PointTest& box) {
    ode::result_of::make_dense_output
      < ode::runge_kutta_dopri5< point, real > >::type integrator =
      ode::make_dense_output(real(1.0e-8), real(0*1.0e-8),
                             ode::runge_kutta_dopri5< point, real >() );
    integrator.initialize(p0, t0, real(1e-2));
    integrator.do_step(sys);
    int n = 1, i = 1;
    point out;
    while (true) {
      real tout = i * ds;
      while (integrator.current_time() < tout) {
        integrator.do_step(sys); ++n;
      }
      integrator.calc_state(tout, out);
      points.push_back(out);
      if (!box(out)) break;
      ++i;
    }
  }
};

// Evaluate gravity potential or gradient along 1 axis
class GravityEval {
  const NormalGravity& _grav;
  bool _zaxis, _grad;
  real _disp;
public:
  GravityEval(const NormalGravity& grav, bool zaxis, bool grad, real disp)
    : _grav(grav)
    , _zaxis(zaxis)
    , _grad(grad)
    , _disp(disp) {};
  real operator() (real v) const {
    real
      X = _zaxis ? _disp : v,
      Y = 0,
      Z = _zaxis ? v : _disp,
      gX, gY, gZ,
      U = _grav.U(X, Y, Z, gX, gY, gZ);
    return _grad ? gX : U;
  }
};

class Interpolator {
private:
  int _maxit;
  real _eps;
public:
  Interpolator()
    : _maxit(20), _eps(sqrt(std::numeric_limits<real>::epsilon())) {}
  real operator() (const GravityEval& gravfun,
                   real x0, real x1, real val) {
    using std::abs;
    real y0 = gravfun(x0) - val, y1;
    for (int i = 0, trip = 0; i < _maxit; ++i) {
      y1 = gravfun(x1) - val;
      if (y1 == y0) break;
      real x2 = x1 - y1 * (x1 - x0) / (y1 - y0);
      x0 = x1; x1 = x2; y0 = y1;
      if (abs(x0 - x1) <= _eps) ++trip;
      if (trip > 2) break;
    }
    return x1;
  }
};

void dump(const std::vector<point>& points) {
  for (auto p = points.begin(); p != points.end(); ++p) {
    std::cout << (*p)[0] << "," << (*p)[1] << ";\n";
  }
}

int main() {
  Utility::set_digits();
  using std::sqrt;
  real a = 1, GM = 1, omega = 3/Math::real(10), f = 2/Math::real(10);
  real gX, gY, gZ, b = (1 - f) * a, E = a * sqrt(f * (2 - f));
  real thresh = real(0.5e-3);
  NormalGravity grav(a, GM, omega, f, true);
  real xmax = 3, ymax = 2;
  PointTest box(0, xmax, 0, ymax);
  Interpolator intpol;
  real
    X0 = a,
    U0 = grav.U(X0, 0, 0, gX, gY, gZ),
    Uc = grav.U(0, 0, 0, gX, gY, gZ);
  real Xnull, Unull;
  {
    GravityEval eval(grav, false, true, 0);
    Xnull = intpol(eval, 1, 2, 0);
    Unull = grav.U(Xnull, 0, 0, gX, gY, gZ);
  }
  int ndiv = 20;
  real del = (U0 - Unull) / 20;
  std::cout << std::setprecision(6);
  std::cout << "a=" << a << "; b=" << b << "; Rnull=" << Xnull
            << "; xmax=" << xmax << "; ymax=" << ymax << ";\n";
  std::cout << "q={}; p={}; qa={}; pa={};\n";
  std::vector<point> points;
  point p0;
  int k = 0;
  for (int i = 0; i <= 90; i += 10) {
    points.clear();
    real s, c;
    Math::sincosd(real(i), s, c);
    p0[0] = a * c; p0[1] = b * s;
    points.push_back(p0);
    if (i == 0) {
      p0[0] = xmax; p0[1] = 0;
      points.push_back(p0);
    } else if (i == 90) {
      p0[0] = 0; p0[1] = ymax;
      points.push_back(p0);
    } else {
      GravityInt sys(grav, 2u);
      GravityFollow::follow(sys, p0, real(0), 1/real(100), points, box);
    }
    LineSimplifier::Simplify(points, thresh);
    std::cout << "q{" << ++k << "}=[\n";
    dump(points);
    std::cout << "];\n";
  }
  {
    points.clear();
    p0[0] = Xnull; p0[1] = 0;
    points.push_back(p0);
    p0[0] = Xnull; p0[1] = real(0.001);
    points.push_back(p0);
    GravityInt sys(grav, 2u);
    GravityFollow::follow(sys, p0, real(0), 1/real(100), points, box);
    LineSimplifier::Simplify(points, thresh);
    std::cout << "q{" << ++k << "}=[\n";
    dump(points);
    std::cout << "];\n";
  }
  for (int i = 1; i <= 5; ++i) {
    points.clear();
    p0[0] = xmax;
    p0[1] = real(i == 1 ? 0.45 :
                 (i == 2 ? 0.9 :
                  (i == 3 ? 1.25 :
                   (i == 4 ? 1.6 : 1.9))));
    points.push_back(p0);
    if (!box(p0)) break;
    GravityInt sys(grav, 2u);
    GravityFollow::follow(sys, p0, real(0), 1/real(100), points, box);
    LineSimplifier::Simplify(points, thresh);
    std::cout << "q{" << ++k << "}=[\n";
    dump(points);
    std::cout << "];\n";
  }
  k = 0;
  for (int i = 0; i <= 90; i += 10) {
    points.clear();
    real s, c;
    Math::sincosd(real(i), s, c);
    p0[0] = a * c; p0[1] = b * s;
    points.push_back(p0);
    if (i == 0) {
      p0[0] = E; p0[1] = 0;
      points.push_back(p0);
    } else if (i == 90) {
      p0[0] = 0; p0[1] = 0;
      points.push_back(p0);
    } else {
      GravityInt sys(grav, 0u);
      GravityFollow::follow(sys, p0, real(0), 1/real(100), points, box);
    }
    LineSimplifier::Simplify(points, thresh);
    std::cout << "qa{" << ++k << "}=[\n";
    dump(points);
    std::cout << "];\n";
  }
  k = 0;
  {
    GravityEval eval(grav, false, false, 0);
    for (int i = 0; i < ndiv; ++i) {
      points.clear();
      real U = U0 - del * i;
      p0[0] = intpol(eval, X0, Xnull, U); p0[1] = 0;
      points.push_back(p0);
      GravityInt sysa(grav, 3u);
      GravityFollow::follow(sysa, p0, real(0), 1/real(100), points, box);
      LineSimplifier::Simplify(points, thresh);
      std::cout << "p{" << ++k << "}=[\n";
      dump(points);
      std::cout << "];\n";
    }
    for (int i = ndiv - 1;; --i) {
      points.clear();
      real U = U0 - del * i;
      p0[0] = intpol(eval, Xnull + 3, Xnull, U); p0[1] = 0;
      if (!box(p0)) break;
      points.push_back(p0);
      GravityInt sysa(grav, 1u);
      GravityFollow::follow(sysa, p0, real(0), 1/real(100), points, box);
      LineSimplifier::Simplify(points, thresh);
      std::cout << "p{" << ++k << "}=[\n";
      dump(points);
      std::cout << "];\n";
    }
  }
  {
    GravityEval eval(grav, true, false, 0);
    for (int i = ndiv + 1;; ++i) {
      points.clear();
      real U = U0 - del * i;
      p0[1] = intpol(eval, X0, Xnull, U); p0[0] = 0;
      if (!box(p0)) break;
      points.push_back(p0);
      GravityInt sysa(grav, 1u);
      GravityFollow::follow(sysa, p0, real(0), 1/real(100), points, box);
      LineSimplifier::Simplify(points, thresh);
      std::cout << "p{" << ++k << "}=[\n";
      dump(points);
      std::cout << "];\n";
    }
  }
  {
    real dy = real(0.02);
    GravityEval eval(grav, false, false, dy);
    points.clear();
    p0[0] = Xnull; p0[1] = 0;
    points.push_back(p0);
    p0[0] = intpol(eval, Xnull - real(0.1), Xnull, Unull); p0[1] = dy;
    points.push_back(p0);
    GravityInt sysa(grav, 3u);
    GravityFollow::follow(sysa, p0, real(0), 1/real(100), points, box);
    LineSimplifier::Simplify(points, thresh);
    std::cout << "p{" << ++k << "}=[\n";
    dump(points);
    std::cout << "];\n";
  }
  {
    real dy = real(0.02);
    GravityEval eval(grav, false, false, dy);
    points.clear();
    p0[0] = Xnull; p0[1] = 0;
    points.push_back(p0);
    p0[0] = intpol(eval, Xnull + real(0.1), Xnull, Unull); p0[1] = dy;
    points.push_back(p0);
    GravityInt sysa(grav, 1u);
    GravityFollow::follow(sysa, p0, real(0), 1/real(100), points, box);
    LineSimplifier::Simplify(points, thresh);
    std::cout << "p{" << ++k << "}=[\n";
    dump(points);
    std::cout << "];\n";
  }
  k = 0;
  {
    GravityEval eval(grav, false, false, 0);
    for (int i = 1;; ++i) {
      points.clear();
      real U = U0 + 5 * del * i;
      if (U > Uc) break;
      p0[0] = intpol(eval, 0, X0, U); p0[1] = 0;
      points.push_back(p0);
      GravityInt sysa(grav, 3u);
      GravityFollow::follow(sysa, p0, real(0), 1/real(100), points, box);
      LineSimplifier::Simplify(points, thresh);
      std::cout << "pa{" << ++k << "}=[\n";
      dump(points);
      std::cout << "];\n";
    }
  }
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_TWO_ADD_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_TWO_ADD_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/two_add.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/simd/include/functions/simd/is_inf.hpp>
#include <boost/simd/include/functions/simd/if_zero_else.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_add_, tag::cpu_,
                          (A0)(X),
                          ((simd_<arithmetic_<A0>,X>))
                          ((simd_<arithmetic_<A0>,X>))
                          ((simd_<arithmetic_<A0>,X>))
                          ((simd_<arithmetic_<A0>,X>))
                         )
  {
    typedef int result_type;
    inline result_type operator()(A0 const& a,A0 const& b,
                                  A0 & r0,A0 & r1) const
    {
      r0 = a+b;
      A0 z = (r0-a);
      r1 =  if_zero_else(is_inf(r0), (a-(r0-z))+(b-z));
      return 0;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_add_, tag::cpu_,
                          (A0)(X),
                          ((simd_<arithmetic_<A0>,X>))
                          ((simd_<arithmetic_<A0>,X>))
                          ((simd_<arithmetic_<A0>,X>))
                         )
  {
    typedef A0 result_type;
    inline result_type operator()(A0 const& a0,A0 const& a1,A0 & a3) const
    {
      A0 a2;
      two_add(a0, a1, a2, a3);
      return a2;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_add_, tag::cpu_,
                          (A0)(X),
                          ((simd_<arithmetic_<A0>,X>))
                          ((simd_<arithmetic_<A0>,X>))
                         )
  {
    typedef typename boost::fusion::tuple<A0, A0>        result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      result_type res;
      two_add(a0,a1, boost::fusion::at_c<0>(res),boost::fusion::at_c<1>(res));
      return res;
    }

  };
} } }
#endif


#include <ipc/friction/friction.hpp>

#include <tbb/parallel_for.h>
#include <tbb/blocked_range.h>
#include <tbb/enumerable_thread_specific.h>

#include <Eigen/Sparse>

#include <ipc/barrier/barrier.hpp>
#include <ipc/distance/edge_edge.hpp>
#include <ipc/distance/edge_edge_mollifier.hpp>
#include <ipc/distance/point_edge.hpp>
#include <ipc/distance/point_triangle.hpp>
#include <ipc/friction/closest_point.hpp>
#include <ipc/friction/normal_force_magnitude.hpp>
#include <ipc/friction/relative_displacement.hpp>
#include <ipc/friction/tangent_basis.hpp>
#include <ipc/utils/eigen_ext.hpp>
#include <ipc/utils/local_to_global.hpp>

namespace ipc {

void construct_friction_constraint_set(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Constraints& contact_constraint_set,
    double dhat,
    double barrier_stiffness,
    double mu,
    FrictionConstraints& friction_constraint_set)
{
    return construct_friction_constraint_set(
        V, E, F, contact_constraint_set, dhat, barrier_stiffness,
        Eigen::VectorXd::Constant(V.rows(), mu), friction_constraint_set);
}

void construct_friction_constraint_set(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Constraints& contact_constraint_set,
    double dhat,
    double barrier_stiffness,
    const Eigen::VectorXd& mus,
    FrictionConstraints& friction_constraint_set)
{
    return construct_friction_constraint_set(
        V, E, F, contact_constraint_set, dhat, barrier_stiffness, mus,
        [](double mu0, double mu1) { return (mu0 + mu1) / 2; },
        // [](double mu0, double mu1) { return mu0 * mu1; },
        // [](double mu0, double mu1) { return std::min(mu0, mu1); },
        // [](double mu0, double mu1) { return std::max(mu0, mu1); },
        friction_constraint_set);
}

void construct_friction_constraint_set(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Constraints& contact_constraint_set,
    double dhat,
    double barrier_stiffness,
    const Eigen::VectorXd& mus,
    const std::function<double(double, double)>& blend_mu,
    FrictionConstraints& friction_constraint_set)
{
    assert(mus.size() == V.rows());

    friction_constraint_set.clear();

    friction_constraint_set.vv_constraints.reserve(
        contact_constraint_set.vv_constraints.size());
    for (const auto& vv_constraint : contact_constraint_set.vv_constraints) {
        const auto& p0 = V.row(vv_constraint.vertex0_index);
        const auto& p1 = V.row(vv_constraint.vertex1_index);

        friction_constraint_set.vv_constraints.emplace_back(vv_constraint);
        // Do not initialize closest point because it is trivial
        friction_constraint_set.vv_constraints.back().tangent_basis =
            point_point_tangent_basis(p0, p1);
        friction_constraint_set.vv_constraints.back().normal_force_magnitude =
            compute_normal_force_magnitude(
                point_point_distance(p0, p1), dhat, barrier_stiffness,
                vv_constraint.minimum_distance);
        friction_constraint_set.vv_constraints.back().mu = blend_mu(
            mus(vv_constraint.vertex0_index), mus(vv_constraint.vertex1_index));
    }

    friction_constraint_set.ev_constraints.reserve(
        contact_constraint_set.ev_constraints.size());
    for (const auto& ev_constraint : contact_constraint_set.ev_constraints) {
        const long& vi = ev_constraint.vertex_index;
        const long& e0i = E(ev_constraint.edge_index, 0);
        const long& e1i = E(ev_constraint.edge_index, 1);
        const auto &p = V.row(vi), &e0 = V.row(e0i), &e1 = V.row(e1i);

        friction_constraint_set.ev_constraints.emplace_back(ev_constraint);

        friction_constraint_set.ev_constraints.back().closest_point.resize(1);
        double alpha = point_edge_closest_point(p, e0, e1);
        friction_constraint_set.ev_constraints.back().closest_point[0] = alpha;
        friction_constraint_set.ev_constraints.back().tangent_basis =
            point_edge_tangent_basis(p, e0, e1);
        friction_constraint_set.ev_constraints.back().normal_force_magnitude =
            compute_normal_force_magnitude(
                point_edge_distance(p, e0, e1, PointEdgeDistanceType::P_E),
                dhat, barrier_stiffness, ev_constraint.minimum_distance);
        double edge_mu = (mus(e1i) - mus(e0i)) * alpha + mus(e0i);
        friction_constraint_set.ev_constraints.back().mu =
            blend_mu(edge_mu, mus(vi));
    }

    friction_constraint_set.ee_constraints.reserve(
        contact_constraint_set.ee_constraints.size());
    for (const auto& ee_constraint : contact_constraint_set.ee_constraints) {
        const long& ea0i = E(ee_constraint.edge0_index, 0);
        const long& ea1i = E(ee_constraint.edge0_index, 1);
        const long& eb0i = E(ee_constraint.edge1_index, 0);
        const long& eb1i = E(ee_constraint.edge1_index, 1);
        const auto& ea0 = V.row(ea0i);
        const auto& ea1 = V.row(ea1i);
        const auto& eb0 = V.row(eb0i);
        const auto& eb1 = V.row(eb1i);

        // Skip EE constraints that are close to parallel
        if (edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1)
            < ee_constraint.eps_x) {
            continue;
        }

        friction_constraint_set.ee_constraints.emplace_back(ee_constraint);

        Eigen::Vector2d alphas = edge_edge_closest_point(ea0, ea1, eb0, eb1);
        friction_constraint_set.ee_constraints.back().closest_point = alphas;
        friction_constraint_set.ee_constraints.back().tangent_basis =
            edge_edge_tangent_basis(ea0, ea1, eb0, eb1);
        friction_constraint_set.ee_constraints.back().normal_force_magnitude =
            compute_normal_force_magnitude(
                // The distance type is known because mollified PP and PE were
                // skipped above.
                edge_edge_distance(
                    ea0, ea1, eb0, eb1, EdgeEdgeDistanceType::EA_EB),
                dhat, barrier_stiffness, ee_constraint.minimum_distance);
        double ea_mu = (mus(ea1i) - mus(ea0i)) * alphas[0] + mus(ea0i);
        double eb_mu = (mus(eb1i) - mus(eb0i)) * alphas[1] + mus(eb0i);
        friction_constraint_set.ee_constraints.back().mu =
            blend_mu(ea_mu, eb_mu);
    }

    friction_constraint_set.fv_constraints.reserve(
        contact_constraint_set.fv_constraints.size());
    for (const auto& fv_constraint : contact_constraint_set.fv_constraints) {
        const long& vi = fv_constraint.vertex_index;
        const long& f0i = F(fv_constraint.face_index, 0);
        const long& f1i = F(fv_constraint.face_index, 1);
        const long& f2i = F(fv_constraint.face_index, 2);
        const auto& p = V.row(vi);
        const auto& t0 = V.row(f0i);
        const auto& t1 = V.row(f1i);
        const auto& t2 = V.row(f2i);

        friction_constraint_set.fv_constraints.emplace_back(fv_constraint);

        Eigen::Vector2d bc = point_triangle_closest_point(p, t0, t1, t2);
        friction_constraint_set.fv_constraints.back().closest_point = bc;
        friction_constraint_set.fv_constraints.back().tangent_basis =
            point_triangle_tangent_basis(p, t0, t1, t2);
        friction_constraint_set.fv_constraints.back().normal_force_magnitude =
            compute_normal_force_magnitude(
                point_triangle_distance(
                    p, t0, t1, t2, PointTriangleDistanceType::P_T),
                dhat, barrier_stiffness, fv_constraint.minimum_distance);
        double face_mu = mus(f0i) + bc[0] * (mus(f1i) - mus(f0i))
            + bc[1] * (mus(f2i) - mus(f0i));
        friction_constraint_set.fv_constraints.back().mu =
            blend_mu(face_mu, mus(vi));
    }
}

///////////////////////////////////////////////////////////////////////////////
//
// compute_friction_potential() in friction.tpp
//
///////////////////////////////////////////////////////////////////////////////

Eigen::VectorXd compute_friction_potential_gradient(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const FrictionConstraints& friction_constraint_set,
    double epsv_times_h)
{
    if (friction_constraint_set.empty()) {
        return Eigen::VectorXd::Zero(V0.size());
    }

    auto U = V1 - V0; // absolute linear dislacement of each point
    int dim = U.cols();

    tbb::enumerable_thread_specific<Eigen::VectorXd> storage(
        Eigen::VectorXd::Zero(U.size()));

    tbb::parallel_for(
        tbb::blocked_range<size_t>(size_t(0), friction_constraint_set.size()),
        [&](const tbb::blocked_range<size_t>& r) {
            auto& local_grad = storage.local();
            for (size_t i = r.begin(); i < r.end(); i++) {
                local_gradient_to_global_gradient(
                    friction_constraint_set[i].compute_potential_gradient(
                        U, E, F, epsv_times_h),
                    friction_constraint_set[i].vertex_indices(E, F), dim,
                    local_grad);
            }
        });

    Eigen::VectorXd grad = Eigen::VectorXd::Zero(U.size());
    for (const auto& local_grad : storage) {
        grad += local_grad;
    }
    return grad;
}

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

Eigen::SparseMatrix<double> compute_friction_potential_hessian(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const FrictionConstraints& friction_constraint_set,
    double epsv_times_h,
    bool project_hessian_to_psd)
{
    if (friction_constraint_set.empty()) {
        return Eigen::SparseMatrix<double>(V0.size(), V0.size());
    }

    auto U = V1 - V0; // absolute linear dislacement of each point
    int dim = U.cols();

    tbb::enumerable_thread_specific<std::vector<Eigen::Triplet<double>>>
        storage;
    tbb::parallel_for(
        tbb::blocked_range<size_t>(size_t(0), friction_constraint_set.size()),
        [&](const tbb::blocked_range<size_t>& r) {
            auto& local_hess_triplets = storage.local();

            for (size_t i = r.begin(); i < r.end(); i++) {
                local_hessian_to_global_triplets(
                    friction_constraint_set[i].compute_potential_hessian(
                        U, E, F, epsv_times_h, project_hessian_to_psd),
                    friction_constraint_set[i].vertex_indices(E, F), dim,
                    local_hess_triplets);
            }
        });

    Eigen::SparseMatrix<double> hess(U.size(), U.size());
    for (const auto& local_hess_triplets : storage) {
        Eigen::SparseMatrix<double> local_hess(U.size(), U.size());
        local_hess.setFromTriplets(
            local_hess_triplets.begin(), local_hess_triplets.end());
        hess += local_hess;
    }
    return hess;
}

} // namespace ipc


//------------------------------------------------------------------------------
/// \file BinarySearchTrees_tests.cpp
/// \date 20201015 15, 17:21 complete
//------------------------------------------------------------------------------
#include "DataStructures/BinarySearchTrees.h"

#include <boost/test/unit_test.hpp>
#include <vector>

using DataStructures::BinarySearchTrees::InorderBstIterator;
using DataStructures::BinarySearchTrees::NodeWithCounter;
using DataStructures::BinarySearchTrees::TreeNode;
using DataStructures::BinarySearchTrees::TreeWithCounter;
using DataStructures::BinarySearchTrees::inorder_traversal;
using DataStructures::BinarySearchTrees::insert_into_bst;
using DataStructures::BinarySearchTrees::is_valid_binary_search_tree;
using DataStructures::BinarySearchTrees::iterative_validate_binary_search_tree;
using DataStructures::BinarySearchTrees::search_bst;
using std::vector;

BOOST_AUTO_TEST_SUITE(DataStructures)
BOOST_AUTO_TEST_SUITE(BinarySearchTrees_tests)

class StoreValues
{
  public:

    StoreValues() = default;

    int operator()(int value)
    {
      values_.emplace_back(value);
      return value;
    }

    vector<int> values() const
    {
      return values_;
    }

  private:

    vector<int> values_;
};

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(InorderTraversalDoesDepthFirstTraversals)
{
  StoreValues value_storage;
  TreeNode node_d3_l1 {4};
  TreeNode node_d3_l2 {5};
  TreeNode node_d2_l2 {3};
  TreeNode node_d2_l1 {2, &node_d3_l1, &node_d3_l2};
  TreeNode node_d1 {1, &node_d2_l1, &node_d2_l2};

  inorder_traversal(&node_d1, value_storage);

  const auto result = value_storage.values();

  BOOST_TEST(result.size() == 5);
  BOOST_TEST(result.at(0) == 4);
  BOOST_TEST(result.at(1) == 2);
  BOOST_TEST(result.at(2) == 5);
  BOOST_TEST(result.at(3) == 1);
  BOOST_TEST(result.at(4) == 3);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(IsValidBinarySearchTreeValidatesBSTs)
{
  {
    TreeNode node_d2_l1 {1};
    TreeNode node_d2_l2 {3};
    TreeNode node_d1_l1 {2, &node_d2_l1, &node_d2_l2};

    BOOST_TEST(is_valid_binary_search_tree(&node_d1_l1));
  }
  {
    TreeNode node_d3_l1 {3};
    TreeNode node_d3_l2 {6};
    TreeNode node_d2_l1 {1};
    TreeNode node_d2_l2 {4, &node_d3_l1, &node_d3_l2};
    TreeNode node_d1_l1 {5, &node_d2_l1, &node_d2_l2};

    BOOST_TEST(!is_valid_binary_search_tree(&node_d1_l1));
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(IterativeValidateBinarySearchTreeValidatesBSTs)
{
  {
    TreeNode node_d2_l1 {1};
    TreeNode node_d2_l2 {3};
    TreeNode node_d1_l1 {2, &node_d2_l1, &node_d2_l2};

    BOOST_TEST(iterative_validate_binary_search_tree(&node_d1_l1));
  }
  {
    TreeNode node_d3_l1 {3};
    TreeNode node_d3_l2 {6};
    TreeNode node_d2_l1 {1};
    TreeNode node_d2_l2 {4, &node_d3_l1, &node_d3_l2};
    TreeNode node_d1_l1 {5, &node_d2_l1, &node_d2_l2};

    BOOST_TEST(!iterative_validate_binary_search_tree(&node_d1_l1));
  }
}

// cf. https://www.geeksforgeeks.org/inorder-tree-traversal-without-recursion/
TreeNode geek_example_node_d3_l1 {4};
TreeNode geek_example_node_d3_l2 {5};
TreeNode geek_example_node_d2_l1 {
  2,
  &geek_example_node_d3_l1,
  &geek_example_node_d3_l2};
TreeNode geek_example_node_d2_l2 {3};
TreeNode geek_example_root {
  1,
  &geek_example_node_d2_l1,
  &geek_example_node_d2_l2};

TreeNode example_node_d3_l1 {9};
TreeNode example_node_d3_l2 {20};
TreeNode example_node_d2_l1 {3};
TreeNode example_node_d2_l2 {15, &example_node_d3_l1, &example_node_d3_l2};
TreeNode example_root {7, &example_node_d2_l1, &example_node_d2_l2};

TreeNode example_root_1 {7, &example_node_d2_l1, &example_node_d3_l2};

// Binary Search Tree Iterator
/// \url https://leetcode.com/explore/learn/card/introduction-to-data-structure-binary-search-tree/140/introduction-to-a-bst/1008/
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(InorderBstIteratorStepsThroughBST)
{
  {
    InorderBstIterator<TreeNode, int> iter {&geek_example_root};
    BOOST_TEST(iter.has_next());
    BOOST_TEST(iter.next() == 4);
    BOOST_TEST(iter.has_next());
    BOOST_TEST(iter.next() == 2);
    BOOST_TEST(iter.has_next());
    BOOST_TEST(iter.next() == 5);
    BOOST_TEST(iter.has_next());
    BOOST_TEST(iter.next() == 1);
    BOOST_TEST(iter.has_next());
    BOOST_TEST(iter.next() == 3);
    BOOST_TEST(!iter.has_next());
  }
  {
    InorderBstIterator<TreeNode, int> iter {&example_root};
    BOOST_TEST(iter.has_next());
    BOOST_TEST(iter.next() == 3);
    BOOST_TEST(iter.has_next());
    BOOST_TEST(iter.next() == 7);
    BOOST_TEST(iter.has_next());
    BOOST_TEST(iter.next() == 9);
    BOOST_TEST(iter.has_next());
    BOOST_TEST(iter.next() == 15);
    BOOST_TEST(iter.has_next());
    BOOST_TEST(iter.next() == 20);
    BOOST_TEST(!iter.has_next());
  }
  {
    InorderBstIterator<TreeNode, int> iter {&example_root_1};
    BOOST_TEST(iter.has_next());
    BOOST_TEST(iter.next() == 3);
    BOOST_TEST(iter.has_next());
    BOOST_TEST(iter.next() == 7);
    BOOST_TEST(iter.has_next());
    BOOST_TEST(iter.next() == 20);
    BOOST_TEST(!iter.has_next());
  }

}

/// \url https://leetcode.com/explore/learn/card/introduction-to-data-structure-binary-search-tree/141/basic-operations-in-a-bst/1000/
/// \brief Search in a Binary Search Tree.
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(SearchBstSearchesInABinarySearchTree)
{
  TreeNode example_node_d3_l1 {1};
  TreeNode example_node_d3_l2 {3};
  TreeNode example_node_d2_l1 {2, &example_node_d3_l1, &example_node_d3_l2};
  TreeNode example_node_d2_l2 {7};
  TreeNode example_root {4, &example_node_d2_l1, &example_node_d2_l2};

  {
    auto result = search_bst<TreeNode, int>(&example_root, 2);
    BOOST_TEST(result->value_ == 2);
    BOOST_TEST(result->left_->value_ == 1);
    BOOST_TEST(result->right_->value_ == 3);
  }
  {
    auto result = search_bst<TreeNode, int>(&example_root, 5);
    BOOST_TEST(result == nullptr);
  }
}

/// \url https://leetcode.com/explore/learn/card/introduction-to-data-structure-binary-search-tree/141/basic-operations-in-a-bst/1003/
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(InsertIntoBst)
{
  {
    TreeNode example_node_d3_l1 {1};
    TreeNode example_node_d3_l2 {3};
    TreeNode example_node_d2_l1 {2, &example_node_d3_l1, &example_node_d3_l2};
    TreeNode example_node_d2_l2 {7};
    TreeNode example_root {4, &example_node_d2_l1, &example_node_d2_l2};

    auto result = insert_into_bst<TreeNode, int>(&example_root, 5);
    InorderBstIterator<TreeNode, int> iter {result};

    BOOST_TEST(iter.has_next());
    BOOST_TEST(iter.next() == 1);
    BOOST_TEST(iter.has_next());
    BOOST_TEST(iter.next() == 2);
    BOOST_TEST(iter.has_next());
    BOOST_TEST(iter.next() == 3);
    BOOST_TEST(iter.has_next());
    BOOST_TEST(iter.next() == 4);
    BOOST_TEST(iter.has_next());
    //BOOST_TEST(iter.next() == 5);
    //BOOST_TEST(iter.has_next());
  }
}

BOOST_AUTO_TEST_SUITE(TreeWithCounter_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DefaultConstructs)
{
  TreeWithCounter tree {};
  BOOST_TEST(tree.root_ptr() == nullptr);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(InsertNewValueCreatesRootForEmptyTree)
{
  // Works
  TreeWithCounter* tree_ptr {new TreeWithCounter()};
  BOOST_TEST(tree_ptr->root_ptr() == nullptr);
  tree_ptr->insert_new_value(5);

  // Doesn't work.
//  BOOST_TEST(tree_ptr->root_ptr()->value_ == 5);

  // Does not work.
  //TreeWithCounter tree {};
  //BOOST_TEST(tree.root_ptr() == nullptr);
  //tree.insert_new_value(5);

  BOOST_TEST(tree_ptr->is_counter_stack_empty());

  NodeWithCounter* check_root {tree_ptr->root_ptr()};

  BOOST_TEST(check_root->value_ == 5);
  BOOST_TEST(check_root->counter_ == 0);
  BOOST_TEST(check_root->left_ == nullptr);
  BOOST_TEST(check_root->right_ == nullptr);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(InsertNewValuePlacesLessThanValuesToTheLeft)
{
  TreeWithCounter* tree_ptr {new TreeWithCounter()};
  BOOST_TEST(tree_ptr->root_ptr() == nullptr);
  tree_ptr->insert_new_value(5);

  NodeWithCounter* check_root {tree_ptr->root_ptr()};
  BOOST_TEST(check_root->value_ == 5);

  tree_ptr->insert_new_value(4);

  BOOST_TEST(tree_ptr->is_counter_stack_empty());

  BOOST_TEST(check_root->value_ == 5);
  BOOST_TEST(check_root->counter_ == 1);
  BOOST_TEST(check_root->left_ != nullptr);
  BOOST_TEST(check_root->left_->value_ == 4);
  BOOST_TEST(check_root->left_->left_ == nullptr);
  BOOST_TEST(check_root->left_->right_ == nullptr);
  BOOST_TEST(check_root->left_->counter_ == 0);

  tree_ptr->insert_new_value(2);
  BOOST_TEST(tree_ptr->is_counter_stack_empty());

  BOOST_TEST(check_root->value_ == 5);
  BOOST_TEST(check_root->counter_ == 2);
  BOOST_TEST(check_root->left_ != nullptr);
  BOOST_TEST(check_root->left_->value_ == 4);
  BOOST_TEST(check_root->left_->left_ != nullptr);
  BOOST_TEST(check_root->left_->right_ == nullptr);
  BOOST_TEST(check_root->left_->counter_ == 1);
  BOOST_TEST(check_root->left_->left_->value_ == 2);
  BOOST_TEST(check_root->left_->left_->counter_ == 0);
  BOOST_TEST(check_root->left_->left_->left_ == nullptr);
  BOOST_TEST(check_root->left_->left_->right_ == nullptr);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(InsertNewValuePlacesGreaterThanValuesToTheRight)
{
  TreeWithCounter* tree_ptr {new TreeWithCounter()};
  BOOST_TEST(tree_ptr->root_ptr() == nullptr);
  tree_ptr->insert_new_value(5);

  NodeWithCounter* check_root {tree_ptr->root_ptr()};
  BOOST_TEST(check_root->value_ == 5);

  tree_ptr->insert_new_value(6);

  BOOST_TEST(tree_ptr->is_counter_stack_empty());

  BOOST_TEST(check_root->value_ == 5);
  BOOST_TEST(check_root->counter_ == 1);
  BOOST_TEST(check_root->right_ != nullptr);
  BOOST_TEST(check_root->right_->value_ == 6);
  BOOST_TEST(check_root->right_->left_ == nullptr);
  BOOST_TEST(check_root->right_->right_ == nullptr);
  BOOST_TEST(check_root->right_->counter_ == 0);

  tree_ptr->insert_new_value(8);
  BOOST_TEST(tree_ptr->is_counter_stack_empty());

  BOOST_TEST(check_root->value_ == 5);
  BOOST_TEST(check_root->counter_ == 2);
  BOOST_TEST(check_root->right_ != nullptr);
  BOOST_TEST(check_root->right_->value_ == 6);
  BOOST_TEST(check_root->right_->left_ == nullptr);
  BOOST_TEST(check_root->right_->right_ != nullptr);
  BOOST_TEST(check_root->right_->counter_ == 1);
  BOOST_TEST(check_root->right_->right_->value_ == 8);
  BOOST_TEST(check_root->right_->right_->counter_ == 0);
  BOOST_TEST(check_root->right_->right_->left_ == nullptr);
  BOOST_TEST(check_root->right_->right_->right_ == nullptr);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(ConstructsWithVectorOfIntegers)
{
  vector<int> input {5, 2, 6, 1, 7, 4, 3};

  TreeWithCounter* tree_ptr {new TreeWithCounter(input)};

  NodeWithCounter* check_root {tree_ptr->root_ptr()};
  BOOST_TEST(check_root->value_ == 5);
  BOOST_TEST(check_root->counter_ == 6);

  // Right subtree.
  BOOST_TEST(check_root->right_->value_ == 6);
  BOOST_TEST(check_root->right_->counter_ == 1);
  BOOST_TEST(check_root->right_->right_->value_ == 7);
  BOOST_TEST(check_root->right_->right_->counter_ == 0);

  BOOST_TEST(check_root->left_->value_ == 2);
  BOOST_TEST(check_root->left_->counter_ == 3);
  BOOST_TEST(check_root->left_->left_->value_ == 1);
  BOOST_TEST(check_root->left_->left_->counter_ == 0);

  BOOST_TEST(check_root->left_->right_->value_ == 4);
  BOOST_TEST(check_root->left_->right_->counter_ == 1);
  BOOST_TEST(check_root->left_->right_->left_->value_ == 3);
  BOOST_TEST(check_root->left_->right_->left_->counter_ == 0);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(FindKthLargestElementFindsKthLargestElement)
{
  vector<int> input {4, 5, 8, 2};

  TreeWithCounter* tree_ptr {new TreeWithCounter(input)};

  NodeWithCounter* check_root {tree_ptr->root_ptr()};
  BOOST_TEST(check_root->value_ == 4);
  BOOST_TEST(check_root->counter_ == 3);
  BOOST_TEST(check_root->right_->value_ == 5);
  BOOST_TEST(check_root->right_->counter_ == 1);

  auto check_kth_element = tree_ptr->find_kth_largest_element(3);
  BOOST_TEST(check_kth_element == 4);
  //BOOST_TEST(check_kth_element != nullptr);
//  BOOST_TEST(check_kth_element->counter_ == 3);
  tree_ptr->insert_new_value(3);
  BOOST_TEST(tree_ptr->find_kth_largest_element(3) == 4);
  BOOST_TEST(check_root->counter_ == 4);

  //tree_ptr->insert_new_value(5);
  //BOOST_TEST(tree_ptr->find_kth_largest_element(3) == 5);
  //BOOST_TEST(check_root->counter_ == 5);

  tree_ptr->insert_new_value(10);
  BOOST_TEST(tree_ptr->find_kth_largest_element(3) == 5);
  BOOST_TEST(check_root->counter_ == 5);

}

BOOST_AUTO_TEST_SUITE_END() // TreeWithCounter_tests
BOOST_AUTO_TEST_SUITE_END() // BinarySearchTrees_tests
BOOST_AUTO_TEST_SUITE_END() // DataStructures


/*
 *  Software License Agreement (BSD License)
 *
 *  Copyright (c) 2014-2016, CNRS-LAAS.
 *  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 Willow Garage, Inc. 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 OWNER 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.
 */

/** \author Florent Lamiraux <florent@laas.fr> */

#define _USE_MATH_DEFINES
#include <cmath>

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

#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps))

#include <cmath>
#include <iostream>
#include <hpp/fcl/distance.h>
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/collision.h>
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/shape/geometric_shapes.h>

#include "utility.h"

using hpp::fcl::CollisionGeometryPtr_t;
using hpp::fcl::CollisionObject;
using hpp::fcl::DistanceRequest;
using hpp::fcl::DistanceResult;
using hpp::fcl::Transform3f;
using hpp::fcl::Vec3f;

BOOST_AUTO_TEST_CASE(distance_box_box_1) {
  CollisionGeometryPtr_t s1(new hpp::fcl::Box(6, 10, 2));
  CollisionGeometryPtr_t s2(new hpp::fcl::Box(2, 2, 2));

  Transform3f tf1;
  Transform3f tf2(Vec3f(25, 20, 5));

  CollisionObject o1(s1, tf1);
  CollisionObject o2(s2, tf2);

  // Enable computation of nearest points
  DistanceRequest distanceRequest(true, 0, 0);
  DistanceResult distanceResult;

  hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult);

  std::cerr << "Applied transformations on two boxes" << std::endl;
  std::cerr << " T1 = " << tf1.getTranslation() << std::endl
            << " R1 = " << tf1.getRotation() << std::endl
            << " T2 = " << tf2.getTranslation() << std::endl
            << " R2 = " << tf2.getRotation() << std::endl;
  std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0]
            << ", p2 = " << distanceResult.nearest_points[1]
            << ", distance = " << distanceResult.min_distance << std::endl;
  double dx = 25 - 3 - 1;
  double dy = 20 - 5 - 1;
  double dz = 5 - 1 - 1;

  const Vec3f& p1 = distanceResult.nearest_points[0];
  const Vec3f& p2 = distanceResult.nearest_points[1];
  BOOST_CHECK_CLOSE(distanceResult.min_distance,
                    sqrt(dx * dx + dy * dy + dz * dz), 1e-4);

  BOOST_CHECK_CLOSE(p1[0], 3, 1e-6);
  BOOST_CHECK_CLOSE(p1[1], 5, 1e-6);
  BOOST_CHECK_CLOSE(p1[2], 1, 1e-6);
  BOOST_CHECK_CLOSE(p2[0], 24, 1e-6);
  BOOST_CHECK_CLOSE(p2[1], 19, 1e-6);
  BOOST_CHECK_CLOSE(p2[2], 4, 1e-6);
}

BOOST_AUTO_TEST_CASE(distance_box_box_2) {
  CollisionGeometryPtr_t s1(new hpp::fcl::Box(6, 10, 2));
  CollisionGeometryPtr_t s2(new hpp::fcl::Box(2, 2, 2));
  static double pi = M_PI;
  Transform3f tf1;
  Transform3f tf2(
      hpp::fcl::makeQuat(cos(pi / 8), sin(pi / 8) / sqrt(3),
                         sin(pi / 8) / sqrt(3), sin(pi / 8) / sqrt(3)),
      Vec3f(0, 0, 10));

  CollisionObject o1(s1, tf1);
  CollisionObject o2(s2, tf2);

  // Enable computation of nearest points
  DistanceRequest distanceRequest(true, 0, 0);
  DistanceResult distanceResult;

  hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult);

  std::cerr << "Applied transformations on two boxes" << std::endl;
  std::cerr << " T1 = " << tf1.getTranslation() << std::endl
            << " R1 = " << tf1.getRotation() << std::endl
            << " T2 = " << tf2.getTranslation() << std::endl
            << " R2 = " << tf2.getRotation() << std::endl;
  std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0]
            << ", p2 = " << distanceResult.nearest_points[1]
            << ", distance = " << distanceResult.min_distance << std::endl;

  const Vec3f& p1 = distanceResult.nearest_points[0];
  const Vec3f& p2 = distanceResult.nearest_points[1];
  double distance = -1.62123444 + 10 - 1;
  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);

  BOOST_CHECK_CLOSE(p1[0], 0.60947571, 1e-4);
  BOOST_CHECK_CLOSE(p1[1], 0.01175873, 1e-4);
  BOOST_CHECK_CLOSE(p1[2], 1, 1e-6);
  BOOST_CHECK_CLOSE(p2[0], 0.60947571, 1e-4);
  BOOST_CHECK_CLOSE(p2[1], 0.01175873, 1e-4);
  BOOST_CHECK_CLOSE(p2[2], -1.62123444 + 10, 1e-4);
}

BOOST_AUTO_TEST_CASE(distance_box_box_3) {
  CollisionGeometryPtr_t s1(new hpp::fcl::Box(1, 1, 1));
  CollisionGeometryPtr_t s2(new hpp::fcl::Box(1, 1, 1));
  static double pi = M_PI;
  Transform3f tf1(hpp::fcl::makeQuat(cos(pi / 8), 0, 0, sin(pi / 8)),
                  Vec3f(-2, 1, .5));
  Transform3f tf2(hpp::fcl::makeQuat(cos(pi / 8), 0, sin(pi / 8), 0),
                  Vec3f(2, .5, .5));

  CollisionObject o1(s1, tf1);
  CollisionObject o2(s2, tf2);

  // Enable computation of nearest points
  DistanceRequest distanceRequest(true, 0, 0);
  DistanceResult distanceResult;

  hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult);

  std::cerr << "Applied transformations on two boxes" << std::endl;
  std::cerr << " T1 = " << tf1.getTranslation() << std::endl
            << " R1 = " << tf1.getRotation() << std::endl
            << " T2 = " << tf2.getTranslation() << std::endl
            << " R2 = " << tf2.getRotation() << std::endl;
  std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0]
            << ", p2 = " << distanceResult.nearest_points[1]
            << ", distance = " << distanceResult.min_distance << std::endl;

  const Vec3f& p1 = distanceResult.nearest_points[0];
  const Vec3f& p2 = distanceResult.nearest_points[1];
  double distance = 4 - sqrt(2);
  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);

  const Vec3f p1Ref(sqrt(2) / 2 - 2, 1, .5);
  const Vec3f p2Ref(2 - sqrt(2) / 2, 1, .5);
  BOOST_CHECK_CLOSE(p1[0], p1Ref[0], 1e-4);
  BOOST_CHECK_CLOSE(p1[1], p1Ref[1], 1e-4);
  BOOST_CHECK_CLOSE(p1[2], p1Ref[2], 1e-4);
  BOOST_CHECK_CLOSE(p2[0], p2Ref[0], 1e-4);
  BOOST_CHECK_CLOSE(p2[1], p2Ref[1], 1e-4);
  BOOST_CHECK_CLOSE(p2[2], p2Ref[2], 1e-4);

  // Apply the same global transform to both objects and recompute
  Transform3f tf3(hpp::fcl::makeQuat(0.435952844074, -0.718287018243,
                                     0.310622451066, 0.444435113443),
                  Vec3f(4, 5, 6));
  tf1 = tf3 * tf1;
  tf2 = tf3 * tf2;
  o1 = CollisionObject(s1, tf1);
  o2 = CollisionObject(s2, tf2);

  distanceResult.clear();
  hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult);

  std::cerr << "Applied transformations on two boxes" << std::endl;
  std::cerr << " T1 = " << tf1.getTranslation() << std::endl
            << " R1 = " << tf1.getRotation() << std::endl
            << " T2 = " << tf2.getTranslation() << std::endl
            << " R2 = " << tf2.getRotation() << std::endl;
  std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0]
            << ", p2 = " << distanceResult.nearest_points[1]
            << ", distance = " << distanceResult.min_distance << std::endl;

  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);

  const Vec3f p1Moved = tf3.transform(p1Ref);
  const Vec3f p2Moved = tf3.transform(p2Ref);
  BOOST_CHECK_CLOSE(p1[0], p1Moved[0], 1e-4);
  BOOST_CHECK_CLOSE(p1[1], p1Moved[1], 1e-4);
  BOOST_CHECK_CLOSE(p1[2], p1Moved[2], 1e-4);
  BOOST_CHECK_CLOSE(p2[0], p2Moved[0], 1e-4);
  BOOST_CHECK_CLOSE(p2[1], p2Moved[1], 1e-4);
  BOOST_CHECK_CLOSE(p2[2], p2Moved[2], 1e-4);
}

BOOST_AUTO_TEST_CASE(distance_box_box_4) {
  hpp::fcl::Box s1(1, 1, 1);
  hpp::fcl::Box s2(1, 1, 1);

  // Enable computation of nearest points
  DistanceRequest distanceRequest(true, 0, 0);
  DistanceResult distanceResult;
  double distance;

  Transform3f tf1(Vec3f(2, 0, 0));
  Transform3f tf2;
  hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);

  distance = 1.;
  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);

  tf1.setTranslation(Vec3f(1.01, 0, 0));
  distanceResult.clear();
  hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);

  distance = 0.01;
  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);

  tf1.setTranslation(Vec3f(0.99, 0, 0));
  distanceResult.clear();
  hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);

  distance = -0.01;
  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);

  tf1.setTranslation(Vec3f(0, 0, 0));
  distanceResult.clear();
  hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);

  distance = -1;
  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);
}


/*
 * AbstractEquation.cpp
 *
 *  Created on: 14.11.2018
 *      Author: thies
 */

#include <deal.II/base/exceptions.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/patterns.h>
#include <deal.II/base/thread_management.h>
#include <deal.II/base/timer.h>
#include <deal.II/base/types.h>
#include <deal.II/base/utilities.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/solver_control.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/vector_tools.h>

#include <iostream>
#include <map>
#include <string>

#include <base/Norm.h>
#include <forward/AbstractEquation.h>

namespace wavepi {
namespace forward {

template <int dim>
void AbstractEquation<dim>::declare_parameters(ParameterHandler &prm) {
  prm.enter_subsection("WaveEquation");
  {
    prm.declare_entry(
        "theta", "0.5", Patterns::Double(0, 1),
        "parameter θ in the time discretization (θ=1 → backward Euler, θ=0 → forward Euler, θ=0.5 → Crank-Nicolson");
    prm.declare_entry("tol", "1e-8", Patterns::Double(0, 1), "relative tolerance for the solution of linear systems");
    prm.declare_entry("max iter", "10000", Patterns::Integer(0),
                      "maximum iteration threshold for the solution of linear systems");
  }
  prm.leave_subsection();
}

template <int dim>
void AbstractEquation<dim>::get_parameters(ParameterHandler &prm) {
  prm.enter_subsection("WaveEquation");
  {
    theta                  = prm.get_double("theta");
    this->solver_tolerance = prm.get_double("tol");
    this->solver_max_iter  = prm.get_double("max iter");
  }
  prm.leave_subsection();
}

template <int dim>
void AbstractEquation<dim>::init_system(size_t first_idx) {
  dof_handler      = mesh->get_dof_handler(first_idx);
  sparsity_pattern = mesh->get_sparsity_pattern(first_idx);
  constraints      = mesh->get_constraint_matrix(first_idx);

  matrix_A.reinit(*sparsity_pattern);
  matrix_B.reinit(*sparsity_pattern);
  matrix_C.reinit(*sparsity_pattern);

  rhs.reinit(dof_handler->n_dofs());

  solution_u.reinit(dof_handler->n_dofs());
  solution_v.reinit(dof_handler->n_dofs());
  system_rhs.reinit(dof_handler->n_dofs());
  system_rhs.reinit(dof_handler->n_dofs());
  system_tmp1.reinit(dof_handler->n_dofs());
  system_tmp2.reinit(dof_handler->n_dofs());

  double time = mesh->get_time(first_idx);

  right_hand_side->set_time(time);
  initial_values(time);
}

template <int dim>
void AbstractEquation<dim>::cleanup() {
  matrix_A.clear();
  matrix_B.clear();
  matrix_C.clear();

  solution_u.reinit(0);
  solution_v.reinit(0);

  system_rhs.reinit(0);
  system_rhs.reinit(0);

  system_tmp1.reinit(0);
  system_tmp2.reinit(0);

  rhs.reinit(0);
}

template <int dim>
void AbstractEquation<dim>::next_mesh(size_t source_idx, size_t target_idx) {
  // TODO: system_tmp2 is a right hand side vector, maybe have to multiply with M^-1 first (and with M after?)
  dof_handler      = mesh->transfer(source_idx, target_idx, {&system_tmp1, &system_tmp2});
  sparsity_pattern = mesh->get_sparsity_pattern(target_idx);
  constraints      = mesh->get_constraint_matrix(target_idx);

  matrix_A.reinit(*sparsity_pattern);
  matrix_B.reinit(*sparsity_pattern);
  matrix_C.reinit(*sparsity_pattern);

  system_matrix.reinit(*sparsity_pattern);
  system_rhs.reinit(dof_handler->n_dofs());

  rhs.reinit(dof_handler->n_dofs());

  solution_u.reinit(dof_handler->n_dofs());
  solution_v.reinit(dof_handler->n_dofs());
}

template <int dim>
void AbstractEquation<dim>::assemble(size_t i) {
  double time = mesh->get_time(i);
  right_hand_side->set_time(time);

  // this helps only a bit because each of the operations is already parallelized
  Threads::TaskGroup<void> task_group;
  task_group += Threads::new_task(&AbstractEquation<dim>::assemble_matrices, *this, i);
  task_group += Threads::new_task(&RightHandSide<dim>::create_right_hand_side, *right_hand_side, *dof_handler,
                                  mesh->get_quadrature(), rhs);
  task_group.join_all();
}

template <int dim>
void AbstractEquation<dim>::assemble_pre(const SparseMatrix<double> &mass_matrix, double time_step) {
  Vector<double> tmp(solution_u.size());

  // grid has not been changed yet,
  // matrix_* contain the matrices of the *last* time step, rhs = rhs_old

  system_tmp2 = rhs;

  matrix_B.vmult(tmp, solution_v);
  system_tmp2.add(-1.0, tmp);

  matrix_A.vmult(tmp, solution_u);
  system_tmp2.add(-1.0, tmp);

  tmp.equ(1.0 - theta, system_tmp2);

  // tmp contains
  // (1-θ) (F^n - B^n V^n - A^n U^n)
  vmult_D_intermediate(mass_matrix, system_tmp2, tmp, this->solver_tolerance);

  vmult_C_intermediate(tmp, solution_v);
  system_tmp2.add(1.0 / time_step, tmp);

  // system_tmp2 contains
  // Y^n = (1-θ) (D^n)^-1 D^{n-1} M^{-1} (F^n - B^n V^n - A^n U^n) + 1/dt * C^{n,n-1} V^n

  system_tmp1.equ(1.0 / time_step, solution_u);
  system_tmp1.add(1.0 - theta, solution_v);

  // system_tmp1 contains
  // X^n = 1/dt U^n + (1-θ) V^n
}

// everything until this point of assembling for u depends on the old mesh and the old matrices
// -> interpolate system_rhs and tmp_u to the new grid and calculate new matrices on new grid

template <int dim>
void AbstractEquation<dim>::assemble_u(double time, double time_step) {
  Vector<double> tmp(solution_u.size());

  system_rhs.equ(theta, system_tmp2);
  system_rhs.add(theta * theta, rhs);

  matrix_C.vmult(tmp, system_tmp1);
  system_rhs.add(1.0 / time_step, tmp);

  matrix_B.vmult(tmp, system_tmp1);
  system_rhs.add(theta, tmp);

  // system_rhs contains
  // θ \bar Y^n + θ² F^{n+1} + (1/dt C^{n+1} + θB^{n+1}) \bar X^n

  system_matrix.copy_from(matrix_C);
  system_matrix *= 1.0 / (time_step * time_step);
  system_matrix.add(theta / time_step, matrix_B);
  system_matrix.add(theta * theta, matrix_A);

  // system_matrix contains
  // θ² A^{n+1} + θ/dt * B^{n+1} + 1/dt² C^{n+1}

  // needed, because hanging node constraints are not already built into the sparsity pattern
  constraints->condense(system_matrix, system_rhs);

  apply_boundary_conditions_u(time);
}

template <int dim>
void AbstractEquation<dim>::assemble_v(double time, double time_step) {
  Vector<double> tmp(solution_u.size());

  system_rhs.equ(1.0, system_tmp2);
  system_rhs.add(theta, rhs);

  matrix_A.vmult(tmp, solution_u);
  system_rhs.add(-1.0 * theta, tmp);

  // system_rhs contains
  // \bar Y^n + θF^{n+1} - θA^{n+1} U^{n+1}

  system_matrix.copy_from(matrix_C);
  system_matrix *= 1.0 / time_step;

  system_matrix.add(theta, matrix_B);

  // system_matrix contains
  // θB^{n+1} + 1/dt C^{n+1}

  // needed, because hanging node constraints are not already built into the sparsity pattern
  constraints->condense(system_matrix, system_rhs);

  apply_boundary_conditions_v(time);
}

template <int dim>
void AbstractEquation<dim>::solve_u() {
  LogStream::Prefix p("solve_u");

  double norm_rhs = system_rhs.l2_norm();

  SolverControl solver_control(this->solver_max_iter, this->solver_tolerance * norm_rhs);
  SolverCG<> cg(solver_control);

  if (precondition_max_age < 0) {
    PreconditionIdentity precondition = PreconditionIdentity();
    cg.solve(system_matrix, solution_u, system_rhs, precondition);
  } else {
    Timer timer;

    if (precondition_u_age < 0 || precondition_u_age > precondition_max_age) {
      timer.start();

      // precondition_u.initialize(system_matrix, PreconditionSSOR<SparseMatrix<double>>::AdditionalData(.8));
      precondition_u.initialize(system_matrix);
      precondition_u_age = 0;

      timer.stop();

      std::ios::fmtflags f(deallog.flags(std::ios_base::scientific));
      deallog << "Computed new preconditioner for u in " << timer.wall_time() << "s" << std::endl;
      deallog.flags(f);
    }
    precondition_u_age++;

    timer.restart();
    cg.solve(system_matrix, solution_u, system_rhs, precondition_u);
    timer.stop();

    std::ios::fmtflags f(deallog.flags(std::ios_base::scientific));
    deallog << "Solved system for u in " << timer.wall_time() << "s" << std::endl;
    deallog.flags(f);
  }

  constraints->distribute(solution_u);

  std::ios::fmtflags f(deallog.flags(std::ios_base::scientific));
  deallog << "Steps: " << solver_control.last_step();
  deallog << ", ‖res‖ = " << solver_control.last_value();
  deallog << ", ‖rhs‖ = " << norm_rhs << std::endl;

  deallog.flags(f);
}

template <int dim>
void AbstractEquation<dim>::solve_v() {
  LogStream::Prefix p("solve_v");

  double norm_rhs = system_rhs.l2_norm();

  SolverControl solver_control(this->solver_max_iter, this->solver_tolerance * norm_rhs);
  SolverCG<> cg(solver_control);

  if (precondition_max_age < 0) {
    PreconditionIdentity precondition = PreconditionIdentity();
    cg.solve(system_matrix, solution_v, system_rhs, precondition);
  } else {
    Timer timer;

    if (precondition_v_age < 0 || precondition_v_age > precondition_max_age) {
      timer.restart();

      // precondition_v.initialize(system_matrix, PreconditionSSOR<SparseMatrix<double>>::AdditionalData(.8));
      precondition_v.initialize(system_matrix);
      precondition_v_age = 0;

      timer.stop();

      std::ios::fmtflags f(deallog.flags(std::ios_base::scientific));
      deallog << "Computed new preconditioner for v in " << timer.wall_time() << "s" << std::endl;
      deallog.flags(f);
    }
    precondition_v_age++;

    timer.restart();
    cg.solve(system_matrix, solution_v, system_rhs, precondition_v);
    timer.stop();

    std::ios::fmtflags f(deallog.flags(std::ios_base::scientific));
    deallog << "Solved system for v in " << timer.wall_time() << "s" << std::endl;
    deallog.flags(f);
  }

  constraints->distribute(solution_v);

  std::ios::fmtflags f(deallog.flags(std::ios_base::scientific));

  deallog << "Steps: " << solver_control.last_step();
  deallog << ", ‖res‖ = " << solver_control.last_value();
  deallog << ", ‖rhs‖ = " << norm_rhs << std::endl;

  deallog.flags(f);
}

template <int dim>
DiscretizedFunction<dim> AbstractEquation<dim>::run(std::shared_ptr<RightHandSide<dim>> right_hand_side,
                                                    Direction direction) {
  LogStream::Prefix p("AbstractEq");
  Assert(mesh->length() >= 2, ExcInternalError());

  Timer timer, assembly_timer;
  timer.start();

  // this is going to be the result
  DiscretizedFunction<dim> u(mesh, std::make_shared<InvalidNorm<DiscretizedFunction<dim>>>(), true);

  // save handle to rhs function so that `assemble` can use it
  this->right_hand_side = right_hand_side;

  for (size_t i = 0; i < mesh->length(); i++) {
    LogStream::Prefix pp("step-" + Utilities::int_to_string(i, 4));
    int time_idx = direction == Backward ? mesh->length() - 1 - i : i;

    if (i == 0) {
      // set dof_handler to first grid,
      // initialize everything and project/interpolate initial values
      init_system(time_idx);

      // create matrices and rhs for time step zero (needed by assemble_pre in next step)
      assemble(mesh->get_time(time_idx));
    } else {
      int last_time_idx = direction == Backward ? mesh->length() - i : i - 1;

      double time      = mesh->get_time(time_idx);
      double last_time = mesh->get_time(last_time_idx);
      double dt        = time - last_time;

      // vector assembling that needs to take place on the old grid
      assemble_pre(*mesh->get_mass_matrix(last_time_idx), dt);

      // set dof_handler to mesh for this time step,
      // interpolate temporary vectors to new mesh
      // set solution_u and solution_v to zero
      next_mesh(last_time_idx, time_idx);

      // assemble new matrices and rhs
      assembly_timer.start();
      assemble(time_idx);
      assembly_timer.stop();

      // finish assembling of rhs_u
      // and solve for u^i
      assemble_u(time, dt);
      solve_u();

      // finish assembling of rhs_u
      // and solve for v^i
      assemble_v(time, dt);
      solve_v();
    }

    u.set(time_idx, solution_u, solution_v);

    std::ios::fmtflags f(deallog.flags(std::ios_base::fixed));
    deallog << std::setprecision(2) << "t=" << time << std::scientific << ", ";
    deallog << "‖u‖=" << solution_u.l2_norm() << ", ‖v‖=" << solution_v.l2_norm() << std::endl;
    deallog.flags(f);
  }

  timer.stop();
  std::ios::fmtflags f(deallog.flags(std::ios_base::fixed));
  deallog << "solved PDE in " << timer.wall_time() << "s (matrix assembly " << assembly_timer.wall_time() << "s)"
          << std::endl;
  deallog.flags(f);

  cleanup();
  return u;
}

template class AbstractEquation<1>;
template class AbstractEquation<2>;
template class AbstractEquation<3>;

} /* namespace forward */
} /* namespace wavepi */


/// Test file for solving AX = XB

#include "bigssMath.h"

#include <Eigen/SVD>

#include <cisstVector/vctTransformationTypes.h>
#include <cisstVector/vctFixedSizeVectorTypes.h>
#include <cisstVector/vctRandom.h>
#include <cisstCommon/cmnRandomSequence.h>

/// conduct a sensitivity analysis of AX=XB problem
/// using simulation data
/// input     X: nominal transformation (answer)
///           max_init_trans: maximum transformation for initial A matrix
///           k: number of sample pairs which we use for the simulation
///           rot_noise: maximum rotational noise which we add to the data
///           trans_noise: maximum translation noise which we add to the data
/// output    err_trans: translational error between X and estimated X
///           err_rot: rotational error between X and estimated X (length of
///                    the rodriguez angle vector in the unit of "degree")

void AX_XB_Simulation_OneTry(vctFrm4x4 &X, /*double max_init_trans,*/ int k, /*double rot_noise, double trans_noise,*/ vctFrm4x4 &Xest)
{

  // generate uncorrupted pairs
  vctDynamicMatrix<double> A_true(4*k, 4);
  vctDynamicMatrix<double> B_true(4*k, 4);

  vctDynamicMatrixRef<double> Aref, Bref;
  A_true.SetAll(0.0);
  B_true.SetAll(0.0);

  vctFrm4x4 Xinv = X.Inverse();

  // generate random rodriguez angle
  // double rot_step = 2*cmnPI/(k+1);
  vctFrm4x4 rot_base;
  rot_base.Rotation().From(vctRot3(1, 0, 0, 0, 0, -1, 0, 1, 0));
  rot_base.Translation().Assign(0.0, 0.0, 550.0);

  vct3 ax;
  double ang;
  vctFrm4x4 frm;

  vctDynamicMatrix<double> XDinv, XD;
  XDinv = Xinv;
  XD = X;
  cmnRandomSequence & randomSequence = cmnRandomSequence::GetInstance();
  for(int i=0; i<k; i++) {
    ang = randomSequence.ExtractRandomDouble(0, 2*cmnPI);
    vctRandom(ax, -1.0, 1.0);
    ax.NormalizedSelf();
    frm.Rotation().From(vctAxAnRot3(ax, ang));
    Aref.SetRef(A_true, 4*i, 0, 4, 4);
    Bref.SetRef(B_true, 4*i, 0, 4, 4);
    Aref.Assign(rot_base * frm * rot_base.Inverse());
    Bref.Assign(XDinv*Aref*XD);
  }

  std::cout << A_true << std::endl << std::endl;
  std::cout << B_true << std::endl << std::endl;

  // solve AX=XB problem from noise-added data
  BIGSS::ax_xb(A_true, B_true, Xest);
//#else
//  // convert cisst to eigen
//  Eigen::MatrixXd A_eig = Eigen::MatrixXd::Zero(4 * k, 4);
//  Eigen::MatrixXd B_eig = Eigen::MatrixXd::Zero(4 * k, 4);
//  Eigen::Matrix4d X_eig;
//
//  for (int i = 0; i < 4*k; i++)
//  {
//    A_eig.row(i) << A_true.Row(i).X(), A_true.Row(i).Y(), A_true.Row(i).Z(), A_true.Row(i).W();
//    B_eig.row(i) << B_true.Row(i).X(), B_true.Row(i).Y(), B_true.Row(i).Z(), B_true.Row(i).W();
//  }
//  BIGSS::ax_xb(A_eig, B_eig, X_eig);
//
//  Xest.Rotation().FromNormalized(vctRot3(vct3x3(
//    X_eig(0, 0), X_eig(0, 1), X_eig(0, 2),
//    X_eig(1, 0), X_eig(1, 1), X_eig(1, 2),
//    X_eig(2, 0), X_eig(2, 1), X_eig(2, 2)
//  )));
//  Xest.Translation().Assign(X_eig(0, 3), X_eig(1, 3), X_eig(2, 3));
//#endif
  //Xest = AX_XB( A_noised, B_noised );
  //  err_trans = norm(X(1:3,4)-X_estimated(1:3,4));
  //err_rot = (180/pi)*norm(RotMat2RodAng(X(1:3,1:3)'*X_estimated(1:3,1:3)));
}

int main()
{
  vct3 rot_axis(0.5, 0.5, 0.2);
  //double angle = 40;
  int k=5;

  vctFrm4x4 X, Xest;
  X.Rotation().FromNormalized(vctRot3(vct3x3(-0.502039,    -0.819662,    -0.275883,
				 0.817477,    -0.345628,    -0.460731,
				 0.282291,    -0.456832,     0.843573)));
  X.Translation().Assign(72.9024, -147.249,  -95.0182);

  //double num_trial = 100; //< number of trial for each condition
  //double rot_noise = cmnPI/100; //< maximum noise added to the rotation component
  //double trans_noise = 3; //< maximum noise added to the translation component

  AX_XB_Simulation_OneTry(X, /*100*sqrt(3.0),*/ k, /*rot_noise, trans_noise,*/ Xest);

  std::cout << X << std::endl << std::endl;
  std::cout << Xest << std::endl << std::endl;

  // test pseudoinverse
  Eigen::MatrixXd jp_eig = Eigen::MatrixXd::Random(6,6);
  Eigen::JacobiSVD<Eigen::MatrixXd> svdOfM(jp_eig, Eigen::ComputeThinU | Eigen::ComputeThinV);
  const Eigen::MatrixXd U = svdOfM.matrixU();
  const Eigen::MatrixXd V = svdOfM.matrixV();
  const Eigen::MatrixXd S = svdOfM.singularValues();
  double tolerance = 1.0e-6;

  Eigen::MatrixXd Sinv = S;
  double maxsv = 0;
  for (unsigned int i = 0; i < S.rows(); ++i)
    if (fabs(S(i)) > maxsv) maxsv = fabs(S(i));
  for (unsigned int i = 0; i < S.rows(); ++i)
  {
    //Those singular values smaller than a percentage of the maximum singular value are removed
    if (fabs(S(i)) > maxsv * tolerance)
      Sinv(i) = 1.0 / S(i);
    else Sinv(i) = 0;
  }

  Eigen::MatrixXd jpinv_eig = V * Sinv.asDiagonal() * U.transpose();
  Eigen::MatrixXd jpinv_comp;

  Eigen::MatrixXd::pinv(jp_eig, jpinv_comp, 1.0e-6);

  std::cout << std::endl << jpinv_comp << std::endl << std::endl;;
  std::cout << jpinv_eig << std::endl;
}