// This file is part of the dune-hdd project:
//   http://users.dune-project.org/projects/dune-hdd
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_HDD_LINEARELLIPTIC_ESTIMATORS_SWIPDG_HH
#define DUNE_HDD_LINEARELLIPTIC_ESTIMATORS_SWIPDG_HH

#include <map>
#include <memory>
#include <string>
#include <vector>

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

#if HAVE_ALUGRID
# include <dune/grid/alugrid.hh>
#endif

#include <dune/stuff/common/memory.hh>
#include <dune/stuff/common/tmp-storage.hh>
#include <dune/stuff/grid/walker.hh>
#include <dune/stuff/grid/walker/functors.hh>
#include <dune/stuff/playground/functions/ESV2007.hh>

#include <dune/pymor/common/exceptions.hh>
#include <dune/pymor/parameters/base.hh>

#include <dune/gdt/discretefunction/default.hh>
#include <dune/gdt/localevaluation/elliptic.hh>
#include <dune/gdt/localevaluation/product.hh>
#include <dune/gdt/localoperator/codim0.hh>
#include <dune/gdt/operators/oswaldinterpolation.hh>
#include <dune/gdt/operators/projections.hh>
#include <dune/gdt/playground/localevaluation/ESV2007.hh>
#include <dune/gdt/playground/operators/fluxreconstruction.hh>
#include <dune/gdt/spaces/fv/default.hh>
#include <dune/gdt/spaces/rt/pdelab.hh>

namespace Dune {
namespace HDD {
namespace LinearElliptic {
namespace Estimators {
namespace internal {
namespace SWIPDG {

static const size_t over_integrate = 2;

class LocalNonconformityESV2007Base
{
public:
  static std::string id() { return "eta_NC_ESV2007"; }
};

template< class SpaceType, class VectorType, class ProblemType, class GridType >
class LocalNonconformityESV2007
  : public LocalNonconformityESV2007Base
{
public:
  static const bool available = false;
};

#if HAVE_ALUGRID

/**
 *  \brief computes the local nonconformity estimator as defined in ESV2007
 */
template< class SpaceType, class VectorType, class ProblemType >
class LocalNonconformityESV2007< SpaceType, VectorType, ProblemType, ALUGrid< 2, 2, simplex, conforming > >
  : public LocalNonconformityESV2007Base
  , public Stuff::Grid::Functor::Codim0< typename SpaceType::GridViewType >
{
  typedef LocalNonconformityESV2007< SpaceType, VectorType, ProblemType, ALUGrid< 2, 2, simplex, conforming > >
    ThisType;
  typedef Stuff::Grid::Functor::Codim0< typename SpaceType::GridViewType > FunctorBaseType;
public:
  static const bool available = true;

  typedef std::map< std::string, Pymor::Parameter > ParametersMapType;

  typedef typename FunctorBaseType::GridViewType GridViewType;
  typedef typename FunctorBaseType::EntityType   EntityType;

  typedef typename ProblemType::RangeFieldType   RangeFieldType;

private:
  typedef GDT::ConstDiscreteFunction< SpaceType, VectorType > ConstDiscreteFunctionType;
  typedef GDT::DiscreteFunction< SpaceType, VectorType > DiscreteFunctionType;
  typedef typename ConstDiscreteFunctionType::DifferenceType DifferenceType;

  typedef typename ProblemType::DiffusionFactorType::NonparametricType DiffusionFactorType;
  typedef typename ProblemType::DiffusionTensorType::NonparametricType DiffusionTensorType;

  typedef GDT::LocalOperator::Codim0Integral< GDT::LocalEvaluation::Elliptic< DiffusionFactorType,
                                                                              DiffusionTensorType > > LocalOperatorType;
  typedef Stuff::Common::TmpMatricesStorage< RangeFieldType > TmpStorageProviderType;

  static const ProblemType& assert_problem(const ProblemType& problem, const Pymor::Parameter& mu_bar)
  {
    if (mu_bar.type() != problem.parameter_type())
      DUNE_THROW(Pymor::Exceptions::wrong_parameter_type,
                 "Given mu_bar is of type " << mu_bar.type() << " and should be of type " << problem.parameter_type()
                 << "!");
    if (problem.diffusion_tensor()->parametric())
      DUNE_THROW(NotImplemented, "Not implemented for parametric diffusion_tensor!");
    return problem;
  } // ... assert_problem(...)

public:
  static RangeFieldType estimate(const SpaceType& space,
                                 const VectorType& vector,
                                 const ProblemType& problem,
                                 const ParametersMapType parameters = ParametersMapType())
  {
    if (problem.diffusion_factor()->parametric() && parameters.find("mu_bar") == parameters.end())
      DUNE_THROW(Stuff::Exceptions::wrong_input_given, "Given parameters are missing 'mu_bar'!");
    const Pymor::Parameter mu_bar = problem.parametric() ? parameters.at("mu_bar") : Pymor::Parameter();
    ThisType estimator(space, vector, problem, mu_bar);
    Stuff::Grid::Walker< GridViewType > grid_walker(space.grid_view());
    grid_walker.add(estimator);
    grid_walker.walk();
    return std::sqrt(estimator.result_);
  } // ... estimate(...)

  LocalNonconformityESV2007(const SpaceType& space,
                            const VectorType& vector,
                            const ProblemType& problem,
                            const Pymor::Parameter mu_bar = Pymor::Parameter())
    : space_(space)
    , vector_(vector)
    , problem_(assert_problem(problem, mu_bar))
    , problem_mu_bar_(problem_.with_mu(mu_bar))
    , discrete_solution_(space_, vector_)
    , oswald_interpolation_(space_)
    , difference_(Stuff::Common::make_unique< DifferenceType >(discrete_solution_ - oswald_interpolation_))
    , local_operator_(over_integrate,
                      *problem_mu_bar_->diffusion_factor()->affine_part(),
                      *problem_.diffusion_tensor()->affine_part())
    , tmp_local_matrices_({1, local_operator_.numTmpObjectsRequired()}, 1, 1)
    , prepared_(false)
    , result_(0.0)
  {}

  virtual void prepare()
  {
    if (!prepared_) {
      const GDT::Operators::OswaldInterpolation< GridViewType > oswald_interpolation_operator(space_.grid_view());
      oswald_interpolation_operator.apply(discrete_solution_, oswald_interpolation_);
      result_ = 0.0;
      prepared_ = true;
    }
  } // ... prepare(...)

  RangeFieldType compute_locally(const EntityType& entity)
  {
    const auto local_difference = difference_->local_function(entity);
    local_operator_.apply(*local_difference,
                          *local_difference,
                          tmp_local_matrices_.matrices()[0][0],
                          tmp_local_matrices_.matrices()[1]);
    assert(tmp_local_matrices_.matrices()[0][0].rows() >= 1);
    assert(tmp_local_matrices_.matrices()[0][0].cols() >= 1);
    return tmp_local_matrices_.matrices()[0][0][0][0];
  } // ... compute_locally(...)

  virtual void apply_local(const EntityType &entity)
  {
    result_ += compute_locally(entity);
  }

private:
  const SpaceType& space_;
  const VectorType& vector_;
  const ProblemType& problem_;
  const std::shared_ptr< const typename ProblemType::NonparametricType > problem_mu_bar_;
  const ConstDiscreteFunctionType discrete_solution_;
  DiscreteFunctionType oswald_interpolation_;
  std::unique_ptr< const DifferenceType > difference_;
  const LocalOperatorType local_operator_;
  TmpStorageProviderType tmp_local_matrices_;
  bool prepared_;
public:
  RangeFieldType result_;
}; // class LocalNonconformityESV2007< ..., ALUGrid< 2, 2, simplex, conforming >, ... >

#endif // HAVE_ALUGRID

class LocalResidualESV2007Base
{
public:
  static std::string id() { return "eta_R_ESV2007"; }
};

template< class SpaceType, class VectorType, class ProblemType, class GridType >
class LocalResidualESV2007
  : public LocalResidualESV2007Base
{
public:
  static const bool available = false;
};

#if HAVE_ALUGRID

/**
 *  \brief computes the local residual estimator as defined in ESV2007
 */
template< class SpaceType, class VectorType, class ProblemType >
class LocalResidualESV2007< SpaceType, VectorType, ProblemType, ALUGrid< 2, 2, simplex, conforming > >
  : public LocalResidualESV2007Base
  , public Stuff::Grid::Functor::Codim0< typename SpaceType::GridViewType >
{
  typedef LocalResidualESV2007< SpaceType, VectorType, ProblemType, ALUGrid< 2, 2, simplex, conforming > >
      ThisType;
  typedef Stuff::Grid::Functor::Codim0< typename SpaceType::GridViewType > FunctorBaseType;
public:
  static const bool available = true;

  typedef typename FunctorBaseType::GridViewType GridViewType;
  typedef typename FunctorBaseType::EntityType   EntityType;

  typedef typename ProblemType::RangeFieldType RangeFieldType;

private:
  typedef GDT::Spaces::FV::Default< GridViewType, RangeFieldType, 1, 1 > P0SpaceType;
  typedef GDT::DiscreteFunction< P0SpaceType, VectorType > DiscreteFunctionType;
  typedef typename DiscreteFunctionType::DifferenceType DifferenceType;

  typedef typename ProblemType::DiffusionFactorType::NonparametricType DiffusionFactorType;
  typedef typename ProblemType::DiffusionTensorType::NonparametricType DiffusionTensorType;

  typedef typename Stuff::Functions::ESV2007::Cutoff< DiffusionFactorType, DiffusionTensorType > CutoffFunctionType;
  typedef GDT::LocalOperator::Codim0Integral< GDT::LocalEvaluation::Product< CutoffFunctionType > > LocalOperatorType;
  typedef DSC::TmpMatricesStorage< RangeFieldType > TmpStorageProviderType;

  static const ProblemType& assert_problem(const ProblemType& problem)
  {
    if (problem.parametric())
      DUNE_THROW(NotImplemented, "Not implemented yet for parametric problems!");
    assert(problem.diffusion_factor()->has_affine_part());
    assert(problem.diffusion_tensor()->has_affine_part());
    assert(problem.force()->has_affine_part());
    return problem;
  } // ... assert_problem(...)

public:
  static RangeFieldType estimate(const SpaceType& space, const VectorType& /*vector*/, const ProblemType& problem)
  {
    ThisType estimator(space, problem);
    Stuff::Grid::Walker< GridViewType > grid_walker(space.grid_view());
    grid_walker.add(estimator);
    grid_walker.walk();
    return std::sqrt(estimator.result_);
  } // ... estimate(...)

  LocalResidualESV2007(const SpaceType& space, const ProblemType& problem)
    : space_(space)
    , problem_(assert_problem(problem))
    , p0_space_(space_.grid_view())
    , p0_force_(p0_space_)
    , difference_(Stuff::Common::make_unique< DifferenceType >(*problem_.force()->affine_part() - p0_force_))
    , cutoff_function_(*problem_.diffusion_factor()->affine_part(),
                       *problem_.diffusion_tensor()->affine_part())
    , local_operator_(over_integrate, cutoff_function_)
    , tmp_local_matrices_({1, local_operator_.numTmpObjectsRequired()}, 1, 1)
    , prepared_(false)
    , result_(0.0)
  {}

  virtual void prepare()
  {
    if (!prepared_) {
      const GDT::Operators::Projection< GridViewType > projection_operator(space_.grid_view(), over_integrate);
      projection_operator.apply(*problem_.force()->affine_part(), p0_force_);
      result_ = 0.0;
      prepared_ = true;
    }
  } // ... prepare(...)

  RangeFieldType compute_locally(const EntityType& entity)
  {
    const auto local_difference = difference_->local_function(entity);
    local_operator_.apply(*local_difference,
                          *local_difference,
                          tmp_local_matrices_.matrices()[0][0],
                          tmp_local_matrices_.matrices()[1]);
    assert(tmp_local_matrices_.matrices()[0][0].rows() >= 1);
    assert(tmp_local_matrices_.matrices()[0][0].cols() >= 1);
    return tmp_local_matrices_.matrices()[0][0][0][0];
  } // ... compute_locally(...)

  virtual void apply_local(const EntityType &entity)
  {
    result_ += compute_locally(entity);
  }

private:
  const SpaceType& space_;
  const ProblemType& problem_;
  const P0SpaceType p0_space_;
  DiscreteFunctionType p0_force_;
  std::unique_ptr< const DifferenceType > difference_;
  const CutoffFunctionType cutoff_function_;
  const LocalOperatorType local_operator_;
  TmpStorageProviderType tmp_local_matrices_;
  bool prepared_;
public:
  RangeFieldType result_;
}; // class LocalResidualESV2007< ..., ALUGrid< 2, 2, simplex, conforming >, ... >

#endif // HAVE_ALUGRID

class LocalResidualESV2007StarBase
{
public:
  static std::string id() { return "eta_R_ESV2007_*"; }
};

template< class SpaceType, class VectorType, class ProblemType, class GridType >
class LocalResidualESV2007Star
  : public LocalResidualESV2007StarBase
{
public:
  static const bool available = false;
};

#if HAVE_ALUGRID

/**
 *  \brief computes the local residual estimator as defined in ESV2007
 */
template< class SpaceType, class VectorType, class ProblemType >
class LocalResidualESV2007Star< SpaceType, VectorType, ProblemType, ALUGrid< 2, 2, simplex, conforming > >
  : public LocalResidualESV2007StarBase
  , public Stuff::Grid::Functor::Codim0< typename SpaceType::GridViewType >
{
  typedef LocalResidualESV2007Star< SpaceType, VectorType, ProblemType, ALUGrid< 2, 2, simplex, conforming > >
      ThisType;
  typedef Stuff::Grid::Functor::Codim0< typename SpaceType::GridViewType > FunctorBaseType;
public:
  static const bool available = true;

  typedef std::map< std::string, Pymor::Parameter > ParametersMapType;

  typedef typename FunctorBaseType::GridViewType GridViewType;
  typedef typename FunctorBaseType::EntityType   EntityType;

  typedef typename ProblemType::RangeFieldType RangeFieldType;

  static const unsigned int dimDomain = SpaceType::dimDomain;

private:
  typedef GDT::ConstDiscreteFunction< SpaceType, VectorType > ConstDiscreteFunctionType;
  typedef GDT::Spaces::RT::PdelabBased< GridViewType, 0, RangeFieldType, dimDomain > RTN0SpaceType;
  typedef GDT::DiscreteFunction< RTN0SpaceType, VectorType > RTN0DiscreteFunctionType;
  typedef typename RTN0DiscreteFunctionType::DivergenceType DivergenceType;
  typedef typename DivergenceType::DifferenceType DifferenceType;

  typedef typename ProblemType::DiffusionFactorType::NonparametricType DiffusionFactorType;
  typedef typename ProblemType::DiffusionTensorType::NonparametricType DiffusionTensorType;

  typedef typename Stuff::Functions::ESV2007::Cutoff< DiffusionFactorType, DiffusionTensorType > CutoffFunctionType;
  typedef GDT::LocalOperator::Codim0Integral< GDT::LocalEvaluation::Product< CutoffFunctionType > > LocalOperatorType;
  typedef DSC::TmpMatricesStorage< RangeFieldType > TmpStorageProviderType;

  static const ProblemType& assert_problem(const ProblemType& problem, const Pymor::Parameter& mu)
  {
    if (mu.type() != problem.parameter_type())
      DUNE_THROW(Pymor::Exceptions::wrong_parameter_type,
                 "Given mu is of type " << mu.type() << " and should be of type " << problem.parameter_type()
                 << "!");
    if (problem.diffusion_tensor()->parametric())
      DUNE_THROW(NotImplemented, "Not implemented for parametric diffusion_tensor!");
    if (problem.force()->parametric())
      DUNE_THROW(NotImplemented, "Not implemented for parametric force!");
    return problem;
  } // ... assert_problem(...)

public:
  static RangeFieldType estimate(const SpaceType& space,
                                 const VectorType& vector,
                                 const ProblemType& problem,
                                 const ParametersMapType parameters = ParametersMapType())
  {
    if (problem.diffusion_factor()->parametric() && parameters.find("mu") == parameters.end())
      DUNE_THROW(Stuff::Exceptions::wrong_input_given, "Given parameters are missing 'mu'!");
    const Pymor::Parameter mu = problem.parametric() ? parameters.at("mu") : Pymor::Parameter();
    ThisType estimator(space, vector, problem, mu);
    Stuff::Grid::Walker< GridViewType > grid_walker(space.grid_view());
    grid_walker.add(estimator);
    grid_walker.walk();
    return std::sqrt(estimator.result_);
  } // ... estimate(...)

  LocalResidualESV2007Star(const SpaceType& space,
                           const VectorType& vector,
                           const ProblemType& problem,
                           const Pymor::Parameter mu = Pymor::Parameter())
    : space_(space)
    , vector_(vector)
    , problem_(assert_problem(problem, mu))
    , problem_mu_(problem_.with_mu(mu))
    , discrete_solution_(space_, vector_)
    , rtn0_space_(space_.grid_view())
    , diffusive_flux_(rtn0_space_)
    , divergence_(diffusive_flux_.divergence())
    , difference_(*problem_.force()->affine_part() - divergence_)
    , cutoff_function_(*problem_.diffusion_factor()->affine_part(),
                       *problem_.diffusion_tensor()->affine_part())
    , local_operator_(over_integrate, cutoff_function_)
    , tmp_local_matrices_({1, local_operator_.numTmpObjectsRequired()}, 1, 1)
    , prepared_(false)
    , result_(0.0)
  {}

  virtual ~LocalResidualESV2007Star() = default;

  virtual void prepare()
  {
    if (!prepared_) {
      const GDT::Operators::DiffusiveFluxReconstruction< GridViewType, DiffusionFactorType, DiffusionTensorType >
        diffusive_flux_reconstruction(space_.grid_view(),
                                      *problem_mu_->diffusion_factor()->affine_part(),
                                      *problem_.diffusion_tensor()->affine_part(),
                                      over_integrate);
      diffusive_flux_reconstruction.apply(discrete_solution_, diffusive_flux_);
      result_ = 0.0;
      prepared_ = true;
    }
  } // ... prepare(...)

  RangeFieldType compute_locally(const EntityType& entity)
  {
    const auto local_difference = difference_.local_function(entity);
    local_operator_.apply(*local_difference,
                          *local_difference,
                          tmp_local_matrices_.matrices()[0][0],
                          tmp_local_matrices_.matrices()[1]);
    assert(tmp_local_matrices_.matrices()[0][0].rows() >= 1);
    assert(tmp_local_matrices_.matrices()[0][0].cols() >= 1);
    return tmp_local_matrices_.matrices()[0][0][0][0];
  } // ... compute_locally(...)

  virtual void apply_local(const EntityType &entity)
  {
    result_ += compute_locally(entity);
  }

private:
  const SpaceType& space_;
  const VectorType& vector_;
  const ProblemType& problem_;
  const std::shared_ptr< typename ProblemType::NonparametricType > problem_mu_;
  const ConstDiscreteFunctionType discrete_solution_;
  const RTN0SpaceType rtn0_space_;
  RTN0DiscreteFunctionType diffusive_flux_;
  const DivergenceType divergence_;
  const DifferenceType difference_;
  const CutoffFunctionType cutoff_function_;
  const LocalOperatorType local_operator_;
  TmpStorageProviderType tmp_local_matrices_;
  bool prepared_;
public:
  RangeFieldType result_;
}; // class LocalResidualESV2007Star< ..., ALUGrid< 2, 2, simplex, conforming >, ... >

#endif // HAVE_ALUGRID

class LocalDiffusiveFluxESV2007Base
{
public:
  static std::string id() { return "eta_DF_ESV2007"; }
};

template< class SpaceType, class VectorType, class ProblemType, class GridType >
class LocalDiffusiveFluxESV2007
  : public LocalDiffusiveFluxESV2007Base
{
public:
  static const bool available = false;
};

#if HAVE_ALUGRID

/**
 *  \brief computes the local diffusive flux estimator as defined in ESV2007
 */
template< class SpaceType, class VectorType, class ProblemType >
class LocalDiffusiveFluxESV2007< SpaceType, VectorType, ProblemType, ALUGrid< 2, 2, simplex, conforming > >
  : public LocalDiffusiveFluxESV2007Base
  , public Stuff::Grid::Functor::Codim0< typename SpaceType::GridViewType >
{
  typedef LocalDiffusiveFluxESV2007< SpaceType, VectorType, ProblemType, ALUGrid< 2, 2, simplex, conforming > >
      ThisType;
  typedef Stuff::Grid::Functor::Codim0< typename SpaceType::GridViewType > FunctorBaseType;
public:
  static const bool available = true;

  typedef std::map< std::string, Pymor::Parameter > ParametersMapType;

  typedef typename FunctorBaseType::GridViewType GridViewType;
  typedef typename FunctorBaseType::EntityType   EntityType;

  typedef typename ProblemType::RangeFieldType RangeFieldType;

  static const unsigned int dimDomain = SpaceType::dimDomain;

private:
  typedef GDT::ConstDiscreteFunction< SpaceType, VectorType > ConstDiscreteFunctionType;
  typedef GDT::Spaces::RT::PdelabBased< GridViewType, 0, RangeFieldType, dimDomain > RTN0SpaceType;
  typedef GDT::DiscreteFunction< RTN0SpaceType, VectorType > RTN0DiscreteFunctionType;

  typedef typename ProblemType::DiffusionFactorType::NonparametricType DiffusionFactorType;
  typedef typename ProblemType::DiffusionTensorType::NonparametricType DiffusionTensorType;

  typedef GDT::LocalOperator::Codim0Integral<
      GDT::LocalEvaluation::ESV2007::DiffusiveFluxEstimate< DiffusionFactorType,
                                                            RTN0DiscreteFunctionType,
                                                            DiffusionTensorType > > LocalOperatorType;
  typedef DSC::TmpMatricesStorage< RangeFieldType > TmpStorageProviderType;

  static const ProblemType& assert_problem(const ProblemType& problem,
                                           const Pymor::Parameter& mu,
                                           const Pymor::Parameter& mu_hat)
  {
    if (mu.type() != problem.parameter_type())
      DUNE_THROW(Pymor::Exceptions::wrong_parameter_type,
                 "Given mu is of type " << mu.type() << " and should be of type " << problem.parameter_type()
                 << "!");
    if (mu_hat.type() != problem.parameter_type())
      DUNE_THROW(Pymor::Exceptions::wrong_parameter_type,
                 "Given mu_hat is of type " << mu_hat.type() << " and should be of type " << problem.parameter_type()
                 << "!");
    if (problem.diffusion_tensor()->parametric())
      DUNE_THROW(NotImplemented, "Not implemented for parametric diffusion_tensor!");
    return problem;
  } // ... assert_problem(...)

public:
  static RangeFieldType estimate(const SpaceType& space,
                                 const VectorType& vector,
                                 const ProblemType& problem,
                                 const ParametersMapType parameters = ParametersMapType())
  {
    if (problem.diffusion_factor()->parametric() && parameters.find("mu") == parameters.end())
      DUNE_THROW(Stuff::Exceptions::wrong_input_given, "Given parameters are missing 'mu'!");
    if (problem.diffusion_factor()->parametric() && parameters.find("mu_hat") == parameters.end())
      DUNE_THROW(Stuff::Exceptions::wrong_input_given, "Given parameters are missing 'mu_hat'!");
    const Pymor::Parameter mu =     problem.parametric() ? parameters.at("mu")     : Pymor::Parameter();
    const Pymor::Parameter mu_hat = problem.parametric() ? parameters.at("mu_hat") : Pymor::Parameter();
    ThisType estimator(space, vector, problem, mu, mu_hat);
    Stuff::Grid::Walker< GridViewType > grid_walker(space.grid_view());
    grid_walker.add(estimator);
    grid_walker.walk();
    return std::sqrt(estimator.result_);
  } // ... estimate(...)

  LocalDiffusiveFluxESV2007(const SpaceType& space,
                            const VectorType& vector,
                            const ProblemType& problem,
                            const Pymor::Parameter mu = Pymor::Parameter(),
                            const Pymor::Parameter mu_hat = Pymor::Parameter())
    : space_(space)
    , vector_(vector)
    , problem_(assert_problem(problem, mu, mu_hat))
    , problem_mu_(problem_.with_mu(mu))
    , problem_mu_hat_(problem.with_mu(mu_hat))
    , discrete_solution_(space_, vector_)
    , rtn0_space_(space.grid_view())
    , diffusive_flux_(rtn0_space_)
    , local_operator_(over_integrate,
                      *problem_mu_hat_->diffusion_factor()->affine_part(),
                      *problem_.diffusion_tensor()->affine_part(),
                      diffusive_flux_)
    , tmp_local_matrices_({1, local_operator_.numTmpObjectsRequired()}, 1, 1)
    , prepared_(false)
    , result_(0.0)
  {}

  virtual void prepare()
  {
    if (!prepared_) {
      const GDT::Operators::DiffusiveFluxReconstruction< GridViewType, DiffusionFactorType, DiffusionTensorType >
        diffusive_flux_reconstruction(space_.grid_view(),
                                      *problem_mu_->diffusion_factor()->affine_part(),
                                      *problem_.diffusion_tensor()->affine_part(),
                                      over_integrate);
      diffusive_flux_reconstruction.apply(discrete_solution_, diffusive_flux_);
      result_ = 0.0;
      prepared_ = true;
    }
  } // ... prepare(...)

  RangeFieldType compute_locally(const EntityType& entity)
  {
    const auto local_discrete_solution = discrete_solution_.local_function(entity);
    local_operator_.apply(*local_discrete_solution,
                          *local_discrete_solution,
                          tmp_local_matrices_.matrices()[0][0],
                          tmp_local_matrices_.matrices()[1]);
    assert(tmp_local_matrices_.matrices()[0][0].rows() >= 1);
    assert(tmp_local_matrices_.matrices()[0][0].cols() >= 1);
    return tmp_local_matrices_.matrices()[0][0][0][0];
  } // ... compute_locally(...)

  virtual void apply_local(const EntityType &entity)
  {
    result_ += compute_locally(entity);
  }

private:
  const SpaceType& space_;
  const VectorType& vector_;
  const ProblemType& problem_;
  const std::shared_ptr< const typename ProblemType::NonparametricType > problem_mu_;
  const std::shared_ptr< const typename ProblemType::NonparametricType > problem_mu_hat_;
  const ConstDiscreteFunctionType discrete_solution_;
  const RTN0SpaceType rtn0_space_;
  RTN0DiscreteFunctionType diffusive_flux_;
  const LocalOperatorType local_operator_;
  TmpStorageProviderType tmp_local_matrices_;
  bool prepared_;
public:
  RangeFieldType result_;
}; // class LocalDiffusiveFluxESV2007< ..., ALUGrid< 2, 2, simplex, conforming >, ... >

#endif // HAVE_ALUGRID

class ESV2007Base
{
public:
  static std::string id()
  {
    return "eta_ESV2007";
  }
};

template< class SpaceType, class VectorType, class ProblemType, class GridType >
class ESV2007
  : public ESV2007Base
{
public:
  static const bool available = false;
};

#if HAVE_ALUGRID

template< class SpaceType, class VectorType, class ProblemType >
class ESV2007< SpaceType, VectorType, ProblemType, ALUGrid< 2, 2, simplex, conforming > >
  : public ESV2007Base
{
  typedef ALUGrid< 2, 2, simplex, conforming > GridType;
public:
  static const bool available = true;

  typedef typename ProblemType::RangeFieldType RangeFieldType;

  static RangeFieldType estimate(const SpaceType& space, const VectorType& vector, const ProblemType& problem)
  {
    LocalNonconformityESV2007< SpaceType, VectorType, ProblemType, GridType > eta_nc(space, vector, problem);
    LocalResidualESV2007< SpaceType, VectorType, ProblemType, GridType >      eta_r(space, problem);
    LocalDiffusiveFluxESV2007< SpaceType, VectorType, ProblemType, GridType > eta_df(space, vector, problem);
    eta_nc.prepare();
    eta_r.prepare();
    eta_df.prepare();

    RangeFieldType eta_squared(0.0);

    const auto& grid_view = space.grid_view();
    const auto entity_it_end = grid_view.template end< 0 >();
    for (auto entity_it = grid_view.template begin< 0 >(); entity_it != entity_it_end; ++entity_it) {
      const auto& entity = *entity_it;
      eta_squared += eta_nc.compute_locally(entity)
                   + std::pow(std::sqrt(eta_r.compute_locally(entity)) + std::sqrt(eta_df.compute_locally(entity)), 2);
    }
    return std::sqrt(eta_squared);
  } // ... estimate(...)

  static Stuff::LA::CommonDenseVector< RangeFieldType > estimate_local(const SpaceType& space,
                                                                       const VectorType& vector,
                                                                       const ProblemType& problem)
  {
    LocalNonconformityESV2007< SpaceType, VectorType, ProblemType, GridType > eta_nc(space, vector, problem);
    LocalResidualESV2007< SpaceType, VectorType, ProblemType, GridType >      eta_r(space, problem);
    LocalDiffusiveFluxESV2007< SpaceType, VectorType, ProblemType, GridType > eta_df(space, vector, problem);
    eta_nc.prepare();
    eta_r.prepare();
    eta_df.prepare();

    const auto& grid_view = space.grid_view();
    Stuff::LA::CommonDenseVector< RangeFieldType >
        local_indicators(boost::numeric_cast< size_t >(grid_view.indexSet().size(0)), 0.0);
    RangeFieldType eta_squared = 0.0;

    const auto entity_it_end = grid_view.template end< 0 >();
    for (auto entity_it = grid_view.template begin< 0 >(); entity_it != entity_it_end; ++entity_it) {
      const auto& entity = *entity_it;
      const auto index = grid_view.indexSet().index(entity);
      const RangeFieldType eta_t_squared
          = eta_nc.compute_locally(entity)
            + std::pow(std::sqrt(eta_r.compute_locally(entity)) + std::sqrt(eta_df.compute_locally(entity)), 2);
      local_indicators[index] = eta_t_squared;
      eta_squared += eta_t_squared;
    }
    for (auto& element : local_indicators)
      element /= eta_squared;
    return local_indicators;
  } // ... estimate_local(...)
}; // class ESV2007< ..., ALUGrid< 2, 2, simplex, conforming >, ... >

#endif // HAVE_ALUGRID

class ESV2007AlternativeSummationBase
{
public:
  static std::string id()
  {
    return "eta_ESV2007_alt";
  }
};

template< class SpaceType, class VectorType, class ProblemType, class GridType >
class ESV2007AlternativeSummation
  : public ESV2007AlternativeSummationBase
{
public:
  static const bool available = false;
};

#if HAVE_ALUGRID

template< class SpaceType, class VectorType, class ProblemType >
class ESV2007AlternativeSummation< SpaceType, VectorType, ProblemType, ALUGrid< 2, 2, simplex, conforming > >
  : public ESV2007AlternativeSummationBase
{
  typedef ALUGrid< 2, 2, simplex, conforming > GridType;
public:
  static const bool available = true;

  typedef typename ProblemType::RangeFieldType RangeFieldType;

  static RangeFieldType estimate(const SpaceType& space, const VectorType& vector, const ProblemType& problem)
  {
    LocalNonconformityESV2007< SpaceType, VectorType, ProblemType, GridType > eta_nc(space, vector, problem);
    LocalResidualESV2007< SpaceType, VectorType, ProblemType, GridType >      eta_r(space, problem);
    LocalDiffusiveFluxESV2007< SpaceType, VectorType, ProblemType, GridType > eta_df(space, vector, problem);
    eta_nc.prepare();
    eta_r.prepare();
    eta_df.prepare();

    RangeFieldType eta_nc_squared(0.0);
    RangeFieldType eta_r_squared(0.0);
    RangeFieldType eta_df_squared(0.0);

    const auto& grid_view = space.grid_view();
    const auto entity_it_end = grid_view.template end< 0 >();
    for (auto entity_it = grid_view.template begin< 0 >(); entity_it != entity_it_end; ++entity_it) {
      const auto& entity = *entity_it;
      eta_nc_squared += eta_nc.compute_locally(entity);
      eta_r_squared += eta_r.compute_locally(entity);
      eta_df_squared += eta_df.compute_locally(entity);
    }
    return std::sqrt(eta_nc_squared) + std::sqrt(eta_r_squared) + std::sqrt(eta_df_squared);
  } // ... estimate(...)

  static Stuff::LA::CommonDenseVector< RangeFieldType > estimate_local(const SpaceType& space,
                                                                       const VectorType& vector,
                                                                       const ProblemType& problem)
  {
    LocalNonconformityESV2007< SpaceType, VectorType, ProblemType, GridType > eta_nc(space, vector, problem);
    LocalResidualESV2007< SpaceType, VectorType, ProblemType, GridType >      eta_r(space, problem);
    LocalDiffusiveFluxESV2007< SpaceType, VectorType, ProblemType, GridType > eta_df(space, vector, problem);
    eta_nc.prepare();
    eta_r.prepare();
    eta_df.prepare();

    const auto grid_view = space.grid_view();
    Stuff::LA::CommonDenseVector< RangeFieldType >
        local_indicators(boost::numeric_cast< size_t >(grid_view.indexSet().size(0)), 0.0);
    RangeFieldType eta_nc_squared(0.0);
    RangeFieldType eta_r_squared(0.0);
    RangeFieldType eta_df_squared(0.0);

    const auto entity_it_end = grid_view.template end< 0 >();
    for (auto entity_it = grid_view.template begin< 0 >(); entity_it != entity_it_end; ++entity_it) {
      const auto& entity = *entity_it;
      const auto index = grid_view.indexSet().index(entity);
      const RangeFieldType eta_nc_t_squared = eta_nc.compute_locally(entity);
      const RangeFieldType eta_r_t_squared = eta_r.compute_locally(entity);
      const RangeFieldType eta_df_t_squared = eta_df.compute_locally(entity);
      eta_nc_squared += eta_nc_t_squared;
      eta_r_squared += eta_r_t_squared;
      eta_df_squared += eta_df_t_squared;
      local_indicators[index] = 3.0 *(eta_nc_t_squared + eta_r_t_squared + eta_df_t_squared);
    }
    const RangeFieldType eta_squared
        = std::pow(std::sqrt(eta_nc_squared) + std::sqrt(eta_r_squared) + std::sqrt(eta_df_squared), 2);
    for (auto& element : local_indicators)
      element /= eta_squared;
    return local_indicators;
  } // ... estimate_local(...)
}; // class ESV2007AlternativeSummation< ..., ALUGrid< 2, 2, simplex, conforming >, ... >

#endif // HAVE_ALUGRID

} // namespace SWIPDG
} // namespace internal

template< class SpaceType, class VectorType, class ProblemType, class GridType >
class SWIPDG
{
public:
  typedef typename ProblemType::RangeFieldType RangeFieldType;

private:
  template< class IndividualEstimator, bool available = false >
  class Caller
  {
  public:
    static std::vector< std::string > append(std::vector< std::string > in)
    {
      return in;
    }

    static bool equals(const std::string& /*type*/)
    {
      return false;
    }

    static RangeFieldType estimate(const SpaceType& /*space*/,
                                   const VectorType& /*vector*/,
                                   const ProblemType& /*problem*/)
    {
      DUNE_THROW(Stuff::Exceptions::internal_error, "This should not happen!");
      return RangeFieldType(0);
    }

    static Stuff::LA::CommonDenseVector< RangeFieldType > estimate_local(const SpaceType& /*space*/,
                                                                         const VectorType& /*vector*/,
                                                                         const ProblemType& /*problem*/)
    {
      DUNE_THROW(Stuff::Exceptions::internal_error, "This should not happen!");
      return Stuff::LA::CommonDenseVector< RangeFieldType >();
    }
  }; // class Caller

  template< class IndividualEstimator >
  class Caller< IndividualEstimator, true >
  {
  public:
    static std::vector< std::string > append(std::vector< std::string > in)
    {
      in.push_back(IndividualEstimator::id());
      return in;
    }

    static bool equals(const std::string& type)
    {
      return IndividualEstimator::id() == type;
    }

    static RangeFieldType estimate(const SpaceType& space, const VectorType& vector, const ProblemType& problem)
    {
      return IndividualEstimator::estimate(space, vector, problem);
    }

    static Stuff::LA::CommonDenseVector< RangeFieldType > estimate_local(const SpaceType& space,
                                                                         const VectorType& vector,
                                                                         const ProblemType& problem)
    {
      return IndividualEstimator::estimate_local(space, vector, problem);
    }
  }; // class Caller< ..., true >

  template< class IndividualEstimator >
  static std::vector< std::string > call_append(std::vector< std::string > in)
  {
    return Caller< IndividualEstimator, IndividualEstimator::available >::append(in);
  }

  template< class IndividualEstimator >
  static bool call_equals(const std::string& type)
  {
    return Caller< IndividualEstimator, IndividualEstimator::available >::equals(type);
  }

  template< class IndividualEstimator >
  static RangeFieldType call_estimate(const SpaceType& space, const VectorType& vector, const ProblemType& problem)
  {
    return Caller< IndividualEstimator, IndividualEstimator::available >::estimate(space, vector, problem);
  }

  template< class IndividualEstimator >
  static Stuff::LA::CommonDenseVector< RangeFieldType > call_estimate_local(const SpaceType& space,
                                                                            const VectorType& vector,
                                                                            const ProblemType& problem)
  {
    return Caller< IndividualEstimator, IndividualEstimator::available >::estimate_local(space, vector, problem);
  }

  typedef internal::SWIPDG::LocalNonconformityESV2007
      < SpaceType, VectorType, ProblemType, GridType >              LocalNonconformityESV2007Type;
  typedef internal::SWIPDG::LocalResidualESV2007
      < SpaceType, VectorType, ProblemType, GridType >              LocalResidualESV2007Type;
  typedef internal::SWIPDG::LocalResidualESV2007Star
      < SpaceType, VectorType, ProblemType, GridType >              LocalResidualESV2007StarType;
  typedef internal::SWIPDG::LocalDiffusiveFluxESV2007
      < SpaceType, VectorType, ProblemType, GridType >              LocalDiffusiveFluxESV2007Type;
  typedef internal::SWIPDG::ESV2007
      < SpaceType, VectorType, ProblemType, GridType >              ESV2007Type;
  typedef internal::SWIPDG::ESV2007AlternativeSummation
      < SpaceType, VectorType, ProblemType, GridType >              ESV2007AlternativeSummationType;

public:
  static std::vector< std::string > available()
  {
    std::vector< std::string > tmp;
    tmp = call_append< LocalNonconformityESV2007Type >(tmp);
    tmp = call_append< LocalResidualESV2007Type >(tmp);
    tmp = call_append< LocalResidualESV2007StarType >(tmp);
    tmp = call_append< LocalDiffusiveFluxESV2007Type >(tmp);
    tmp = call_append< ESV2007Type >(tmp);
    tmp = call_append< ESV2007AlternativeSummationType >(tmp);
    return tmp;
  } // ... available(...)

  static std::vector< std::string > available_local()
  {
    std::vector< std::string > tmp;
    tmp = call_append< ESV2007Type >(tmp);
    tmp = call_append< ESV2007AlternativeSummationType >(tmp);
    return tmp;
  } // ... available_local(...)

  static RangeFieldType estimate(const SpaceType& space,
                                 const VectorType& vector,
                                 const ProblemType& problem,
                                 const std::string type)
  {
    if (call_equals< LocalNonconformityESV2007Type >(type))
      return call_estimate< LocalNonconformityESV2007Type >(space, vector, problem);
    else if (call_equals< LocalResidualESV2007Type >(type))
      return call_estimate< LocalResidualESV2007Type >(space, vector, problem);
    else if (call_equals< LocalResidualESV2007StarType >(type))
      return call_estimate< LocalResidualESV2007StarType >(space, vector, problem);
    else if (call_equals< LocalDiffusiveFluxESV2007Type >(type))
      return call_estimate< LocalDiffusiveFluxESV2007Type >(space, vector, problem);
    else if (call_equals< ESV2007Type >(type))
      return call_estimate< ESV2007Type >(space, vector, problem);
    else if (call_equals< ESV2007AlternativeSummationType >(type))
      return call_estimate< ESV2007AlternativeSummationType >(space, vector, problem);
    else
      DUNE_THROW(Stuff::Exceptions::you_are_using_this_wrong,
                 "Requested type '" << type << "' is not one of available()!");
  } // ... estimate(...)

  static Stuff::LA::CommonDenseVector< RangeFieldType > estimate_local(const SpaceType& space,
                                                                       const VectorType& vector,
                                                                       const ProblemType& problem,
                                                                       const std::string type)
  {
    if (call_equals< ESV2007Type >(type))
      return call_estimate_local< ESV2007Type >(space, vector, problem);
    else if (call_equals< ESV2007AlternativeSummationType >(type))
      return call_estimate_local< ESV2007AlternativeSummationType >(space, vector, problem);
    else
      DUNE_THROW(Stuff::Exceptions::you_are_using_this_wrong,
                 "Requested type '" << type << "' is not one of available_local()!");
  } // ... estimate_local(...)
}; // class SWIPDG

} // namespace Discretizations
} // namespace Estimators
} // namespace HDD
} // namespace Dune

#endif // DUNE_HDD_LINEARELLIPTIC_ESTIMATORS_SWIPDG_HH


/** @file
    @brief Implementation

    @date 2016

    @author
    Sensics, Inc.
    <http://sensics.com/osvr>
*/

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

#define KALMANFRAMEWORK_HAVE_BOOST

// Internal Includes
#include "GenerateBlobDebugImage.h"
#include "QuatToEuler.h"
#include "unifiedvideoinertial/CSV.h"
#include "unifiedvideoinertial/ConfigParams.h"
#include "unifiedvideoinertial/ConfigurationParser.h"
#include "unifiedvideoinertial/MakeHDKTrackingSystem.h"
#include "unifiedvideoinertial/MiniArgsHandling.h"
#include "unifiedvideoinertial/TimeValue.h"
#include "unifiedvideoinertial/TrackedBodyTarget.h"
#include "videotrackershared/CameraParameters.h"
#include "videotrackershared/EdgeHoleBasedLedExtractor.h"
#include "videotrackershared/UndistortMeasurements.h"
#include "videotrackershared/cvUtils.h"

// Library/third-party includes
#include <Eigen/Core>
#include <boost/algorithm/string/predicate.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

// Standard includes
#include <chrono>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <stdexcept>

#include "unifiedvideoinertial/CSVCellGroup.h" // must include after Eigen

namespace videotracker {
namespace uvbi {

    namespace {
        struct BodyIdOrdering {
            bool operator()(BodyId const &lhs, BodyId const &rhs) const {
                return lhs.value() < rhs.value();
            }
        };
    } // namespace

    class TrackerOfflineProcessing {
      public:
        TrackerOfflineProcessing(ConfigParams const &initialParams)
            : camParamsDistorted_(getHDKCameraParameters()),
              camParams_(camParamsDistorted_.createUndistortedVariant()),
              params_(initialParams), extractor_(initialParams.extractParams) {
            params_.performingOptimization = true;
            params_.silent = true;
            params_.debug = false;
            params_.streamBeaconDebugInfo = true;

            params_.offsetToCentroid = false;
            params_.includeRearPanel = false;
            params_.imu.path = "";
            params_.imu.useOrientation = false;

            system_ = makeHDKTrackingSystem(params_);
            body_ = &(system_->getBody(bodyIdOfInterest));
            target_ = body_->getTarget(targetIdOfInterest);
        }

        void processFrame(cv::Mat const &frame);

        bool everHadPose() const { return everHadPose_; }
        bool hasPose() const { return hasPose_; }

        cv::Mat getDebugImage();

        void outputCSV(std::ostream &os) { csv_.output(os); }

        /// To get a time that matches the timestamp
        std::size_t getFrameCount() const { return frame_ + 1; }

        /// Return a string with decimal seconds in it, that has never touched
        /// floating point.
        std::string carefullyFormatElapsedTime() const;

        using FrameTimeUnit = std::chrono::microseconds;

      private:
        /// Substitute for performInitialImageProcessing that lets us get more
        /// of the innards: sets rawMeasurements_, undistortedMeasurements_, and
        /// leaves useful state in extractor_.
        ImageOutputDataPtr imageProc(cv::Mat const &frame);
        void logRow();

        /// @name Constants
        /// @{
        const int fps_ = 100;
        const FrameTimeUnit frameTime_ =
            std::chrono::duration_cast<FrameTimeUnit>(std::chrono::seconds(1)) /
            fps_;

        const BodyId bodyIdOfInterest = BodyId(0);
        const TargetId targetIdOfInterest = TargetId(0);
        /// @}

        /// Get the whole number of seconds passed in the simulation.
        std::chrono::seconds getSeconds() const;

        /// Get whatever time passed that's left after subtracting all whole
        /// seconds (value will be less than a second).
        FrameTimeUnit getFractionalRemainder() const;

        const CameraParameters camParamsDistorted_;
        const CameraParameters camParams_;
        ConfigParams params_;
        EdgeHoleBasedLedExtractor extractor_;
        std::unique_ptr<TrackingSystem> system_;
        TrackedBody *body_ = nullptr;
        TrackedBodyTarget *target_ = nullptr;
        util::TimeValue currentTime_ = {};
        LedMeasurementVec rawMeasurements_;
        LedMeasurementVec undistortedMeasurements_;
        cv::Mat lastFrame_;
        util::CSV csv_;
        std::size_t frame_ = 0;
        bool hasPose_ = false;
        bool everHadPose_ = false;
    };

    void TrackerOfflineProcessing::processFrame(cv::Mat const &frame) {
        if ((frame_ % 100) == 0) {
            std::cout << "Processing frame " << frame_ << std::endl;
        }
        /// Advance the clock
        currentTime_.microseconds += frameTime_.count();
        uvbiTimeValueNormalize(&currentTime_);

        /// Image processing.
        auto imageData = imageProc(frame);

        /// Hand off the image processing results
        auto indices = system_->updateBodiesFromVideoData(std::move(imageData));
        logRow();
        frame_++;
    }

    cv::Mat TrackerOfflineProcessing::getDebugImage() {
        cv::Mat input = extractor_.getInputGrayImage();
        const cv::Size sz = input.size();
        /// Returns a rect for a ROI allowing us to build up our debug image
        /// from blocks.
        auto getRect = [&](std::size_t i) {
            return cv::Rect(cv::Point2i(i * sz.width, 0), sz);
        };

        const cv::Size largerSize{sz.width * 4, sz.height};
        cv::Mat composite{};
        composite.create(largerSize, CV_8UC3);
        /// Copies a block into our debug image.
        auto copyToComposite = [&](cv::Mat const &mat, std::size_t i) {
            cv::Mat colorSrc;
            if (mat.channels() == 1) {
                cv::cvtColor(mat, colorSrc, cv::COLOR_GRAY2BGR);
            } else {
                colorSrc = mat;
            }
            colorSrc.copyTo(composite(getRect(i)));
        };

        copyToComposite(input, 0);
        copyToComposite(extractor_.getEdgeDetectedImage(), 1);
        copyToComposite(extractor_.getEdgeDetectedBinarizedImage(), 2);
        copyToComposite(generateBlobDebugImage(lastFrame_, extractor_), 3);

        return composite;
    }

    ImageOutputDataPtr
    TrackerOfflineProcessing::imageProc(cv::Mat const &frame) {
        lastFrame_ = frame.clone();
        ImageOutputDataPtr ret(new ImageProcessingOutput);
        ret->tv = currentTime_;
        ret->frame = frame;
        cv::Mat gray;
        cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
        ret->frameGray = gray;
        ret->camParams = camParams_; // undistorted!
        rawMeasurements_ = extractor_(gray, params_.blobParams, false);
        undistortedMeasurements_ =
            undistortLeds(rawMeasurements_, camParamsDistorted_);
        ret->ledMeasurements = undistortedMeasurements_;
        return ret;
    }

    void TrackerOfflineProcessing::logRow() {
        using namespace util;
        auto row = csv_.row();
#if 0
        // time as the two-part time value
        row << cellGroup(currentTime_);
#endif

        // time as a pristinely-formatted decimal number of seconds
        row << cell("Time", carefullyFormatElapsedTime());

        hasPose_ = body_->hasPoseEstimate();
        everHadPose_ |= hasPose_;

        row << cell("TrackerDropped", hasPose_ ? "" : "0");

        if (hasPose_) {
            Eigen::Quaterniond quat = body_->getState().getQuaternion();
            Eigen::Vector3d xlate = body_->getState().position();
            row << cellGroup(xlate) << cellGroup(quat)
                << cellGroup<QuatAsEulerTag>(quat);
        }
        row << cell("Measurements", rawMeasurements_.size());

        std::size_t numArea = 0;
        std::size_t numCenterPointValue = 0;
        std::size_t numCircularity = 0;
        std::size_t numConvexity = 0;
        for (auto &reject : extractor_.getRejectList()) {
            RejectReason reason = std::get<1>(reject);

            switch (reason) {
            case RejectReason::Area:
                numArea++;
                break;
            case RejectReason::CenterPointValue:
                numCenterPointValue++;
                break;
            case RejectReason::Circularity:
                numCircularity++;
                break;
            case RejectReason::Convexity:
                numConvexity++;
                break;
            default:
                break;
            }
        }

        auto getStatus = [&](TargetStatusMeasurement meas) {
            return target_->getInternalStatusMeasurement(meas);
        };
        row << cell("Rejects.Area", numArea)
            << cell("Rejects.CenterPointValue", numCenterPointValue)
            << cell("Rejects.Circularity", numCircularity)
            << cell("Rejects.Convexity", numConvexity)
            << cell("Leds", getStatus(TargetStatusMeasurement::NumUsableLeds))
            << cell("UsedLeds",
                    getStatus(TargetStatusMeasurement::NumUsedLeds));
        if (everHadPose_) {
            row << cell("PosErrorVariance",
                        getStatus(TargetStatusMeasurement::MaxPosErrorVariance))
                << cell("PosErrorVarianceBound",
                        getStatus(
                            TargetStatusMeasurement::PosErrorVarianceLimit));
        }
    }

    /// Takes a std::ratio like std::micro and counts the number of digits to
    /// the right of the decimal point we need to use.
    template <typename Rat> inline std::size_t countDigits() {
        auto denom = Rat::den;
        auto ret = std::size_t{0};
        while (denom > 1) {
            denom /= 10;
            ret++;
        }
        return ret;
    }

    inline std::string
    TrackerOfflineProcessing::carefullyFormatElapsedTime() const {
        std::ostringstream os;
        os << getSeconds().count() << ".";
        using FractionalType = decltype(getFractionalRemainder());
        using FractionalPeriod = FractionalType::period;
        static const auto numDigits = countDigits<FractionalPeriod>();
        os << std::setw(numDigits) << std::setfill('0')
           << getFractionalRemainder().count();
        return os.str();
    }

    inline std::chrono::seconds TrackerOfflineProcessing::getSeconds() const {
        return std::chrono::seconds(getFrameCount() / fps_);
    }

    inline TrackerOfflineProcessing::FrameTimeUnit
    TrackerOfflineProcessing::getFractionalRemainder() const {
        auto remainderFrames = getFrameCount() % fps_;
        auto ret = remainderFrames * frameTime_;
        VIDEOTRACKER_ASSERT_MSG(
            ret < std::chrono::seconds(1),
            "This is a remainder - it must be less than a second!");
        return ret;
    }

    static bool g_saveFramesLostFix = false;

    bool processAVI(std::string const &fn, TrackerOfflineProcessing &app) {
        cv::VideoCapture capture;
        capture.open(fn);
        if (!capture.isOpened()) {
            std::cerr << "Could not open video file " << fn << std::endl;
            return false;
        }
        cv::Mat frame;
        capture >> frame;
        while (capture.read(frame)) {
            app.processFrame(frame);
            if (g_saveFramesLostFix && !app.hasPose() && app.everHadPose()) {
                // we had pose but lost it
                std::ostringstream os;
                os << fn << "." << app.carefullyFormatElapsedTime() << ".png";
                cv::Mat image = app.getDebugImage();
                cv::imwrite(os.str(), image);
            }
        }
        return true;
    }

} // namespace uvbi
} // namespace videotracker

static const auto DEBUG_FRAMES_SWITCH = "--save-debug-frames";

using namespace videotracker::util::args;
int main(int argc, char *argv[]) {

    bool gotParams = false;
    videotracker::uvbi::ConfigParams params;
    std::vector<std::string> videoNames;
    auto args = makeArgList(argc, argv);
    try {
        /// parse json file arguments.
        auto numJson = handle_arg(args, [&](std::string const &arg) {
            if (!boost::iends_with(arg, ".json")) {
                return false;
            }
            std::ifstream configFile(arg);
            if (!configFile) {
                std::cerr << "Tried to load " << arg
                          << " as a config file but could not open it!"
                          << std::endl;
                throw std::invalid_argument(
                    "Could not open json config file passed");
            }
            Json::Value root;
            Json::Reader reader;
            if (!reader.parse(configFile, root)) {
                std::cerr << "Could not parse " << arg << " as JSON! "
                          << reader.getFormattedErrorMessages() << std::endl;
                throw std::runtime_error(
                    "Config file could not be parsed as JSON!");
            }
            params = videotracker::uvbi::parseConfigParams(root);
            return true;
        });
        if (numJson > 1) {
            std::cerr << "At most one .json config file passed to this app!"
                      << std::endl;
            return -1;
        }

        /// Get video filename
        auto numVideoNames = handle_arg(args, [&](std::string const &arg) {
            auto ret = boost::iends_with(arg, ".avi");
            if (ret) {
                videoNames.push_back(arg);
            }
            return ret;
        });
        if (numVideoNames < 1) {
            std::cerr << "Must pass at least one video filename to this app!"
                      << std::endl;
            return -1;
        }

        videotracker::uvbi::g_saveFramesLostFix =
            handle_has_iswitch(args, DEBUG_FRAMES_SWITCH);
        if (videotracker::uvbi::g_saveFramesLostFix) {
            std::cout << "Will save debug frames when tracker loses fix"
                      << std::endl;
        }

        if (!args.empty()) {
            std::cerr
                << "Unrecognized arguments left after parsing command line!"
                << std::endl;
            return -1;
        }
    } catch (std::exception &e) {
        std::cerr << e.what() << std::endl;
        return -1;
    }

    /// 0 is everything successful - each error, we increment...
    int returnValue = 0;
    for (auto &videoName : videoNames) {
        std::cout << "Processing input video " << videoName << std::endl;
        videotracker::uvbi::TrackerOfflineProcessing app(params);
        auto success = videotracker::uvbi::processAVI(videoName, app);

        if (success) {
            std::cout << "Processed a total of " << app.getFrameCount()
                      << " frames." << std::endl;
            auto outname = videoName + ".csv";
            std::cout << "Writing output data to: " << outname << std::endl;
            std::ofstream of(outname);
            if (!of) {
                std::cout << "Can't write to that file!" << std::endl;
                returnValue++;
            } else {
                app.outputCSV(of);
            }
            std::cout << "File finished!\n\n" << std::endl;
        } else {
            std::cout << "File skipped!\n\n" << std::endl;
            returnValue++;
        }
    }

    if (returnValue != 0) {
        std::cerr << "One or more errors! Press enter to exit after reviewing "
                     "the errors."
                  << std::endl;
        std::cin.ignore();
    }

    return returnValue;
}


#include "BBBoundingBox.h"
#include "Utils/BBUtils.h"
#include <cfloat>
#include "Render/BufferObject/BBVertexBufferObject.h"
#include "Render/BBMaterial.h"
#include "Render/BBDrawCall.h"
#include <Eigen/Eigen>
#include "Render/BBCamera.h"

using namespace Eigen;

/**
 * @brief BBBoundingBox::BBBoundingBox
 */
BBBoundingBox::BBBoundingBox()
    : BBBoundingBox(0, 0, 0, 0, 0, 0, 1, 1, 1)
{

}

BBBoundingBox::BBBoundingBox(float px, float py, float pz,
                             float rx, float ry, float rz,
                             float sx, float sy, float sz)
    : BBRenderableObject(px, py, pz, rx, ry, rz, sx, sy, sz)
{
    m_nBoxVertexCount = 0;
    m_pOriginalBoxVertexes = NULL;
    m_pTransformedBoxVertexes = NULL;
    m_DefaultColor = QVector3D(0.909804f, 0.337255f, 0.333333f);
}

BBBoundingBox::~BBBoundingBox()
{
    BB_SAFE_DELETE_ARRAY(m_pOriginalBoxVertexes);
    BB_SAFE_DELETE_ARRAY(m_pTransformedBoxVertexes);
}

bool BBBoundingBox::hit(const BBRay &ray, float &fDistance)
{
    Q_UNUSED(ray);
    Q_UNUSED(fDistance);
    return m_bActive;
}

QVector3D BBBoundingBox::getCenter()
{
    return QVector3D(m_Center[0], m_Center[1], m_Center[2]);
}

void BBBoundingBox::setModelMatrix(float px, float py, float pz,
                                   const QQuaternion &r,
                                   float sx, float sy, float sz)
{
    // When the corresponding model is transformed, invoke
    BBGameObject::setModelMatrix(px, py, pz, r, sx, sy, sz);
    for (int i = 0; i < m_nBoxVertexCount; i++)
    {
        m_pTransformedBoxVertexes[i] = m_ModelMatrix * m_pOriginalBoxVertexes[i];
    }
}

/**
 * @brief BBRectBoundingBox3D::BBRectBoundingBox3D
 * @param fCenterX
 * @param fCenterY
 * @param fCenterZ
 * @param fHalfLengthX
 * @param fHalfLengthY
 * @param fHalfLengthZ
 */
BBRectBoundingBox3D::BBRectBoundingBox3D(float fCenterX, float fCenterY, float fCenterZ,
                                         float fHalfLengthX, float fHalfLengthY, float fHalfLengthZ)
    : BBBoundingBox()
{
    m_nBoxVertexCount = 4;
    m_pOriginalBoxVertexes = new QVector3D[m_nBoxVertexCount];
    m_pTransformedBoxVertexes = new QVector3D[m_nBoxVertexCount];

    m_Center[0] = fCenterX;
    m_Center[1] = fCenterY;
    m_Center[2] = fCenterZ;
    if (fHalfLengthX == 0)
    {
        m_pOriginalBoxVertexes[0] = QVector3D(fCenterX, fCenterY + fHalfLengthY, fCenterZ + fHalfLengthZ);
        m_pOriginalBoxVertexes[1] = QVector3D(fCenterX, fCenterY - fHalfLengthY, fCenterZ + fHalfLengthZ);
        m_pOriginalBoxVertexes[2] = QVector3D(fCenterX, fCenterY - fHalfLengthY, fCenterZ - fHalfLengthZ);
        m_pOriginalBoxVertexes[3] = QVector3D(fCenterX, fCenterY + fHalfLengthY, fCenterZ - fHalfLengthZ);
    }
    else if (fHalfLengthY == 0)
    {
        m_pOriginalBoxVertexes[0] = QVector3D(fCenterX + fHalfLengthX, fCenterY, fCenterZ + fHalfLengthZ);
        m_pOriginalBoxVertexes[1] = QVector3D(fCenterX - fHalfLengthX, fCenterY, fCenterZ + fHalfLengthZ);
        m_pOriginalBoxVertexes[2] = QVector3D(fCenterX - fHalfLengthX, fCenterY, fCenterZ - fHalfLengthZ);
        m_pOriginalBoxVertexes[3] = QVector3D(fCenterX + fHalfLengthX, fCenterY, fCenterZ - fHalfLengthZ);
    }
    else if (fHalfLengthZ == 0)
    {
        m_pOriginalBoxVertexes[0] = QVector3D(fCenterX + fHalfLengthX, fCenterY + fHalfLengthY, fCenterZ);
        m_pOriginalBoxVertexes[1] = QVector3D(fCenterX - fHalfLengthX, fCenterY + fHalfLengthY, fCenterZ);
        m_pOriginalBoxVertexes[2] = QVector3D(fCenterX - fHalfLengthX, fCenterY - fHalfLengthY, fCenterZ);
        m_pOriginalBoxVertexes[3] = QVector3D(fCenterX + fHalfLengthX, fCenterY - fHalfLengthY, fCenterZ);
    }

    m_pTransformedBoxVertexes[0] = m_pOriginalBoxVertexes[0];
    m_pTransformedBoxVertexes[1] = m_pOriginalBoxVertexes[1];
    m_pTransformedBoxVertexes[2] = m_pOriginalBoxVertexes[2];
    m_pTransformedBoxVertexes[3] = m_pOriginalBoxVertexes[3];
}

BBRectBoundingBox3D::~BBRectBoundingBox3D()
{

}

void BBRectBoundingBox3D::init()
{
    m_pVBO = new BBVertexBufferObject(m_nBoxVertexCount);
    for (int i = 0; i < m_nBoxVertexCount; i++)
    {
        m_pVBO->setPosition(i, m_pOriginalBoxVertexes[i].x(),
                               m_pOriginalBoxVertexes[i].y(),
                               m_pOriginalBoxVertexes[i].z());
        m_pVBO->setColor(i, m_DefaultColor);
    }

    m_nIndexCount = 8;
    unsigned short indexes[] = {0, 1, 1, 2, 2, 3, 3, 0};
    m_pIndexes = new unsigned short[m_nIndexCount];
    for (int i = 0; i < m_nIndexCount; i++)
    {
        m_pIndexes[i] = indexes[i];
    }

    m_pCurrentMaterial->init("UI",
                             BB_PATH_RESOURCE_SHADER(UI.vert),
                             BB_PATH_RESOURCE_SHADER(UI.frag));
    m_pCurrentMaterial->setVector4(LOCATION_TEXTURE_SETTING0, 0.0f, 0.0f, 0.0f, 0.0f);
    m_pCurrentMaterial->setVector4(LOCATION_CAMERA_PARAMETERS0, 800.0f, 600.0f, 0.0f, 0.0f);

    BBRenderableObject::init();

    BBDrawCall *pDrawCall = new BBDrawCall;
    pDrawCall->setMaterial(m_pCurrentMaterial);
    pDrawCall->setVBO(m_pVBO);
    pDrawCall->setEBO(m_pEBO, GL_LINES, m_nIndexCount, 0);
    appendDrawCall(pDrawCall);
}

bool BBRectBoundingBox3D::hit(const BBRay &ray, float &fDistance)
{
    // If it is not activated, no collision occurs
    if (!BBBoundingBox::hit(ray, fDistance))
        return false;

    QVector3D intersection;
    bool result = false;
    fDistance = FLT_MAX;

    if (ray.computeIntersectWithRectangle(m_pTransformedBoxVertexes[0],
                                          m_pTransformedBoxVertexes[1],
                                          m_pTransformedBoxVertexes[2],
                                          m_pTransformedBoxVertexes[3], intersection))
    {
        float tmp = ray.computeIntersectDistance(intersection);
        if (tmp >= 0.0f)
        {
            result = true;
            fDistance = tmp;
        }
    }

    return result;
}

/**
 * @brief BBTriangleBoundingBox3D::BBTriangleBoundingBox3D
 * @param point1
 * @param point2
 * @param point3
 */
BBTriangleBoundingBox3D::BBTriangleBoundingBox3D(const QVector3D &point1,
                                                 const QVector3D &point2,
                                                 const QVector3D &point3)
    : BBBoundingBox()
{
    m_nBoxVertexCount = 3;
    m_pOriginalBoxVertexes = new QVector3D[m_nBoxVertexCount];
    m_pTransformedBoxVertexes = new QVector3D[m_nBoxVertexCount];

    m_pOriginalBoxVertexes[0] = point1;
    m_pOriginalBoxVertexes[1] = point2;
    m_pOriginalBoxVertexes[2] = point3;
}

bool BBTriangleBoundingBox3D::hit(const BBRay &ray, float &fDistance)
{
    // If it is not activated, no collision occurs
    if (!BBBoundingBox::hit(ray, fDistance))
        return false;

    QVector3D intersection;
    bool result = false;
    fDistance = FLT_MAX;

    if (ray.computeIntersectWithTriangle(m_pTransformedBoxVertexes[0],
                                         m_pTransformedBoxVertexes[1],
                                         m_pTransformedBoxVertexes[2], intersection))
    {
        float tmp = ray.computeIntersectDistance(intersection);
        if (tmp >= 0.0f)
        {
            result = true;
            fDistance = tmp;
        }
    }
    return result;
}

/**
 * @brief BBQuarterCircleBoundingBox3D::BBQuarterCircleBoundingBox3D
 * @param fCenterX
 * @param fCenterY
 * @param fCenterZ
 * @param fRadius
 * @param ePlaneName
 */
BBQuarterCircleBoundingBox3D::BBQuarterCircleBoundingBox3D(float fCenterX, float fCenterY, float fCenterZ,
                                                           float fRadius, const BBPlaneName &ePlaneName)
    : BBBoundingBox()
{
    m_nBoxVertexCount = 1;
    m_pOriginalBoxVertexes = new QVector3D[m_nBoxVertexCount];
    m_pTransformedBoxVertexes = new QVector3D[m_nBoxVertexCount];

    m_Center[0] = fCenterX;
    m_Center[1] = fCenterY;
    m_Center[2] = fCenterZ;

    m_eSelectedPlaneName = ePlaneName;

    m_QuadrantFlag = QVector3D(0, 0, 0);

    if (ePlaneName == BBPlaneName::YOZ)
    {
        m_pOriginalBoxVertexes[0] = QVector3D(m_Center[0], m_Center[1], m_Center[2]) + QVector3D(0, fRadius, 0);
    }
    else // XOZ or XOY
    {
        m_pOriginalBoxVertexes[0] = QVector3D(m_Center[0], m_Center[1], m_Center[2]) + QVector3D(fRadius, 0, 0);
    }
}

bool BBQuarterCircleBoundingBox3D::hit(const BBRay &ray, float &fDistance)
{
    // If it is not activated, no collision occurs
    if (!BBBoundingBox::hit(ray, fDistance))
        return false;

    QVector3D intersection;
    bool result = false;
    fDistance = FLT_MAX;

    if (ray.computeIntersectWithQuarterCircle(m_Position,
                                              (m_pTransformedBoxVertexes[0] - m_Position).length(),
                                              m_eSelectedPlaneName,
                                              intersection,
                                              m_QuadrantFlag))
    {
        float tmp = ray.computeIntersectDistance(intersection);
        if (tmp >= 0.0f)
        {
            result = true;
            fDistance = tmp;
        }
    }
    return result;
}

/**
 * @brief BBBoundingBox3D::BBBoundingBox3D
 * @param vertexes
 */
BBBoundingBox3D::BBBoundingBox3D(const QList<QVector4D> &vertexes)
    : BBBoundingBox3D(0, 0, 0, 0, 0, 0, 1, 1, 1, vertexes)
{

}

BBBoundingBox3D::BBBoundingBox3D(float px, float py, float pz,
                                 float rx, float ry, float rz,
                                 float sx, float sy, float sz,
                                 const QList<QVector4D> &vertexes)
    : BBBoundingBox(px, py, pz, rx, ry, rz, sx, sy, sz)
{
    m_nBoxVertexCount = 8;
    m_pOriginalBoxVertexes = new QVector3D[m_nBoxVertexCount];
    m_pTransformedBoxVertexes = new QVector3D[m_nBoxVertexCount];

    m_DefaultColor = QVector3D(0.909804f, 0.337255f, 0.333333f);
    m_Center[0] = 0;
    m_Center[1] = 0;
    m_Center[2] = 0;
    m_HalfLength[0] = 0.5f;
    m_HalfLength[1] = 0.5f;
    m_HalfLength[2] = 0.5f;
    m_Axis[0][0] = 1;
    m_Axis[0][1] = 0;
    m_Axis[0][2] = 0;
    m_Axis[1][0] = 0;
    m_Axis[1][1] = 1;
    m_Axis[1][2] = 0;
    m_Axis[2][0] = 0;
    m_Axis[2][1] = 0;
    m_Axis[2][2] = 1;

    computeBoxVertexes(vertexes);
}

BBBoundingBox3D::BBBoundingBox3D(const QVector3D &position, const QVector3D &rotation, const QVector3D &scale,
                                 const QVector3D &center, const QVector3D &halfLength)
    : BBBoundingBox3D(position.x(), position.y(), position.z(),
                      rotation.x(), rotation.y(), rotation.z(),
                      scale.x(), scale.y(), scale.z(),
                      center.x(), center.y(), center.z(),
                      halfLength.x(), halfLength.y(), halfLength.z())
{

}

BBBoundingBox3D::BBBoundingBox3D(float px, float py, float pz,
                                 float rx, float ry, float rz,
                                 float sx, float sy, float sz,
                                 float fCenterX, float fCenterY, float fCenterZ,
                                 float fHalfLengthX, float fHalfLengthY, float fHalfLengthZ)
    : BBBoundingBox(px, py, pz, rx, ry, rz, sx, sy, sz)
{
    m_nBoxVertexCount = 8;
    m_pOriginalBoxVertexes = new QVector3D[m_nBoxVertexCount];
    m_pTransformedBoxVertexes = new QVector3D[m_nBoxVertexCount];

    m_Center[0] = fCenterX;
    m_Center[1] = fCenterY;
    m_Center[2] = fCenterZ;
    m_HalfLength[0] = fHalfLengthX;
    m_HalfLength[1] = fHalfLengthY;
    m_HalfLength[2] = fHalfLengthZ;
    m_Axis[0][0] = 1;
    m_Axis[0][1] = 0;
    m_Axis[0][2] = 0;
    m_Axis[1][0] = 0;
    m_Axis[1][1] = 1;
    m_Axis[1][2] = 0;
    m_Axis[2][0] = 0;
    m_Axis[2][1] = 0;
    m_Axis[2][2] = 1;

    QList<QVector4D> vertexes;
    computeBoxVertexes(vertexes);
}

BBBoundingBox3D::~BBBoundingBox3D()
{

}

void BBBoundingBox3D::init()
{
    m_pVBO = new BBVertexBufferObject(8);
    for (int i = 0; i < 8; i++)
    {
        m_pVBO->setPosition(i, m_pOriginalBoxVertexes[i].x(),
                               m_pOriginalBoxVertexes[i].y(),
                               m_pOriginalBoxVertexes[i].z());
        m_pVBO->setColor(i, m_DefaultColor);
    }

    m_nIndexCount = 24;
    unsigned short indexes[] = {0, 1, 1, 2, 2, 3, 3, 0, 4, 5, 5, 6, 6, 7, 7, 4, 0, 4, 1, 5, 2, 6, 3, 7};
    m_pIndexes = new unsigned short[m_nIndexCount];
    for (int i = 0; i < m_nIndexCount; i++)
    {
        m_pIndexes[i] = indexes[i];
    }

    m_pCurrentMaterial->init("base",
                             BB_PATH_RESOURCE_SHADER(base.vert),
                             BB_PATH_RESOURCE_SHADER(base.frag));

    BBRenderableObject::init();

    BBDrawCall *pDrawCall = new BBDrawCall;
    pDrawCall->setMaterial(m_pCurrentMaterial);
    pDrawCall->setVBO(m_pVBO);
    pDrawCall->setEBO(m_pEBO, GL_LINES, m_nIndexCount, 0);
    appendDrawCall(pDrawCall);
}

bool BBBoundingBox3D::hit(const BBRay &ray, float &fDistance)
{
    // If it is not activated, no collision occurs
    if (!BBBoundingBox::hit(ray, fDistance))
        return false;

    QVector3D intersection;
    bool result = false;
    fDistance = FLT_MAX;
    // Calculate whether to hit the 6 faces of the bounding box
    static unsigned int indexes[] = {0, 1, 2, 3, 4, 5, 6, 7,
                                     1, 2, 6, 5, 0, 3, 7, 4,
                                     0, 1, 5, 4, 3, 2, 6, 7};
    for (int i = 0; i < 24; i += 4)
    {
        if (ray.computeIntersectWithRectangle(m_pTransformedBoxVertexes[indexes[i]],
                                              m_pTransformedBoxVertexes[indexes[i + 1]],
                                              m_pTransformedBoxVertexes[indexes[i + 2]],
                                              m_pTransformedBoxVertexes[indexes[i + 3]], intersection))
        {
            float temp = ray.computeIntersectDistance(intersection);
            if (temp >= 0.0f)
            {
                if (temp < fDistance)
                    fDistance = temp;
                result = true;
            }
        }
    }
    return result;
}

bool BBBoundingBox3D::belongToSelectionRegion(const BBFrustum &frustum)
{
    for (int i = 0; i < m_nBoxVertexCount; i++)
    {
        BB_PROCESS_ERROR_RETURN_FALSE(frustum.contain(m_pTransformedBoxVertexes[i]));
    }
    return true;
}

QVector3D BBBoundingBox3D::getHalfLength()
{
    return QVector3D(m_HalfLength[0], m_HalfLength[1], m_HalfLength[2]);
}

void BBBoundingBox3D::computeBoxVertexes(const QList<QVector4D> &vertexes)
{
    Q_UNUSED(vertexes);
    FloatData temp[8];
    int sign[8][3] = {{1, 1, 1},
                      {-1, 1, 1},
                      {-1, -1, 1},
                      {1, -1, 1},
                      {1, 1, -1},
                      {-1, 1, -1},
                      {-1, -1, -1},
                      {1, -1, -1}};
    for (int index = 0; index < 8; index++)
    {
        for (int i = 0; i < 3; i++)
        {
            temp[index].v[i] = m_Center[i];
            for (int j = 0; j < 3; j++)
            {
                temp[index].v[i] += m_Axis[j][i] * m_HalfLength[j] * sign[index][j];
            }
        }
        m_pOriginalBoxVertexes[index] = QVector3D(temp[index].v[0], temp[index].v[1], temp[index].v[2]);
        m_pTransformedBoxVertexes[index] = m_ModelMatrix * m_pOriginalBoxVertexes[index];
    }
}

/**
 * @brief BBAABBBoundingBox3D::BBAABBBoundingBox3D
 * @param vertexes
 */
BBAABBBoundingBox3D::BBAABBBoundingBox3D(const QList<QVector4D> &vertexes)
    : BBAABBBoundingBox3D(0, 0, 0, 0, 0, 0, 1, 1, 1, vertexes)
{

}

BBAABBBoundingBox3D::BBAABBBoundingBox3D(float px, float py, float pz,
                                         float rx, float ry, float rz,
                                         float sx, float sy, float sz,
                                         const QList<QVector4D> &vertexes)
    : BBBoundingBox3D(px, py, pz, rx, ry, rz, sx, sy, sz, vertexes)
{
    m_HalfLength[0] = 0;
    m_HalfLength[1] = 0;
    m_HalfLength[2] = 0;
    computeBoxVertexes(vertexes);
}

BBAABBBoundingBox3D::BBAABBBoundingBox3D(const QVector3D &position, const QVector3D &rotation, const QVector3D &scale,
                                         const QVector3D &center, const QVector3D &halfLength)
    : BBBoundingBox3D(position, rotation, scale, center, halfLength)
{

}

bool BBAABBBoundingBox3D::computeIntersectWithPlane(const QVector3D &point, const QVector3D &normal)
{
    // The vertex closest to the plane
    QVector3D p = getMin();
    if (normal.x() >= 0)
        p.setX(getMax().x());
    if (normal.y() >= 0)
        p.setY(getMax().y());
    if (normal.z() >= 0)
        p.setZ(getMax().z());
    // The vertex of the farthest diagonal
    QVector3D n = getMax();
    if (normal.x() >= 0)
        n.setX(getMin().x());
    if (normal.y() >= 0)
        n.setY(getMin().y());
    if (normal.z() >= 0)
        n.setZ(getMin().z());

    if (p.distanceToPlane(point, normal) < 0)
        return false;

    if (n.distanceToPlane(point, normal) < 0)
        return true;

    return false;
}

void BBAABBBoundingBox3D::computeBoxVertexes(const QList<QVector4D> &vertexes)
{
    int nVertexCount = vertexes.count();

    // init matrix
    MatrixXf m(3, nVertexCount);
    for (int i = 0; i < nVertexCount; i++)
    {
        m(0, i) = vertexes.at(i).x();
        m(1, i) = vertexes.at(i).y();
        m(2, i) = vertexes.at(i).z();
    }
    // Find the mean value of x y z, center
    VectorXf avg = m.rowwise().mean();
    // compute the distance from each point to the center
    m.colwise() -= avg;
    for (int i = 0; i < 3; i++)
    {
        m_Center[i] = avg[i];
        for (int j = 0; j < nVertexCount; j++)
        {
            m_HalfLength[i] = std::max(m_HalfLength[i], std::fabs(m(i, j)));
        }
    }

    BBBoundingBox3D::computeBoxVertexes(vertexes);
}

QVector3D BBAABBBoundingBox3D::getMax()
{
    return m_pTransformedBoxVertexes[0];
}

QVector3D BBAABBBoundingBox3D::getMin()
{
    return m_pTransformedBoxVertexes[6];
}

//void OBBBoundingBox3D::computeBoundingBox()
//{
//    int vertexCount = mVertexes.count();

//    //初始化矩阵
//    MatrixXf input(3, vertexCount);
//    MatrixXf m(3, vertexCount);
//    for (int i = 0; i < vertexCount; i++)
//    {
//        m(0, i) = mVertexes.at(i).x();
//        m(1, i) = mVertexes.at(i).y();
//        m(2, i) = mVertexes.at(i).z();
//    }
//    input = m;
//    //cout << input << endl << endl;
//    //求取x y z均值
//    VectorXf avg = m.rowwise().mean();
//    m.colwise() -= avg;
//    //计算协方差矩阵 adjoint转置
//    MatrixXf covMat = (m * m.adjoint()) / float(vertexCount - 1);
//    //cout << covMat << endl << endl;
//    //求特征向量和特征值
//    SelfAdjointEigenSolver<MatrixXf> eigen(covMat);
//    MatrixXf eigenvectors = eigen.eigenvectors();
//    //cout << eigenvectors << endl << endl;
//    //将各点的（X，Y，Z）坐标投影到计算出的坐标轴上 avg由累加所有点再求均值得到 求出center和半长度
//    VectorXf minExtents(3);
//    VectorXf maxExtents(3);
//    minExtents << FLT_MAX, FLT_MAX, FLT_MAX;
//    maxExtents << -FLT_MAX, -FLT_MAX, -FLT_MAX;
//    for (int i = 0; i < vertexCount; i++)
//    {
//        VectorXf vertex = input.col(i);
//        VectorXf displacement = vertex - avg;
//        for (int j = 0; j < 3; j++)
//        {
//            minExtents[j] = min(minExtents[j], displacement.dot(eigenvectors.col(j)));
//            maxExtents[j] = max(maxExtents[j], displacement.dot(eigenvectors.col(j)));
//        }
//    }
//    VectorXf offset = (maxExtents - minExtents) / 2.0f + minExtents;
//    for (int i = 0; i < 3; i++)
//    {
//        avg += eigenvectors.col(i) * offset[i];
//    }

//    for (int i = 0; i < 3; i++)
//    {
//        center[i] = avg[i];
//        radius[i] = (maxExtents[i] - minExtents[i]) / 2.0f;
//        for (int j = 0; j < 3; j++)
//            axis[i][j] = eigenvectors.col(i)[j];
//    }
//    /*qDebug() << center[0] << center[1] << center[2];
//    qDebug() << radius[0] << radius[1] << radius[2];
//    qDebug() << axis[0][0] << axis[0][1] << axis[0][2];
//    qDebug() << axis[1][0] << axis[1][1] << axis[1][2];
//    qDebug() << axis[2][0] << axis[2][1] << axis[2][2];
//    qDebug() << endl;*/
//    getBoundingBoxVertexes();
//}


#ifndef SIMULATION_HPP
#define SIMULATION_HPP

#include <iostream>
#include <iomanip>
#include <sstream>
#include <string>
#include <boost/shared_ptr.hpp>

#include "config.hpp"
#include "values.hpp"
#include "mesh.hpp"
#include "physicalParams.hpp"
#include "init.hpp"
#include "output.hpp"

typedef boost::shared_ptr<Event> EventPtr;
typedef std::vector<EventPtr> EventCollection;
typedef EventCollection::iterator EventIterator;

struct BoreParams {
  RealType x0, Hl, ul, vl;
  RealType S;
  BoreParams():
    x0(0.), Hl(1.), ul(0.), vl(0.), S(0.) {}
};

struct GaussianLandslideParams {
  RealType A, v, lx, ly;
  GaussianLandslideParams():
    A(0.), v(1.), lx(0.), ly(0.) {}
};

struct InitExpression {
  std::string eta;
  std::string U;
  std::string V;
  std::string bathymetry;
  std::string bathyrelative;
  InitExpression():
    eta(""), U(""), V(""), bathymetry(""), bathyrelative("") {}
};

class Simulation {
public:
  BoreParams bore_params;
  GaussianLandslideParams gaussian_landslide_params;
  PhysicalParams Params;
  Timer EventTimer;
  std::string EventName, EventFilename;
  unsigned int BoundaryRegionNumber;
  Timer timer;
  RealType FinalTime, Dtmax;
  RealType CFL;
  RealType Mn;
  unsigned int Spherical;
  std::string MeshFileName;
  Point LocationPoint;
  Mesh mesh;
  // mathematical formulas for Initial value and bathymetry
  InitExpression InitFormulas;
  EventCollection events;
  Values CellValues;
  std::string InitVar;
  std::string InitFilename;
  std::string InitFormula;
  ScalarValue Bathymetry;
  ScalarValue BathyRelative;
  //yac::controller mathParser;
  Simulation();
  Simulation( istream &is );
  void init();
private:

};

Simulation::Simulation():
  FinalTime(INFTY), Dtmax(INFTY), CellValues(10000), Spherical(0),
  InitFilename(""), InitFormula("") {};

void Simulation::init() {

  std::ifstream ifs( MeshFileName.c_str() );
  // std::string mesh_str MeshFileName;

  if (MeshFileName.find(".msh") != string::npos) {
    mesh.readGmsh(ifs);
    std::cerr << "GMSH file \'" << MeshFileName << "\'" << std::endl;

  } else if (MeshFileName.find(".txt") != string::npos) {
    mesh.readOceanMesh( ifs );
    std::cerr << "OceanMesh2D file \'" << MeshFileName << "\'" << std::endl;

  } else if ( mesh.nx!= 0 && mesh.ny!=0 ) {
      mesh.InitRectangle();

  } else {
        std::cerr << "Could not initialize the mesh, aborting\n";
        std::abort();
      }

  // mesh.RCMRenumbering();
  mesh.ComputeConnectivity();

  // std::ofstream stream3( "bandwith.ppm" );
  // mesh.WriteMeshBandwith( stream3 );
  // stream3.close();

  //mesh.RCMRenumbering();
  //mesh.ComputeConnectivity();
  mesh.LegacyInterface();
  mesh.ComputeGeometricQuantities();
  // mesh.ComputeGradientInterpolator();

  // std::ofstream stream2( "bandwith_rcm.ppm" );
  // mesh.WriteMeshBandwith( stream2 );
  // stream2.close();

  std::vector<RealType> X, Y;
  for ( int i =0; i < mesh.NVolumes; ++i ) {
    X.push_back( mesh.CellCenters.x(i) );
    Y.push_back( mesh.CellCenters.y(i) );
  }

  mesh.mathParser.updateReservedVariables( X, Y );

	CellValues.H.resize( mesh.NVolumes );
	CellValues.U.resize( mesh.NVolumes );
	CellValues.V.resize( mesh.NVolumes );
	CellValues.Zb.resize( mesh.NVolumes );

	for ( int i = 0; i<mesh.NVolumes; ++i ) {
		CellValues.H(i) = 0.0f;
		CellValues.U(i) = 0.0f;
		CellValues.V(i) = 0.0f;
		CellValues.Zb(i) = 0.0f;
	}

	if (InitFormulas.eta.begin() != InitFormulas.eta.end() ||
			InitFormulas.U.begin() != InitFormulas.U.end() ||
			InitFormulas.V.begin() != InitFormulas.V.end() ||
      InitFormulas.bathyrelative.begin() != InitFormulas.bathyrelative.end() ||
			InitFormulas.bathymetry.begin() != InitFormulas.bathymetry.end()) {
		printf("Unsupported method of specifiying initial values: please use Init events\n");
		exit(-1);
	}
}

#endif // SIMULATION_HPP


//=================================================================================================
// Copyright (c) 2012-2016, Institute of Flight Systems and Automatic Control,
// Technische Universität Darmstadt.
// 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 hector_quadrotor 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 HOLDER 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.
//=================================================================================================

#include <controller_interface/controller.h>

#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/WrenchStamped.h>

#include <limits>

#include <ros/ros.h>
#include <ros/subscriber.h>

#include <boost/thread/mutex.hpp>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>

#include <tf/transform_listener.h> // for tf::getPrefixParam()
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_eigen/tf2_eigen.h>

#include <visualization_msgs/Marker.h>
#include <cstdlib>
#include <cmath>
#include <pluginlib/class_list_macros.h>

#include <hector_quadrotor_interface/helpers.h>
#include <hector_quadrotor_interface/quadrotor_interface.h>
#include <hector_quadrotor_interface/limiters.h>

#include <std_msgs/Bool.h>
#include "Eigen/Dense"

namespace hector_quadrotor_controllers
{

using namespace hector_quadrotor_interface;
using namespace Eigen;

class PositionController : public controller_interface::Controller<hector_quadrotor_interface::QuadrotorInterface>
{
public:
  PositionController()
    : pose_command_valid_(false)
  {
  }

  virtual ~PositionController()
  {
    pose_subscriber_.shutdown();
  }
  virtual bool init(hector_quadrotor_interface::QuadrotorInterface *interface, ros::NodeHandle &root_nh,
            ros::NodeHandle &controller_nh)
  {
    // get interface handles

    double Kp_h, Kp_v, Kv_h, Kv_v, KR_h, KR_v, Kw_h, Kw_v;

    root_nh.param<double>("Kp_h", Kp_h, 0.0);
    root_nh.param<double>("Kp_v", Kp_v, 0.0);
    root_nh.param<double>("Kv_h", Kv_h, 0.0);
    root_nh.param<double>("Kv_v", Kv_v, 0.0);
    root_nh.param<double>("KR_h", KR_h, 0.0);
    root_nh.param<double>("KR_v", KR_v, 0.0);
    root_nh.param<double>("Kw_h", Kw_h, 0.0);
    root_nh.param<double>("Kw_v", Kw_v, 0.0);

    Kp.diagonal() << Kp_h, Kp_h, Kp_v;
    Kv.diagonal() << Kv_h, Kv_h, Kv_v;
    KR.diagonal() << KR_h, KR_h, KR_v;
    Kw.diagonal() << Kw_h, Kw_h, Kw_v;

    pose_ = interface->getPose();
    twist_ = interface->getTwist();
    accel_ = interface->getAccel();
    motor_status_ = interface->getMotorStatus();

    root_nh.param<std::string>("base_link_frame", base_link_frame_, "base_link");
    root_nh.param<std::string>("world_frame", world_frame_, "/world");
    root_nh.param<std::string>("base_stabilized_frame", base_stabilized_frame_, "base_stabilized");

    getMassAndInertia(root_nh, mass_, inertia_);

    // resolve frames
    tf_prefix_ = tf::getPrefixParam(root_nh);
    world_frame_ = tf::resolve(tf_prefix_, world_frame_);
    base_link_frame_ = tf::resolve(tf_prefix_, base_link_frame_);
    base_stabilized_frame_ = tf::resolve(tf_prefix_, base_stabilized_frame_);

    // Setup pose visualization marker output
    initMarker(root_nh.getNamespace());
    marker_publisher_ = root_nh.advertise<visualization_msgs::Marker>("command/pose_marker", 1);

    // Initialize inputs/outputs
    pose_input_ = interface->addInput<PoseCommandHandle>("pose");
    wrench_output_ = interface->addOutput<WrenchCommandHandle>("wrench");

    // subscribe to commanded pose and velocity
    pose_subscriber_ = root_nh.subscribe<geometry_msgs::PoseStamped>("command/pose", 1, boost::bind(
        &PositionController::poseCommandCallback, this, _1));

    return true;
  }

  void reset()
  {
    // Set commanded pose to robot's current pose
    updatePoseCommand(pose_->pose());
    wrench_control_ = geometry_msgs::WrenchStamped();
    pose_command_valid_ = false;
  }

  virtual void starting(const ros::Time &time)
  {
    reset();
    wrench_output_->start();
  }

  virtual void stopping(const ros::Time &time)
  {
    wrench_output_->stop();
    pose_command_valid_ = false;
  }

  void poseCommandCallback(const geometry_msgs::PoseStampedConstPtr &command)
  {
    boost::mutex::scoped_lock lock(command_mutex_);

    ros::Time start_time = command->header.stamp;
    if (start_time.isZero()) start_time = ros::Time::now();
    if (!isRunning()) this->startRequest(start_time);

    updatePoseCommand(*command);
  }

  virtual void update(const ros::Time &time, const ros::Duration &period)
  {
    boost::mutex::scoped_lock lock(command_mutex_);

    // Get pose command command input
    if (pose_input_->connected() && pose_input_->enabled())
    {
      updatePoseCommand(pose_input_->getCommand());
    }

   // Check if motors are running
    if (motor_status_->motorStatus().running == false) {
      if (pose_command_valid_) {
        ROS_INFO_NAMED("position_controller", "Disabled position control while motors are not running.");
      }
      pose_command_valid_ = false;
    }

    // Abort if no pose command is available
    if (!pose_command_valid_) {
      reset();
      wrench_output_->stop();
      return;
    } else {
      wrench_output_->start();
    }

    Pose pose = pose_->pose();

    double yaw_command;
    {
      tf2::Quaternion q;
      double temp;
      tf2::fromMsg(pose_command_.orientation, q);
      tf2::Matrix3x3(q).getRPY(temp, temp, yaw_command);
    }

    double sin_yaw, cos_yaw;
    sincos(yaw_command, &sin_yaw, &cos_yaw);

    Twist twist = twist_->twist(), twist_body;
    twist_body.linear = pose_->toBody(twist.linear);
    twist_body.angular = pose_->toBody(twist.angular);

    const double gravity = 9.8065;
    // period.toSec()
    Vector3d ep, ev, eR, ew, zw, xB, yB, zB,  zB_des, yB_des, xB_des, xC_des, Fdes, u;
    Matrix3d Rdes,  R, Re;
    Affine3d  T;

   tf2::fromMsg(pose, T);
    R = T.rotation();

    xB = R.col(0);
    yB = R.col(1);
    zB = R.col(2);
    zw << 0, 0, 1;

    ep << pose_command_.position.x - pose.position.x, pose_command_.position.y - pose.position.y, pose_command_.position.z - pose.position.z;
    ev <<  -twist.linear.x, -twist.linear.y, -twist.linear.z;
    Fdes =mass_ * gravity*zw + Kp*ep +  Kv*ev;

    xC_des << cos_yaw, sin_yaw , 0;
    zB_des = Fdes / Fdes.norm();
    yB_des = zB_des.cross(xC_des);
    yB_des = yB_des / yB_des.norm();
    if (yB.dot(yB_des) <0) yB_des = -yB_des;
    xB_des = yB_des.cross(zB_des);

    Rdes.col(0) = xB_des;
    Rdes.col(1) = yB_des;
    Rdes.col(2) = zB_des;

    Re = 0.5*(Rdes.transpose()* R - R.transpose()*Rdes);

    eR << -Re(2,1), -Re(0,2), -Re(1,0);
    ew << -twist_body.angular.x, -twist_body.angular.y, -twist_body.angular.z;
    u = KR*eR + Kw*ew;

    // set wrench output
    wrench_control_.wrench.torque.x =  u(0) ;
    wrench_control_.wrench.torque.y =  u(1);
    wrench_control_.wrench.torque.z =  u(2);

    wrench_control_.wrench.force.x  = 0.0;
    wrench_control_.wrench.force.y  = 0.0;
    wrench_control_.wrench.force.z =   Fdes.dot(zB);

    if (wrench_control_.wrench.force.z > 30.0) wrench_control_.wrench.force.z = 30.0;
    if (wrench_control_.wrench.force.z <   0.0) wrench_control_.wrench.force.z =   0.0;

    if (wrench_control_.wrench.torque.x > 10.0) wrench_control_.wrench.torque.x = 10.0;
    if (wrench_control_.wrench.torque.x < -10.0) wrench_control_.wrench.torque.x = -10.0;

    if (wrench_control_.wrench.torque.y > 10.0) wrench_control_.wrench.torque.y = 10.0;
    if (wrench_control_.wrench.torque.y < -10.0) wrench_control_.wrench.torque.y = -10.0;

    if (wrench_control_.wrench.torque.z > 1.0) wrench_control_.wrench.torque.z = 1.0;
    if (wrench_control_.wrench.torque.z < -1.0) wrench_control_.wrench.torque.z = -1.0;

    wrench_control_.header.stamp = time;
    wrench_control_.header.frame_id = base_link_frame_;
    wrench_output_->setCommand(wrench_control_.wrench);
  }

private:
  void updatePoseCommand(const geometry_msgs::PoseStamped &new_pose)
  {
    // TODO TF to world frame
    if (new_pose.header.frame_id != world_frame_) {
      ROS_WARN_STREAM_THROTTLE_NAMED(1.0, "position_controller", "Pose commands must be given in the " << world_frame_ << " frame, ignoring command");
    }
    else
    {
      updatePoseCommand(new_pose.pose);
    }
  }

  void updatePoseCommand(const geometry_msgs::Pose &new_pose)
  {
    {
      pose_command_.position = new_pose.position;
      // Strip non-yaw components from orientation
      tf2::Quaternion q;
      double roll, pitch, yaw;
      tf2::fromMsg(new_pose.orientation, q);
      tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
      q.setRPY(0, 0, yaw);
      pose_command_.orientation = tf2::toMsg(q);
      pose_command_valid_ = true;
    }
    pose_marker_.pose = pose_command_;
    marker_publisher_.publish(pose_marker_);
  }

  void initMarker(std::string name)
  {
    pose_marker_.header.frame_id = world_frame_;
    pose_marker_.ns = name;
    pose_marker_.id = 0;
    pose_marker_.type = visualization_msgs::Marker::ARROW;
    pose_marker_.scale.x = 0.15;
    pose_marker_.scale.y = pose_marker_.scale.z = 0.03;
    pose_marker_.color.r = 0.5;
    pose_marker_.color.g = 0.5;
    pose_marker_.color.r = 0.5;
    pose_marker_.color.a = 1.0;
  }

  PoseHandlePtr pose_;
  TwistHandlePtr twist_;
  AccelerationHandlePtr accel_;
  MotorStatusHandlePtr motor_status_;

  PoseCommandHandlePtr pose_input_;
  WrenchCommandHandlePtr wrench_output_;

  ros::Subscriber pose_subscriber_;
  ros::Publisher marker_publisher_;

  visualization_msgs::Marker pose_marker_;

  geometry_msgs::Pose pose_command_;
  geometry_msgs::WrenchStamped wrench_control_;
  bool pose_command_valid_ ;

  std::string base_link_frame_, base_stabilized_frame_, world_frame_;
  std::string tf_prefix_;

  double mass_;
  double inertia_[3];

  DiagonalMatrix<double, 3>  Kp, Kv, KR, Kw;
  boost::mutex command_mutex_;
};

} // namespace hector_quadrotor_controllers

PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controllers::PositionController, controller_interface::ControllerBase)