#ifndef STAN_MODEL_MODEL_BASE_HPP
#define STAN_MODEL_MODEL_BASE_HPP

#include <stan/io/var_context.hpp>
#include <stan/math/rev/core.hpp>
#include <stan/model/prob_grad.hpp>
#include <boost/random/additive_combine.hpp>
#include <ostream>
#include <string>
#include <utility>
#include <vector>

namespace stan {
namespace model {

/**
 * The base class for models defining all virtual methods required for
 * services.  Any class extending this class and defining all of its
 * virtual methods can be used with any of the Stan services for
 * sampling, optimization, or variational inference.
 *
 *
Implementation Details: The reason there are so many
 * overloads of the `log_prob` and `write_array` methods is that
 * template methods cannot be declared virtual.  This class extends
 * `stan::model::prob_grad` in order to define sizing for the number
 * of unconstrained parameters;  thus it is not a pure virtual base
 * class.
 *
 *
The approach to defining models used by the Stan language code
 * generator is use the curiously recursive template base class defined
 * in the extension `stan::model::model_base_crtp`.
 */
class model_base : public prob_grad {
 public:
  /**
   * Construct a model with the specified number of real valued
   * unconstrained parameters.
   *
   * @param[in] num_params_r number of real-valued, unconstrained
   * parameters
   */
  explicit model_base(size_t num_params_r) : prob_grad(num_params_r) {}

  /**
   * Destructor.  This class has a no-op destructor.
   */
  virtual ~model_base() {}

  /**
   * Return the name of the model.
   *
   * @return model name
   */
  virtual std::string model_name() const = 0;

  /**
   * Returns the compile information of the model:
   * stanc version and stanc flags used to compile the model.
   *
   * @return model name
   */
  virtual std::vector<std::string> model_compile_info() const = 0;

  /**
   * Set the specified argument to sequence of parameters, transformed
   * parameters, and generated quantities in the order in which they
   * were declared.  The input sequence is cleared and resized.
   *
   * @param[in,out] names sequence of names parameters, transformed
   * parameters, and generated quantities
   */
  virtual void get_param_names(std::vector<std::string>& names) const = 0;

  /**
   * Set the dimensionalities of constrained parameters, transformed
   * parameters, and generated quantities.  The input sequence is
   * cleared and resized.  The dimensions of each parameter
   * dimensionality is represented by a sequence of sizes.  Scalar
   * real and integer parameters are represented as an empty sequence
   * of dimensions.
   *
   *
Indexes are output in the order they are used in indexing. For
   * example, a 2 x 3 x 4 array will have dimensionality
   * `std::vector<size_t>{ 2, 3, 4 }`, whereas a 2-dimensional array
   * of 3-vectors will have dimensionality `std::vector<size_t>{ 2, 3
   * }`, and a 2-dimensional array of 3 x 4 matrices will have
   * dimensionality `std::vector<size_t>{2, 3, 4}`.
   *
   * @param[in,out] dimss sequence of dimensions specifications to set
   */
  virtual void get_dims(std::vector<std::vector<size_t> >& dimss) const = 0;

  /**
   *  Set the specified sequence to the indexed, scalar, constrained
   *  parameter names.  Each variable is output with a
   *  period-separated list of indexes as a suffix, indexing from 1.
   *
   *
A real parameter `alpha` will produce output `alpha` with no
   * indexes.
   *
   *
A 3-dimensional vector (row vector, simplex, etc.)
   * `theta` will produce output `theta.1`, `theta.2`, `theta.3`.  The
   * dimensions are the constrained dimensions.
   *
   *
Matrices are output column major to match their internal
   * representation in the Stan math library, so that a 2 x 3 matrix
   * `X` will produce output `X.1.1, X.2.1, X.1.2, X.2.2, X.1.3,
   * X.2.3`.
   *
   *
Arrays are handled in natural C++ order, 2 x 3 x 4 array `a`
   * will produce output `a.1.1.1`, `a.1.1.2`, `a.1.1.3`, `a.1.1.4`,
   * `a.1.2.1`, `a.1.2.2`, `a.1.2.3`, `a.1.2.4`, `a.1.3.1`, `a.1.3.2`,
   * `a.1.3.3`, `a.1.3.4`, `a.2.1.1`, `a.2.1.2`, `a.2.1.3`, `a.2.1.4`,
   * `a.2.2.1`, `a.2.2.2`, `a.2.2.3`, `a.2.2.4`, `a.2.3.1`, `a.2.3.2`,
   * `a.2.3.3`, `a.2.3.4`.
   *
   *
Arrays of vectors are handled as expected, so that a
   * 2-dimensional array of 3-vectors is output as `B.1.1`, `B.2.1`,
   * `B.1.2`, `B.2.2`, `B.1.3`, `B.2.3`.

   *
Arrays of matrices are generated in row-major order for the
   * array components and column-major order for the matrix component.
   * Thus a 2-dimensional array of 3 by 4 matrices `B` will be of
   * dimensionality 2 x 3 x 4 (indexes `B[1:2, 1:3, 1:4]`) and will be output
   * as `B.1.1.1`, `B.2.1.1`, `B.1.2.1`, `B.2.2.1`, `B.1.3.1`,
   * `B.2.3.1`, `B.1.1.2`, `B.2.1.2`, `B.1.2.2`, `B.2.2.2`, `B.1.3.2`,
   * `B.2.3.2`, `B.1.1.3`, `B.2.1.3`, `B.1.2.3`, `B.2.2.3`, `B.1.3.3`,
   * `B.2.3.3`, `B.1.1.4`, `B.2.1.4`, `B.1.2.4`, `B.2.2.4`, `B.1.3.4`,
   * `B.2.3.4`
   */
  virtual void constrained_param_names(std::vector<std::string>& param_names,
                                       bool include_tparams = true,
                                       bool include_gqs = true) const = 0;

  /**
   * Set the specified sequence of parameter names to the
   * unconstrained parameter names.  Each unconstrained parameter is
   * represented as a simple one-dimensional sequence of values.  The
   * actual transforms are documented in the reference manual.
   *
   *
The sizes will not be the declared sizes for types such as
   * simplexes, correlation, and covariance matrices.  A simplex of
   * size `N` has `N - 1` unconstrained parameters, an `N` by `N`
   * correlation matrix (or Cholesky factor thereof) has `N` choose 2
   * unconstrained parameters, and a covariance matrix (or Cholesky
   * factor thereof) has `N` choose 2 plus `N` unconstrained
   * parameters.
   *
   *
Full details of the transforms and their underlying
   * representations as sequences are detailed in the Stan reference
   * manual.  This also provides details of the order of each
   * parameter type.
   *
   * @param[in,out] param_names sequence of names to set
   * @param[in] include_tparams true if transformed parameters should
   * be included
   * @param[in] include_gqs true if generated quantities should be
   * included
   */
  virtual void unconstrained_param_names(std::vector<std::string>& param_names,
                                         bool include_tparams = true,
                                         bool include_gqs = true) const = 0;

  /**
   * Return the log density for the specified unconstrained
   * parameters, without Jacobian and with normalizing constants for
   * probability functions.
   *
   * @param[in] params_r unconstrained parameters
   * @param[in,out] msgs message stream
   * @return log density for specified parameters
   */
  virtual double log_prob(Eigen::VectorXd& params_r,
                          std::ostream* msgs) const = 0;

  /**
   * Return the log density for the specified unconstrained
   * parameters, without Jacobian and with normalizing constants for
   * probability functions.
   *
   * @param[in] params_r unconstrained parameters
   * @param[in,out] msgs message stream
   * @return log density for specified parameters
   */
  virtual math::var log_prob(Eigen::Matrix<math::var, -1, 1>& params_r,
                             std::ostream* msgs) const = 0;

  /**
   * Return the log density for the specified unconstrained
   * parameters, with Jacobian correction for constraints and with
   * normalizing constants for probability functions.
   *
   *
The Jacobian is of the inverse transform from unconstrained
   * parameters to constrained parameters; full details for Stan
   * language types can be found in the language reference manual.
   *
   * @param[in] params_r unconstrained parameters
   * @param[in,out] msgs message stream
   * @return log density for specified parameters
   */
  virtual double log_prob_jacobian(Eigen::VectorXd& params_r,
                                   std::ostream* msgs) const = 0;

  /**
   * Return the log density for the specified unconstrained
   * parameters, with Jacobian correction for constraints and with
   * normalizing constants for probability functions.
   *
   *
The Jacobian is of the inverse transform from unconstrained
   * parameters to constrained parameters; full details for Stan
   * language types can be found in the language reference manual.
   *
   * @param[in] params_r unconstrained parameters
   * @param[in,out] msgs message stream
   * @return log density for specified parameters
   */
  virtual math::var log_prob_jacobian(Eigen::Matrix<math::var, -1, 1>& params_r,
                                      std::ostream* msgs) const = 0;

  /**
   * Return the log density for the specified unconstrained
   * parameters, without Jacobian correction for constraints and
   * dropping normalizing constants.
   *
   *
This method is for completeness as `double`-based inputs are
   * always constant and will thus cause all probability functions to
   * be dropped from the result.  To get the value of this
   * calculation, use the overload for `math::var`.
   *
   * @param[in] params_r unconstrained parameters
   * @param[in,out] msgs message stream
   * @return log density for specified parameters
   */
  virtual double log_prob_propto(Eigen::VectorXd& params_r,
                                 std::ostream* msgs) const = 0;

  /**
   * Return the log density for the specified unconstrained
   * parameters, without Jacobian correction for constraints and
   * dropping normalizing constants.
   *
   * @param[in] params_r unconstrained parameters
   * @param[in,out] msgs message stream
   * @return log density for specified parameters
   */
  virtual math::var log_prob_propto(Eigen::Matrix<math::var, -1, 1>& params_r,
                                    std::ostream* msgs) const = 0;

  /**
   * Return the log density for the specified unconstrained
   * parameters, with Jacobian correction for constraints and dropping
   * normalizing constants.
   *
   *
The Jacobian is of the inverse transform from unconstrained
   * parameters to constrained parameters; full details for Stan
   * language types can be found in the language reference manual.
   *
   *
This method is for completeness as `double`-based inputs are
   * always constant and will thus cause all probability functions to
   * be dropped from the result.  To get the value of this
   * calculation, use the overload for `math::var`.
   *
   * @param[in] params_r unconstrained parameters
   * @param[in,out] msgs message stream
   * @return log density for specified parameters
   */
  virtual double log_prob_propto_jacobian(Eigen::VectorXd& params_r,
                                          std::ostream* msgs) const = 0;

  /**
   * Return the log density for the specified unconstrained
   * parameters, with Jacobian correction for constraints and dropping
   * normalizing constants.
   *
   *
The Jacobian is of the inverse transform from unconstrained
   * parameters to constrained parameters; full details for Stan
   * language types can be found in the language reference manual.
   *
   * @param[in] params_r unconstrained parameters
   * @param[in,out] msgs message stream
   * @return log density for specified parameters
   */
  virtual math::var log_prob_propto_jacobian(
      Eigen::Matrix<math::var, -1, 1>& params_r, std::ostream* msgs) const = 0;

  /**
   * Convenience template function returning the log density for the
   * specified unconstrained parameters, with Jacobian and normalizing
   * constant inclusion controlled by the template parameters.
   *
   *
This non-virtual template method delegates to the appropriate
   * overloaded virtual function.  This allows external interfaces to
   * call the convenient template methods rather than the individual
   * virtual functions.
   *
   * @tparam propto `true` if normalizing constants should be dropped
   * and result returned up to an additive constant
   * @tparam jacobian `true` if the log Jacobian adjustment is
   * included for the change of variables from unconstrained to
   * constrained parameters
   * @tparam T type of scalars in the vector of parameters
   * @param[in] params_r unconstrained parameters
   * @param[in,out] msgs stream to which messages are written
   * @return log density with normalizing constants and Jacobian
   * included as specified by the template parameters
   */
  template <bool propto, bool jacobian, typename T>
  inline T log_prob(Eigen::Matrix<T, -1, 1>& params_r,
                    std::ostream* msgs) const {
    if (propto && jacobian)
      return log_prob_propto_jacobian(params_r, msgs);
    else if (propto && !jacobian)
      return log_prob_propto(params_r, msgs);
    else if (!propto && jacobian)
      return log_prob_jacobian(params_r, msgs);
    else  // if (!propto && !jacobian)
      return log_prob(params_r, msgs);
  }

  /**
   * Read constrained parameter values from the specified context,
   * unconstrain them, then concatenate the unconstrained sequences
   * into the specified parameter sequence.  Output messages go to the
   * specified stream.
   *
   * @param[in] context definitions of variable values
   * @param[in,out] params_r unconstrained parameter values produced
   * @param[in,out] msgs stream to which messages are written
   */
  virtual void transform_inits(const io::var_context& context,
                               Eigen::VectorXd& params_r,
                               std::ostream* msgs) const = 0;

  /**
   * Convert the specified sequence of unconstrained parameters to a
   * sequence of constrained parameters, optionally including
   * transformed parameters and including generated quantities.  The
   * generated quantities may use the random number generator.  Any
   * messages are written to the specified stream.  The output
   * parameter sequence will be resized if necessary to match the
   * number of constrained scalar parameters.
   *
   * @param base_rng RNG to use for generated quantities
   * @param[in] params_r unconstrained parameters input
   * @param[in,out] params_constrained_r constrained parameters produced
   * @param[in] include_tparams true if transformed parameters are
   * included in output
   * @param[in] include_gqs true if generated quantities are included
   * in output
   * @param[in,out] msgs msgs stream to which messages are written
   */
  virtual void write_array(boost::ecuyer1988& base_rng,
                           Eigen::VectorXd& params_r,
                           Eigen::VectorXd& params_constrained_r,
                           bool include_tparams = true, bool include_gqs = true,
                           std::ostream* msgs = 0) const = 0;

  // TODO(carpenter): cut redundant std::vector versions from here ===

  /**
   * Return the log density for the specified unconstrained
   * parameters, without Jacobian and with normalizing constants for
   * probability functions.
   *
   * \deprecated Use Eigen vector versions
   *
   * @param[in] params_r unconstrained parameters
   * @param[in] params_i integer parameters (ignored)
   * @param[in,out] msgs message stream
   * @return log density for specified parameters
   */
  virtual double log_prob(std::vector<double>& params_r,
                          std::vector<int>& params_i,
                          std::ostream* msgs) const = 0;

  /**
   * Return the log density for the specified unconstrained
   * parameters, without Jacobian and with normalizing constants for
   * probability functions.
   *
   * \deprecated Use Eigen vector versions
   *
   * @param[in] params_r unconstrained
   * @param[in] params_i integer parameters (ignored)
   * @param[in,out] msgs message stream
   * @return log density for specified parameters
   */
  virtual math::var log_prob(std::vector<math::var>& params_r,
                             std::vector<int>& params_i,
                             std::ostream* msgs) const = 0;

  /**
   * Return the log density for the specified unconstrained
   * parameters, with Jacobian correction for constraints and with
   * normalizing constants for probability functions.
   *
   *
The Jacobian is of the inverse transform from unconstrained
   * parameters to constrained parameters; full details for Stan
   * language types can be found in the language reference manual.
   *
   * \deprecated Use Eigen vector versions
   *
   * @param[in] params_r unconstrained parameters
   * @param[in] params_i integer parameters (ignored)
   * @param[in,out] msgs message stream
   * @return log density for specified parameters
   */
  virtual double log_prob_jacobian(std::vector<double>& params_r,
                                   std::vector<int>& params_i,
                                   std::ostream* msgs) const = 0;

  /**
   * Return the log density for the specified unconstrained
   * parameters, with Jacobian correction for constraints and with
   * normalizing constants for probability functions.
   *
   *
The Jacobian is of the inverse transform from unconstrained
   * parameters to constrained parameters; full details for Stan
   * language types can be found in the language reference manual.
   *
   * \deprecated Use Eigen vector versions
   *
   * @param[in] params_r unconstrained parameters
   * @param[in] params_i integer parameters (ignored)
   * @param[in,out] msgs message stream
   * @return log density for specified parameters
   */
  virtual math::var log_prob_jacobian(std::vector<math::var>& params_r,
                                      std::vector<int>& params_i,
                                      std::ostream* msgs) const = 0;

  /**
   * Return the log density for the specified unconstrained
   * parameters, without Jacobian correction for constraints and
   * dropping normalizing constants.
   *
   *
This method is for completeness as `double`-based inputs are
   * always constant and will thus cause all probability functions to
   * be dropped from the result.  To get the value of this
   * calculation, use the overload for `math::var`.
   *
   * \deprecated Use Eigen vector versions
   *
   * @param[in] params_r unconstrained parameters
   * @param[in] params_i integer parameters (ignored)
   * @param[in,out] msgs message stream
   * @return log density for specified parameters
   */
  virtual double log_prob_propto(std::vector<double>& params_r,
                                 std::vector<int>& params_i,
                                 std::ostream* msgs) const = 0;

  /**
   * Return the log density for the specified unconstrained
   * parameters, without Jacobian correction for constraints and
   * dropping normalizing constants.
   *
   * \deprecated Use Eigen vector versions
   *
   * @param[in] params_r unconstrained parameters
   * @param[in] params_i integer parameters (ignored)
   * @param[in,out] msgs message stream
   * @return log density for specified parameters
   */
  virtual math::var log_prob_propto(std::vector<math::var>& params_r,
                                    std::vector<int>& params_i,
                                    std::ostream* msgs) const = 0;

  /**
   * Return the log density for the specified unconstrained
   * parameters, with Jacobian correction for constraints and dropping
   * normalizing constants.
   *
   *
The Jacobian is of the inverse transform from unconstrained
   * parameters to constrained parameters; full details for Stan
   * language types can be found in the language reference manual.
   *
   *
This method is for completeness as `double`-based inputs are
   * always constant and will thus cause all probability functions to
   * be dropped from the result.  To get the value of this
   * calculation, use the overload for `math::var`.
   *
   * \deprecated Use Eigen vector versions
   *
   * @param[in] params_r unconstrained parameters
   * @param[in] params_i integer parameters (ignored)
   * @param[in,out] msgs message stream
   * @return log density for specified parameters
   */
  virtual double log_prob_propto_jacobian(std::vector<double>& params_r,
                                          std::vector<int>& params_i,
                                          std::ostream* msgs) const = 0;

  /**
   * Return the log density for the specified unconstrained
   * parameters, with Jacobian correction for constraints and dropping
   * normalizing constants.
   *
   *
The Jacobian is of the inverse transform from unconstrained
   * parameters to constrained parameters; full details for Stan
   * language types can be found in the language reference manual.
   *
   * \deprecated Use Eigen vector versions
   *
   * @param[in] params_r unconstrained parameters
   * @param[in] params_i integer parameters (ignored)
   * @param[in,out] msgs message stream
   * @return log density for specified parameters
   */
  virtual math::var log_prob_propto_jacobian(std::vector<math::var>& params_r,
                                             std::vector<int>& params_i,
                                             std::ostream* msgs) const = 0;

  /**
   * Convenience template function returning the log density for the
   * specified unconstrained parameters, with Jacobian and normalizing
   * constant inclusion controlled by the template parameters.
   *
   *
This non-virtual template method delegates to the appropriate
   * overloaded virtual function.  This allows external interfaces to
   * call the convenient template methods rather than the individual
   * virtual functions.
   *
   * \deprecated Use Eigen vector versions
   *
   * @tparam propto `true` if normalizing constants should be dropped
   * and result returned up to an additive constant
   * @tparam jacobian `true` if the log Jacobian adjustment is
   * included for the change of variables from unconstrained to
   * constrained parameters.
   * @tparam T type of scalars in the vector of parameters
   * @param[in] params_r unconstrained parameters
   * @param[in] params_i integer parameters (ignored)
   * @param[in,out] msgs stream to which messages are written
   * @return log density with normalizing constants and Jacobian
   * included as specified by the template parameters
   */
  template <bool propto, bool jacobian, typename T>
  inline T log_prob(std::vector<T>& params_r, std::vector<int>& params_i,
                    std::ostream* msgs) const {
    if (propto && jacobian)
      return log_prob_propto_jacobian(params_r, params_i, msgs);
    else if (propto && !jacobian)
      return log_prob_propto(params_r, params_i, msgs);
    else if (!propto && jacobian)
      return log_prob_jacobian(params_r, params_i, msgs);
    else  // if (!propto && !jacobian)
      return log_prob(params_r, params_i, msgs);
  }

  /**
   * Read constrained parameter values from the specified context,
   * unconstrain them, then concatenate the unconstrained sequences
   * into the specified parameter sequence.  Output messages go to the
   * specified stream.
   *
   * \deprecated Use Eigen vector versions
   *
   * @param[in] context definitions of variable values
   * @param[in] params_i integer parameters (ignored)
   * @param[in,out] params_r unconstrained parameter values produced
   * @param[in,out] msgs stream to which messages are written
   */
  virtual void transform_inits(const io::var_context& context,
                               std::vector<int>& params_i,
                               std::vector<double>& params_r,
                               std::ostream* msgs) const = 0;

  /**
   * Convert the specified sequence of unconstrained parameters to a
   * sequence of constrained parameters, optionally including
   * transformed parameters and including generated quantities.  The
   * generated quantities may use the random number generator.  Any
   * messages are written to the specified stream.  The output
   * parameter sequence will be resized if necessary to match the
   * number of constrained scalar parameters.
   *
   * @param base_rng RNG to use for generated quantities
   * @param[in] params_r unconstrained parameters input
   * @param[in] params_i integer parameters (ignored)
   * @param[in,out] params_r_constrained constrained parameters produced
   * @param[in] include_tparams true if transformed parameters are
   * included in output
   * @param[in] include_gqs true if generated quantities are included
   * in output
   * @param[in,out] msgs msgs stream to which messages are written
   */
  virtual void write_array(boost::ecuyer1988& base_rng,
                           std::vector<double>& params_r,
                           std::vector<int>& params_i,
                           std::vector<double>& params_r_constrained,
                           bool include_tparams = true, bool include_gqs = true,
                           std::ostream* msgs = 0) const = 0;

  /**
   * Return as json string the unconstrained size types
   */
  virtual std::string get_unconstrained_sizedtypes() const = 0;
};

}  // namespace model
}  // namespace stan
#endif


#include <utility>

#include <utility>

#include <iostream>
#include <boost/program_options.hpp>
#include <H5Cpp.h>
#include "../include/def.h"
#include "../include/utils.h"
#include "../include/state_type.h"
#include "../include/observer_type.h"
#include "../include/solver_type.h"
#include "../include/herring_func.hpp"

class HerringSolverType: public RBCSystem::SolverType {
public:
    explicit HerringSolverType(const Real &Prandtl_number = 1.0, const Real &Rayleigh_number = 1000.0,
                               const Real &wave_number = 1.0, const Real &start_time = 0.0, const Real &end_time = 1.0,
                               const String &in_h5file_name = static_cast<String>("in_data.h5"),
                               const String &in_h5dset_name = static_cast<String>("/group/state"),
                               const String &out_h5file_name = static_cast<String>("out_data.h5"),
                               const String &out_h5group_name = static_cast<String>("/group/state"));

    HerringSolverType(const HerringSolverType &rhs);

    HerringSolverType &operator=(const HerringSolverType &rhs);

    ~HerringSolverType(void);

    const String &in_h5file_name(void) const;

    void in_h5file_name(const String &in_h5file_name);

    const String &in_h5dset_name(void) const;

    void in_h5dset_name(const String &in_h5dset_name);

    const String &out_h5file_name(void) const;

    void out_h5file_name(const String &out_h5file_name);

    const String &out_h5group_name(void) const;

    void out_h5group_name(const String &out_h5group_name);

    void operator()(const RBCSystem::StateType &f, RBCSystem::StateType &dfdt, const Real &t) const override;

    void parseArgv(int argc, char *argv[]);

private:
    String inf_name, ind_name, outf_name, outg_name;
};

int main(int argc, char* argv[])
{
    HerringSolverType solver;
    H5::H5File inf, outf;

    solver.parseArgv(argc, argv);
    //inf = H5::H5File(mysolver.in_h5file_name, H5F_ACC_RDONLY);
    //initial_state.loadFile(inf, mysolver.in_h5obj_name);
    //inf.close();
    //solver.initial_state(initial_state);
    solver.evaluate();
    outf = H5::H5File(solver.out_h5file_name(), H5F_ACC_TRUNC);
    solver.saveFile(outf, solver.out_h5group_name());
    outf.close();

    return 0;
}

HerringSolverType::HerringSolverType(const Real &Prandtl_number, const Real &Rayleigh_number, const Real &wave_number,
        const Real &start_time, const Real &end_time,
        const String &in_h5file_name, const String &in_h5dset_name,
        const String &out_h5file_name, const String &out_h5group_name):
        RBCSystem::SolverType(Prandtl_number, Rayleigh_number, wave_number, RBCSystem::StateType::TrivialState(), start_time, end_time),
        inf_name(in_h5file_name), ind_name(in_h5dset_name),
        outf_name(out_h5file_name), outg_name(out_h5group_name) { return; }

HerringSolverType::HerringSolverType(const HerringSolverType &rhs):
        RBCSystem::SolverType(rhs),
        inf_name(rhs.inf_name), ind_name(rhs.ind_name),
        outf_name(rhs.inf_name), outg_name(rhs.outg_name) { return; }

HerringSolverType &HerringSolverType::operator=(const HerringSolverType &rhs)
{
    RBCSystem::SolverType::operator=(rhs);
    inf_name = rhs.inf_name;
    ind_name = rhs.ind_name;
    outf_name = rhs.outf_name;
    outg_name = rhs.outg_name;

    return *this;
}

HerringSolverType::~HerringSolverType(void) { return; }

inline const String &HerringSolverType::in_h5file_name(void) const { return inf_name; }

inline void HerringSolverType::in_h5file_name(const String &in_h5file_name) { inf_name = in_h5file_name; return; }

inline const String &HerringSolverType::in_h5dset_name(void) const { return ind_name; }

inline void HerringSolverType::in_h5dset_name(const String &in_h5dset_name) { ind_name = in_h5dset_name; return; }

inline const String &HerringSolverType::out_h5file_name(void) const { return outf_name; }

inline void HerringSolverType::out_h5file_name(const String &out_h5file_name) { outf_name = out_h5file_name; return; }

inline const String &HerringSolverType::out_h5group_name(void) const { return outg_name; }

inline void HerringSolverType::out_h5group_name(const String &out_h5group_name) { outg_name = out_h5group_name; return; }

void HerringSolverType::operator()(const RBCSystem::StateType &f, RBCSystem::StateType &dfdt, const Real &t) const
{
    func(f, dfdt, t, Prandtl_number(), Rayleigh_number(), wave_number());

    return;
}

void HerringSolverType::parseArgv(int argc, char *argv[])
{
    using namespace boost::program_options;

    options_description desc("**This program is used to simulate HerringSolver's model.**\nUsages:");
    desc.add_options()
            ("help,h", "display usages;")
            ("Prandtl_number", value<Real>()->default_value(1.0), "Prandl_number;")
            ("Rayleigh_number", value<Real>()->default_value(1000.0), "Rayleigh_number;")
            ("wave_number", value<Real>()->default_value(0.5), "wave_number;")
            ("start_time", value<Real>()->default_value(0.0), "start_time")
            ("end_time", value<Real>()->default_value(1.0), "end_time")
            ("input_h5file", value<String>()->default_value(static_cast<String>("in_data.h5")), "input h5file's name;")
            ("input_h5obj", value<String>()->default_value(static_cast<String>("/group/state")), "input h5obj's name;")
            ("output_h5file", value<String>()->default_value(static_cast<String>("out_data.h5")), "output h5file's name;")
            ("output_h5obj", value<String>()->default_value(static_cast<String>("/group")), "output h5obj's name.");
    variables_map vm;
    store(parse_command_line(argc, argv, desc), vm);
    notify(vm);
    if(vm.count("help") ){
        std::cout << desc << std::endl;
        std::exit(1);
    }
    Prandtl_number(vm["Prandtl_number"].as<Real>());
    Rayleigh_number(vm["Rayleigh_number"].as<Real>());
    wave_number(vm["wave_number"].as<Real>());
    start_time(vm["start_time"].as<Real>());
    end_time(vm["end_time"].as<Real>());
    in_h5file_name(vm["input_h5file"].as<String>());
    in_h5dset_name(vm["input_h5obj"].as<String>());
    out_h5file_name(vm["output_h5file"].as<String>());
    out_h5group_name(vm["output_h5obj"].as<String>());

    return;
}


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  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 Willow Garage 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: Melonee Wise
  Contributors: Dave Coleman, Jonathan Bohren, Bob Holmberg, Wim Meeussen
  Desc: Implements a standard proportional-integral-derivative controller
*/

#include <control_toolbox/pid.h>
#include <tinyxml.h>

#include <boost/algorithm/clamp.hpp>
#include <boost/algorithm/minmax.hpp>

namespace control_toolbox {

static const std::string DEFAULT_NAMESPACE = "pid"; // \todo better default prefix?

Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup)
  : dynamic_reconfig_initialized_(false)
{
  setGains(p,i,d,i_max,i_min,antiwindup);

  reset();
}

Pid::Pid(const Pid &source)
   : dynamic_reconfig_initialized_(false)
{
  // Copy the realtime buffer to then new PID class
  gains_buffer_ = source.gains_buffer_;

  // Reset the state of this PID controller
  reset();
}

Pid::~Pid()
{
}

void Pid::initPid(double p, double i, double d, double i_max, double i_min,
  const ros::NodeHandle& /*node*/)
{
  initPid(p, i, d, i_max, i_min);

  // Create node handle for dynamic reconfigure
  ros::NodeHandle nh(DEFAULT_NAMESPACE);
  initDynamicReconfig(nh);
}

void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup,
  const ros::NodeHandle& /*node*/)
{
  initPid(p, i, d, i_max, i_min, antiwindup);

  // Create node handle for dynamic reconfigure
  ros::NodeHandle nh(DEFAULT_NAMESPACE);
  initDynamicReconfig(nh);
}

void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup)
{
  setGains(p,i,d,i_max,i_min, antiwindup);

  reset();
}

bool Pid::initParam(const std::string& prefix, const bool quiet)
{
  ros::NodeHandle nh(prefix);
  return init(nh, quiet);
}

bool Pid::init(const ros::NodeHandle &node, const bool quiet)
{
  ros::NodeHandle nh(node);

  Gains gains;

  // Load PID gains from parameter server
  if (!nh.getParam("p", gains.p_gain_))
  {
    if (!quiet) {
      ROS_ERROR("No p gain specified for pid.  Namespace: %s", nh.getNamespace().c_str());
    }
    return false;
  }
  // Only the P gain is required, the I and D gains are optional and default to 0:
  nh.param("i", gains.i_gain_, 0.0);
  nh.param("d", gains.d_gain_, 0.0);

  // Load integral clamp from param server or default to 0
  double i_clamp;
  nh.param("i_clamp", i_clamp, 0.0);
  gains.i_max_ = std::abs(i_clamp);
  gains.i_min_ = -std::abs(i_clamp);
  if(nh.hasParam("i_clamp_min"))
  {
    nh.param("i_clamp_min", gains.i_min_, gains.i_min_); // use i_clamp_min parameter, otherwise keep -i_clamp
    gains.i_min_ = -std::abs(gains.i_min_); // make sure the value is <= 0
  }
  if(nh.hasParam("i_clamp_max"))
  {
    nh.param("i_clamp_max", gains.i_max_, gains.i_max_); // use i_clamp_max parameter, otherwise keep i_clamp
    gains.i_max_ = std::abs(gains.i_max_); // make sure the value is >= 0
  }
  nh.param("antiwindup", gains.antiwindup_, false);

  nh.param("publish_state", publish_state_, false);

  if(publish_state_)
  {
    state_publisher_.reset(new realtime_tools::RealtimePublisher<control_msgs::PidState>());
    state_publisher_->init(nh, "state", 1);
  }

  setGains(gains);

  reset();
  initDynamicReconfig(nh);

  return true;
}

bool Pid::initXml(TiXmlElement *config)
{
  // Create node handle for dynamic reconfigure
  ros::NodeHandle nh(DEFAULT_NAMESPACE);

  double i_clamp;
  i_clamp = config->Attribute("iClamp") ? atof(config->Attribute("iClamp")) : 0.0;

  setGains(
    config->Attribute("p") ? atof(config->Attribute("p")) : 0.0,
    config->Attribute("i") ? atof(config->Attribute("i")) : 0.0,
    config->Attribute("d") ? atof(config->Attribute("d")) : 0.0,
    std::abs(i_clamp),
    -std::abs(i_clamp),
    config->Attribute("antiwindup") ? atof(config->Attribute("antiwindup")) : false
  );

  reset();
  initDynamicReconfig(nh);

  return true;
}

void Pid::initDynamicReconfig(ros::NodeHandle &node)
{
  ROS_DEBUG_STREAM_NAMED("pid","Initializing dynamic reconfigure in namespace "
    << node.getNamespace());

  // Start dynamic reconfigure server
  param_reconfig_server_.reset(new DynamicReconfigServer(param_reconfig_mutex_, node));
  dynamic_reconfig_initialized_ = true;

  // Set Dynamic Reconfigure's gains to Pid's values
  updateDynamicReconfig();

  // Set callback
  param_reconfig_callback_ = boost::bind(&Pid::dynamicReconfigCallback, this, _1, _2);
  param_reconfig_server_->setCallback(param_reconfig_callback_);
}

void Pid::reset()
{
  p_error_last_ = 0.0;
  p_error_ = 0.0;
  i_error_ = 0.0;
  d_error_ = 0.0;
  cmd_ = 0.0;
}

void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
{
  bool antiwindup;
  getGains(p, i, d, i_max, i_min, antiwindup);
}

void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup)
{
  Gains gains = *gains_buffer_.readFromRT();

  p     = gains.p_gain_;
  i     = gains.i_gain_;
  d     = gains.d_gain_;
  i_max = gains.i_max_;
  i_min = gains.i_min_;
  antiwindup = gains.antiwindup_;
}

Pid::Gains Pid::getGains()
{
  return *gains_buffer_.readFromRT();
}

void Pid::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup)
{
  Gains gains(p,i,d,i_max,i_min, antiwindup);

  setGains(gains);
}

void Pid::setGains(const Gains &gains)
{
  gains_buffer_.writeFromNonRT(gains);

  // Update dynamic reconfigure with the new gains
  updateDynamicReconfig(gains);
}

void Pid::updateDynamicReconfig()
{
  // Make sure dynamic reconfigure is initialized
  if(!dynamic_reconfig_initialized_)
    return;

  // Get starting values
  control_toolbox::ParametersConfig config;

  // Get starting values
  getGains(config.p, config.i, config.d, config.i_clamp_max, config.i_clamp_min, config.antiwindup);

  updateDynamicReconfig(config);
}

void Pid::updateDynamicReconfig(Gains gains_config)
{
  // Make sure dynamic reconfigure is initialized
  if(!dynamic_reconfig_initialized_)
    return;

  control_toolbox::ParametersConfig config;

  // Convert to dynamic reconfigure format
  config.p = gains_config.p_gain_;
  config.i = gains_config.i_gain_;
  config.d = gains_config.d_gain_;
  config.i_clamp_max = gains_config.i_max_;
  config.i_clamp_min = gains_config.i_min_;
  config.antiwindup = gains_config.antiwindup_;

  updateDynamicReconfig(config);
}

void Pid::updateDynamicReconfig(control_toolbox::ParametersConfig config)
{
  // Make sure dynamic reconfigure is initialized
  if(!dynamic_reconfig_initialized_)
    return;

  // Set starting values, using a shared mutex with dynamic reconfig
  param_reconfig_mutex_.lock();
  param_reconfig_server_->updateConfig(config);
  param_reconfig_mutex_.unlock();
}

void Pid::dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t /*level*/)
{
  ROS_DEBUG_STREAM_NAMED("pid","Dynamics reconfigure callback recieved.");

  // Set the gains
  setGains(config.p, config.i, config.d, config.i_clamp_max, config.i_clamp_min, config.antiwindup);
}

double Pid::computeCommand(double error, ros::Duration dt)
{

  if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
    return 0.0;

  double error_dot = d_error_;

  // Calculate the derivative error
  if (dt.toSec() > 0.0)
  {
    error_dot = (error - p_error_last_) / dt.toSec();
    p_error_last_ = error;
  }

  return computeCommand(error, error_dot, dt);
}

double Pid::updatePid(double error, ros::Duration dt)
{
  return -computeCommand(error, dt);
}

double Pid::computeCommand(double error, double error_dot, ros::Duration dt)
{
  // Get the gain parameters from the realtime buffer
  Gains gains = *gains_buffer_.readFromRT();

  double p_term, d_term, i_term;
  p_error_ = error; // this is error = target - state
  d_error_ = error_dot;

  if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || std::isinf(error_dot))
    return 0.0;

  // Calculate proportional contribution to command
  p_term = gains.p_gain_ * p_error_;

  // Calculate the integral of the position error
  i_error_ += dt.toSec() * p_error_;

  if(gains.antiwindup_ && gains.i_gain_!=0)
  {
    // Prevent i_error_ from climbing higher than permitted by i_max_/i_min_
    boost::tuple<double, double> bounds = boost::minmax<double>(gains.i_min_ / gains.i_gain_, gains.i_max_ / gains.i_gain_);
    i_error_ = boost::algorithm::clamp(i_error_, bounds.get<0>(), bounds.get<1>());
  }

  // Calculate integral contribution to command
  i_term = gains.i_gain_ * i_error_;

  if(!gains.antiwindup_)
  {
    // Limit i_term so that the limit is meaningful in the output
    i_term = boost::algorithm::clamp(i_term, gains.i_min_, gains.i_max_);
  }

  // Calculate derivative contribution to command
  d_term = gains.d_gain_ * d_error_;

  // Compute the command
  cmd_ = p_term + i_term + d_term;

  // Publish controller state if configured
  if (publish_state_ && state_publisher_)
  {
    if (state_publisher_->trylock())
    {
      state_publisher_->msg_.header.stamp = ros::Time::now();
      state_publisher_->msg_.timestep = dt;
      state_publisher_->msg_.error = error;
      state_publisher_->msg_.error_dot = error_dot;
      state_publisher_->msg_.p_error = p_error_;
      state_publisher_->msg_.i_error = i_error_;
      state_publisher_->msg_.d_error = d_error_;
      state_publisher_->msg_.p_term = p_term;
      state_publisher_->msg_.i_term = i_term;
      state_publisher_->msg_.d_term = d_term;
      state_publisher_->msg_.i_max = gains.i_max_;
      state_publisher_->msg_.i_min = gains.i_min_;
      state_publisher_->msg_.output = cmd_;
      state_publisher_->unlockAndPublish();
    }
  }

  return cmd_;
}

double Pid::updatePid(double error, double error_dot, ros::Duration dt)
{
  return -computeCommand(error, error_dot, dt);
}

void Pid::setCurrentCmd(double cmd)
{
  cmd_ = cmd;
}

double Pid::getCurrentCmd()
{
  return cmd_;
}

void Pid::getCurrentPIDErrors(double *pe, double *ie, double *de)
{
  // Get the gain parameters from the realtime buffer
  Gains gains = *gains_buffer_.readFromRT();

  *pe = p_error_;
  *ie = i_error_;
  *de = d_error_;
}

void Pid::printValues()
{
  Gains gains = getGains();

  ROS_INFO_STREAM_NAMED("pid","Current Values of PID Class:\n"
    << "  P Gain: " << gains.p_gain_ << "\n"
    << "  I Gain: " << gains.i_gain_ << "\n"
    << "  D Gain: " << gains.d_gain_ << "\n"
    << "  I_Max:  " << gains.i_max_  << "\n"
    << "  I_Min:  " << gains.i_min_  << "\n"
    << "  Antiwindup:  " << gains.antiwindup_  << "\n"
    << "  P_Error_Last: " << p_error_last_  << "\n"
    << "  P_Error:      " << p_error_  << "\n"
    << "  I_Error:       " << i_error_  << "\n"
    << "  D_Error:      " << d_error_  << "\n"
    << "  Command:      " << cmd_
  );

}

} // namespace


#pragma once

#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include <boost/assert.hpp>

#include <fstream>
#include <iostream>

template <typename T>
Eigen::Block<const T>
ftcut(const Eigen::DenseBase<T> &m, int rows, int cols)
{
  int this_rows = m.rows();
  int this_cols = m.cols();

  int row_offset = this_rows / 2 - rows / 2;
  int col_offset = this_cols / 2 - cols / 2;
  assert(row_offset >= 0);
  assert(col_offset >= 0);
  return m.block(row_offset, col_offset, rows, cols);
}

template <typename T>
Eigen::Block<T>
ftcut(Eigen::DenseBase<T> &m, int rows, int cols)
{
  int this_rows = m.rows();
  int this_cols = m.cols();

  int row_offset = this_rows / 2 - rows / 2;
  int col_offset = this_cols / 2 - cols / 2;
  assert(row_offset >= 0);
  assert(col_offset >= 0);
  return m.block(row_offset, col_offset, rows, cols);
}

template <typename T>
Eigen::Block<const T>
ftcut(const Eigen::SparseMatrixBase<T> &m, int rows, int cols)
{
  int this_rows = m.rows();
  int this_cols = m.cols();

  int row_offset = this_rows / 2 - rows / 2;
  int col_offset = this_cols / 2 - cols / 2;
  assert(row_offset >= 0);
  assert(col_offset >= 0);
  return m.block(row_offset, col_offset, rows, cols);
}

template <typename T>
Eigen::Block<T>
ftcut(Eigen::DenseBase<T> &&m, int rows, int cols)
{
  (void)m;
  (void)rows;
  (void)cols;
  static_assert(sizeof(T) == -1, "invalid instantiation");
}

template <typename DERIVED1, typename DERIVED2>
void
ftpad(Eigen::SparseMatrixBase<DERIVED1> &dst,
      const Eigen::SparseMatrixBase<DERIVED2> &src,
      int nrows,
      int ncols)
{
  BOOST_VERIFY(nrows >= src.rows() && ncols >= src.cols());

  // new row and column offsets
  auto offset = [](int nnew, int nold) {
    if ((nnew % 2) == (nold % 2)) {
      /*  (even,even) or (odd, odd) */
      return (nnew - nold) / 2;
    } else if (nnew % 2 == 0 && nold % 2 == 1) {
      /*  (even, odd)  */
      return (nnew - nold + 1) / 2;
    } else if (nnew % 2 == 1 && nold % 2 == 0) {
      /* (odd, even)  */
      return (nnew - nold) / 2;
    } else {
      assert(false);
    }
  };

  int row_offset = offset(nrows, src.rows());
  int col_offset = offset(ncols, src.cols());

  // todo: this could be implemented (on-the-fly) as a transformation returning
  // an iterator
  std::vector<Eigen::Triplet<double, int>> triplets(src.derived().nonZeros());
  int count = 0;
  for (int ii = 0; ii < src.outerSize(); ++ii) {
    for (typename DERIVED2::InnerIterator it(src.derived(), ii); it; ++it) {
      triplets[count++] =
          Eigen::Triplet<double, int>(it.row() + row_offset, it.col() + col_offset, it.value());
    }
  }
  BOOST_VERIFY(count == src.derived().nonZeros());
  dst.derived().resize(nrows, ncols);
  dst.derived().setFromTriplets(triplets.begin(), triplets.end());
}

template <typename DERIVED1, typename DERIVED2>
void
ftpad(Eigen::SparseMatrixBase<DERIVED2> &src, int nrows, int ncols)
{
  BOOST_VERIFY(nrows >= src.rows() && ncols >= src.cols());

  // new row and column offsets
  auto offset = [](int nnew, int nold) {
    if ((nnew % 2) == (nold % 2)) {
      /*  (even,even) or (odd, odd) */
      return (nnew - nold) / 2;
    } else if (nnew % 2 == 0 && nold % 2 == 1) {
      /*  (even, odd)  */
      return (nnew - nold + 1) / 2;
    } else if (nnew % 2 == 1 && nold % 2 == 0) {
      /* (odd, even)  */
      return (nnew - nold) / 2;
    } else {
      assert(false);
    }
  };

  int row_offset = offset(nrows, src.rows());
  int col_offset = offset(ncols, src.cols());

  // todo: this could be implemented (on-the-fly) as a transformation returning
  // an iterator
  std::vector<Eigen::Triplet<double, int>> triplets(src.nonZeros());
  int count = 0;
  for (int ii = 0; ii < src.outerSize(); ++ii) {
    for (typename DERIVED2::InnerIterator it(src.derived(), ii); it; ++it) {
      triplets[count++] =
          Eigen::Triplet<double, int>(it.row() + row_offset, it.col() + col_offset, it.value());
    }
  }
  BOOST_VERIFY(count == src.derived().nonZeros());
  src.derived().resize(nrows, ncols);
  src.derived().setFromTriplets(triplets.begin(), triplets.end());
}

/**
 *  @brief set maximal negative frequency coefficients to zero
 *
 *
 *  @param Y Fourier coefficients in zero-centered frequency storage
 */
template <typename DERIVED>
void
hf_zero(Eigen::DenseBase<DERIVED> &Y)
{
  Y.derived().col(0).setZero();
  Y.derived().row(0).setZero();
}


//
//
// The MIT License (MIT)
//
// Copyright (c) 2016  Michael J. Wouters
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include <stdio.h>
#include <cstdlib>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <cstring>
#include <cmath>

#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <vector>
#include <boost/concept_check.hpp>

#include "Antenna.h"
#include "Application.h"
#include "Debug.h"
#include "GPS.h"
#include "HexBin.h"
#include "Ublox.h"
#include "ReceiverMeasurement.h"
#include "SVMeasurement.h"

extern ostream *debugStream;
extern Application *app;

#define CLIGHT 299792458.0
#define ICD_PI 3.1415926535898

#define MAX_CHANNELS 16 // max channels per constellation
#define SLOPPINESS 0.1
#define CLOCKSTEP  0.001
#define MAX_GAP    3   // maximum permissible gap in observation sequence before ambiguity resolution is triggered
// types used by ublox receivers
typedef signed char    I1;
typedef unsigned char  U1;
typedef unsigned char  X1;
typedef unsigned short U2;
typedef short          I2;
typedef int            I4;
typedef unsigned int   U4;
typedef float          R4;
typedef double         R8;

// messages that are parsed
#define MSG0121 0x01
#define MSG0122 0x02
#define MSG0215 0x04
#define MSG0D01 0x08

Ublox::Ublox(Antenna *ant,string m):Receiver(ant)
{
	modelName=m;
	manufacturer="ublox";
	swversion="0.1";
	constellations=GNSSSystem::GPS;
	codes=GNSSSystem::C1;
	channels=32;
	if (modelName == "LEA8MT"){
		// For the future
	}
	else if (modelName == "NEO8MT"){
	}
	else{
		app->logMessage("Unknown receiver model: " + modelName);
		app->logMessage("Assuming NEO8MT");
	}
}

Ublox::~Ublox()
{
}

bool Ublox::readLog(string fname,int mjd,int startTime,int stopTime,int rinexObsInterval)
{
	DBGMSG(debugStream,INFO,"reading " << fname);

	ifstream infile (fname.c_str());
	string line;
	int linecount=0;

	string msgid,currpctime,pctime,msg,gpstime;

	I4 sawtooth;
	I4 clockBias;
	R8 measTOW;
	U2 measGPSWN;
	I1 measLeapSecs;

	U2 UTCyear;
	U1 UTCmon,UTCday,UTChour,UTCmin,UTCsec,UTCvalid;

	U1 u1buf;
	I2 i2buf;
	I4 i4buf;
	U4 u4buf;
	R4 r4buf;
	R8 r8buf;

	float rxTimeOffset; // single

	vector<SVMeasurement *> svmeas;

	gotUTCdata=false;
	gotIonoData=false;

	unsigned int currentMsgs=0;
	unsigned int reqdMsgs =  MSG0121 | MSG0122 | MSG0215 | MSG0D01 ;

  if (infile.is_open()){
    while ( getline (infile,line) ){
			linecount++;

			if (line.size()==0) continue; // skip empty line
			if ('#' == line.at(0)) continue; // skip comments
			if ('%' == line.at(0)) continue;
			if ('@' == line.at(0)) continue;

			stringstream sstr(line);
			sstr >> msgid >> currpctime >> msg;
			if (sstr.fail()){
				DBGMSG(debugStream,WARNING," bad data at line " << linecount);
				currentMsgs=0;
				deleteMeasurements(svmeas);
				continue;
			}

			// The 0x0215 message starts each second
			if(msgid == "0215"){ // raw measurements

				if (currentMsgs == reqdMsgs){ // save the measurements from the previous second
					if (svmeas.size() > 0){
						ReceiverMeasurement *rmeas = new ReceiverMeasurement();
						measurements.push_back(rmeas);

						rmeas->sawtooth=sawtooth*1.0E-12; // units are ps, must be added to TIC measurement
						rmeas->timeOffset=clockBias*1.0E-9; // units are ns WARNING no sign convention defined yet ...

						int pchh,pcmm,pcss;
						if ((3==sscanf(pctime.c_str(),"%d:%d:%d",&pchh,&pcmm,&pcss))){
							rmeas->pchh=pchh;
							rmeas->pcmm=pcmm;
							rmeas->pcss=pcss;
						}

						// GPSTOW is used for pseudorange estimations
						// note: this is rounded because measurements are interpolated on a 1s grid
						rmeas->gpstow=rint(measTOW);
						rmeas->gpswn=measGPSWN % 1024; // Converted to truncated WN. Not currently used

						// UTC time of measurement
						// We could use other time information to calculate this eg gpstow,gpswn and leap seconds
						rmeas->tmUTC.tm_sec=UTCsec;
						rmeas->tmUTC.tm_min=UTCmin;
						rmeas->tmUTC.tm_hour=UTChour;
						rmeas->tmUTC.tm_mday=UTCday;
						rmeas->tmUTC.tm_mon=UTCmon-1;
						rmeas->tmUTC.tm_year=UTCyear-1900;
						rmeas->tmUTC.tm_isdst=0;

						// Calculate GPS time of measurement
						// FIXME why do this ? why not just convert from UTC ? and full WN is known anyway
						time_t tgps = GPS::GPStoUnix(rmeas->gpstow,rmeas->gpswn);
						struct tm *tmGPS = gmtime(&tgps);
						rmeas->tmGPS=*tmGPS;

						//rmeas->tmfracs = measTOW - (int)(measTOW);
						//if (rmeas->tmfracs > 0.5) rmeas->tmfracs -= 1.0; // place in the previous second

						rmeas->tmfracs=0.0;

						if (constellations & GNSSSystem::GPS){
							for (unsigned int sv=0;sv<svmeas.size();sv++){
								svmeas.at(sv)->dbuf3 = svmeas.at(sv)->meas; // save for debugging
								svmeas.at(sv)->meas -= clockBias*1.0E-9; // evidently it is subtracted
								// Now subtract the ms part so that ms ambiguity resolution works
								svmeas.at(sv)->meas -= 1.0E-3*floor(svmeas.at(sv)->meas/1.0E-3);
								svmeas.at(sv)->rm=rmeas;
							}
							rmeas->meas=svmeas;
							svmeas.clear(); // don't delete - we only made a shallow copy!
						}

						// KEEP THIS it's useful for debugging measurement-time related problems
					//fprintf(stderr,"PC=%02d:%02d:%02d tmUTC=%02d:%02d:%02d tmGPS=%4d %02d:%02d:%02d gpstow=%d gpswn=%d measTOW=%.12lf tmfracs=%g clockbias=%g\n",
					//	pchh,pcmm,pcss,UTChour,UTCmin,UTCsec, rmeas->tmGPS.tm_year+1900,rmeas->tmGPS.tm_hour, rmeas->tmGPS.tm_min,rmeas->tmGPS.tm_sec,
					//	(int) rmeas->gpstow,(int) rmeas->gpswn,measTOW,rmeas->tmfracs,clockBias*1.0E-9  );

					//fprintf(stderr,"%02d:%02d:%02d %02d:%02d:%02d %02d:%02d:%02d %d %d %.12lf %g %g\n",
					//pchh,pcmm,pcss,UTChour,UTCmin,UTCsec, rmeas->tmGPS.tm_hour, rmeas->tmGPS.tm_min,rmeas->tmGPS.tm_sec,
					//(int) rmeas->gpstow,(int) rmeas->gpswn,measTOW,rmeas->tmfracs,clockBias*1.0E-9  );

					}// if (gpsmeas.size() > 0)
				}
				else{
					DBGMSG(debugStream,TRACE,pctime << " reqd message missing, flags = " << currentMsgs);
					deleteMeasurements(svmeas);
				}

				pctime=currpctime;
				currentMsgs = 0;

				if (msg.size()-2*2-16*2 > 0){ // don't know the expected message size yet but if we've got the header ...
					HexToBin((char *) msg.substr(11*2,2*sizeof(U1)).c_str(),sizeof(U1),(unsigned char *) &u1buf);
					unsigned int nmeas=u1buf;
					if (msg.size() == (2+16+nmeas*32)*2){
						HexToBin((char *) msg.substr(0*2,2*sizeof(R8)).c_str(),sizeof(R8),(unsigned char *) &measTOW); //measurement TOW (s)
						HexToBin((char *) msg.substr(8*2,2*sizeof(U2)).c_str(),sizeof(U2),(unsigned char *) &measGPSWN); // full WN
						HexToBin((char *) msg.substr(10*2,2*sizeof(I1)).c_str(),sizeof(I1),(unsigned char *) &measLeapSecs);
						DBGMSG(debugStream,TRACE,currpctime << " meas tow=" << measTOW << setprecision(12) << " gps wn=" << (int) measGPSWN << " leap=" << (int) measLeapSecs);
						//DBGMSG(debugStream,TRACE,nmeas);
						for (unsigned int m=0;m<nmeas;m++){
							HexToBin((char *) msg.substr((36+32*m)*2,2*sizeof(U1)).c_str(),sizeof(U1),(unsigned char *) &u1buf); //GNSS id
							int gnssSys = 0;
							switch (u1buf){
								case 0: gnssSys=GNSSSystem::GPS; break;
								case 1:case 4: case 5: break;
								case 2: gnssSys=GNSSSystem::GALILEO; break;
								case 3: gnssSys=GNSSSystem::BEIDOU; break;
								case 6: gnssSys=GNSSSystem::GLONASS; break;
								default: break;
							}
							//DBGMSG(debugStream,TRACE,gnssSys);
							if (gnssSys & constellations ){
								// Since we get all the measurements in one message (which starts each second) there's no need to check for multiple measurement messages
								// like with eg the Resolution T
								HexToBin((char *) msg.substr((16+32*m)*2,2*sizeof(R8)).c_str(),sizeof(R8),(unsigned char *) &r8buf); //pseudorange (m)
								HexToBin((char *) msg.substr((37+32*m)*2,2*sizeof(U1)).c_str(),sizeof(U1),(unsigned char *) &u1buf); //svid
								int svID=u1buf;
								HexToBin((char *) msg.substr((46+32*m)*2,2*sizeof(U1)).c_str(),sizeof(U1),(unsigned char *) &u1buf);
								int prStdDev= u1buf & 0x0f;
								HexToBin((char *) msg.substr((46+32*m)*2,2*sizeof(U1)).c_str(),sizeof(U1),(unsigned char *) &u1buf);
								int trkStat=u1buf;
								// When PR is reported, trkStat is always 1 but .
								if (trkStat > 0 && r8buf/CLIGHT < 1.0){
									SVMeasurement *svm = new SVMeasurement(svID,gnssSys,GNSSSystem::C1,r8buf/CLIGHT,NULL);
									//svm->dbuf1=0.01*pow(2.0,prStdDev);
									svmeas.push_back(svm);
								}
								DBGMSG(debugStream,TRACE,"SYS " <<gnssSys << " SV" << svID << " pr=" << r8buf/CLIGHT << setprecision(8) << " trkStat= " << (int) trkStat);
							}
						}
						currentMsgs |= MSG0215;
					}
					else{
						DBGMSG(debugStream,WARNING,"Bad 0215 message size");
					}
				}
				else{
					DBGMSG(debugStream,WARNING,"empty/malformed 0215 message");
				}

				continue;

			} // raw measurements

			// 0x0D01 Timepulse time data (sawtooth correction)
			if(msgid == "0d01"){

				if (msg.size()==(16+2)*2){
					X1 TPflags,TPrefInfo;
					U4 TPTOW;
					HexToBin((char *) msg.substr(0*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &TPTOW); // (ms)
					HexToBin((char *) msg.substr(8*2,2*sizeof(I4)).c_str(),sizeof(I4),(unsigned char *) &sawtooth); // (ps)
					HexToBin((char *) msg.substr(14*2,2*sizeof(X1)).c_str(),sizeof(X1),(unsigned char *) &TPflags);
					HexToBin((char *) msg.substr(15*2,2*sizeof(X1)).c_str(),sizeof(X1),(unsigned char *) &TPrefInfo);
					DBGMSG(debugStream,TRACE,currpctime << " tow= " << (int) TPTOW << " sawtooth=" << sawtooth << " ps" << std::hex << " flags=0x" << (unsigned int) TPflags <<
						" ref=0x" << (unsigned int) TPrefInfo << std::dec);
					currentMsgs |= MSG0D01;
				}
				else{
					DBGMSG(debugStream,WARNING,"Bad 0d01 message size");
				}
			}
			// 0x0135 UBX-NAV-SAT satellite information

			// 0x0121 UBX-NAV-TIME-UTC UTC time solution
			if(msgid == "0121"){
				if (msg.size()==(20+2)*2){
					HexToBin((char *) msg.substr(12*2,2*sizeof(U2)).c_str(),sizeof(U2),(unsigned char *) &UTCyear);
					HexToBin((char *) msg.substr(14*2,2*sizeof(U1)).c_str(),sizeof(U1),(unsigned char *) &UTCmon);
					HexToBin((char *) msg.substr(15*2,2*sizeof(U1)).c_str(),sizeof(U1),(unsigned char *) &UTCday);
					HexToBin((char *) msg.substr(16*2,2*sizeof(U1)).c_str(),sizeof(U1),(unsigned char *) &UTChour);
					HexToBin((char *) msg.substr(17*2,2*sizeof(U1)).c_str(),sizeof(U1),(unsigned char *) &UTCmin);
					HexToBin((char *) msg.substr(18*2,2*sizeof(U1)).c_str(),sizeof(U1),(unsigned char *) &UTCsec);
					HexToBin((char *) msg.substr(19*2,2*sizeof(X1)).c_str(),sizeof(X1),(unsigned char *) &UTCvalid);
					DBGMSG(debugStream,TRACE,currpctime << " UTC:" << UTCyear << " " << (int) UTCmon << " " << (int) UTCday << " "
						<< (int) UTChour << ":" << (int) UTCmin << ":" << (int) UTCsec << " valid=" << (unsigned int) UTCvalid);
					if (UTCvalid & 0x04)
						currentMsgs |= MSG0121;
					else{
						DBGMSG(debugStream,WARNING,"UTC not valid yet");
					}
				}
				else{
					DBGMSG(debugStream,WARNING,"Bad 0121 message size");
				}
				continue;
			}

			// 0x0122 UBX-NAV-CLOCK clock solution  (clock bias)
			if(msgid == "0122"){
				if (msg.size()==(20+2)*2){
						HexToBin((char *) msg.substr(0*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf); // GPS tow of navigation epoch (ms)
						HexToBin((char *) msg.substr(4*2,2*sizeof(I4)).c_str(),sizeof(I4),(unsigned char *) &clockBias); // in ns

						DBGMSG(debugStream,TRACE,"GPS tow=" << u4buf << "ms" << " clock bias=" << clockBias << " ns");
						currentMsgs |= MSG0122;
				}
				else{
					DBGMSG(debugStream,WARNING,"Bad 0122 message size");
				}
				continue;
			}

			//
			// Messages needed to contruct the RINEX navigation file
			//

			// Ionosphere parameters, UTC parameters
			if (!gotUTCdata){
				if(msgid == "0b02"){
					if (msg.size()==(72+2)*2){
						HexToBin((char *) msg.substr(4*2,2*sizeof(R8)).c_str(),sizeof(R8),(unsigned char *) &(gps.UTCdata.A0));
						HexToBin((char *) msg.substr(12*2,2*sizeof(R8)).c_str(),sizeof(R8),(unsigned char *) &r8buf);
						gps.UTCdata.A1=r8buf;
						HexToBin((char *) msg.substr(20*2,2*sizeof(I4)).c_str(),sizeof(I4),(unsigned char *) &i4buf);
						gps.UTCdata.t_ot = i4buf;
						HexToBin((char *) msg.substr(24*2,2*sizeof(I4)).c_str(),sizeof(I4),(unsigned char *) &i2buf);
						gps.UTCdata.WN_t=i2buf;
						HexToBin((char *) msg.substr(26*2,2*sizeof(I4)).c_str(),sizeof(I4),(unsigned char *) &i2buf);
						leapsecs = i2buf;
						HexToBin((char *) msg.substr(28*2,2*sizeof(I4)).c_str(),sizeof(I4),(unsigned char *) &i2buf);
						gps.UTCdata.WN_LSF=i2buf;
						HexToBin((char *) msg.substr(30*2,2*sizeof(I4)).c_str(),sizeof(I4),(unsigned char *) &i2buf);
						gps.UTCdata.DN=i2buf;
						HexToBin((char *) msg.substr(32*2,2*sizeof(I4)).c_str(),sizeof(I4),(unsigned char *) &i2buf);
						gps.UTCdata.dt_LSF=i2buf;

						HexToBin((char *) msg.substr(36*2,2*sizeof(R4)).c_str(),sizeof(R4),(unsigned char *) &(gps.ionoData.a0));
						HexToBin((char *) msg.substr(40*2,2*sizeof(R4)).c_str(),sizeof(R4),(unsigned char *) &(gps.ionoData.a1));
						//gps.ionoData.a1 /= ICD_PI;
						HexToBin((char *) msg.substr(44*2,2*sizeof(R4)).c_str(),sizeof(R4),(unsigned char *) &(gps.ionoData.a2));
						//gps.ionoData.a2 /= (ICD_PI*ICD_PI);
						HexToBin((char *) msg.substr(48*2,2*sizeof(R4)).c_str(),sizeof(R4),(unsigned char *) &(gps.ionoData.a3));
						//gps.ionoData.a3 /= (ICD_PI*ICD_PI*ICD_PI);

						HexToBin((char *) msg.substr(52*2,2*sizeof(R4)).c_str(),sizeof(R4),(unsigned char *) &(gps.ionoData.B0));
						HexToBin((char *) msg.substr(56*2,2*sizeof(R4)).c_str(),sizeof(R4),(unsigned char *) &(gps.ionoData.B1));
						//gps.ionoData.B1 /= ICD_PI;
						HexToBin((char *) msg.substr(60*2,2*sizeof(R4)).c_str(),sizeof(R4),(unsigned char *) &(gps.ionoData.B2));
						//gps.ionoData.B2 /= (ICD_PI*ICD_PI);
						HexToBin((char *) msg.substr(64*2,2*sizeof(R4)).c_str(),sizeof(R4),(unsigned char *) &(gps.ionoData.B3));
						//gps.ionoData.B3 /= (ICD_PI*ICD_PI*ICD_PI);

						gotUTCdata=true;
						gotIonoData=true;
					}
					else{
						DBGMSG(debugStream,WARNING,"Bad 0b02 message size");
					}
					continue;
				}
			}

			// Ephemeris
			if(msgid == "0b31"){
				if (msg.size()==(8+2)*2){
					DBGMSG(debugStream,WARNING,"Empty ephemeris");
				}
				else if (msg.size()==(104+2)*2){
					GPS::EphemerisData *ed = decodeGPSEphemeris(msg);
					int pchh,pcmm,pcss;
						if ((3==sscanf(pctime.c_str(),"%d:%d:%d",&pchh,&pcmm,&pcss)))
							ed->tLogged = pchh*3600 + pcmm*60 + pcss;
						else
							ed->tLogged = -1;
					gps.addEphemeris(ed);
				}
				else{
					DBGMSG(debugStream,WARNING,"Bad 0b31 message size");
				}
				continue;
			} // ephemeris
		}
	} // infile is open
	else{
		app->logMessage(" unable to open " + fname);
		return false;
	}
	infile.close();

	if (!gotUTCdata){
		app->logMessage("failed to find ionosphere/UTC parameters - no 0b02 messages");
		//return false; // FIXME temporary
	}

	if (measurements.size() == 0){
		app->logMessage(" no measurements available in " + fname);
		return false;
	}

	// Pass through the data to realign the sawtooth correction.
	// This could be done in the main loop but it's more flexible this way.
	// For the ublox, the sawtooth correction applies to the next second
	// If we're missing the sawtooth correction because of eg a gap in the data
	// then we'll just use the current sawtooth.

	DBGMSG(debugStream,TRACE,"Fixing sawtooth");
	double prevSawtooth=measurements.at(0)->sawtooth;
	time_t    tPrevSawtooth=mktime(&(measurements.at(0)->tmUTC));
	int nBadSawtoothCorrections =1; // first is bad !
	// First point is untouched
	for (unsigned int i=1;i<measurements.size();i++){
		double sawTmp = measurements.at(i)->sawtooth;
		time_t tTmp= mktime(&(measurements.at(i)->tmUTC));
		if (tTmp - tPrevSawtooth == 1){
			measurements.at(i)->sawtooth = prevSawtooth;
		}
		else{
			// do nothing - the current value is the best guess
			nBadSawtoothCorrections++;
		}
		prevSawtooth=sawTmp;
		tPrevSawtooth=tTmp;
	}

	// The Ublox sometime reports what appears to be an incorrect pseudorange after picking up an SV
	// If you wanted to filter these out, this is where you should do it

	// Fix 1 ms ambiguities/steps in the pseudorange
	// Do this initially and then every time a step is detected

	DBGMSG(debugStream,TRACE,"Fixing ms ambiguities");
	int nDropped;
	for (int g=GNSSSystem::GPS;g<=GNSSSystem::GALILEO;(g <<= 1)){
		if (!(g & constellations)) continue;
		GNSSSystem *gnss;
		switch (g){
			case GNSSSystem::BEIDOU:gnss=&beidou;break;
			case GNSSSystem::GALILEO:gnss=&galileo;break;
			case GNSSSystem::GLONASS:gnss=&glonass;break;
			case GNSSSystem::GPS:gnss=&gps;break;
			default:break;
		}
		for (int svn=1;svn<=gnss->nsats();svn++){
			unsigned int lasttow=99999999,currtow=99999999;
			double lastmeas=0,currmeas;
			double corr=0.0;
			bool first=true;
			bool ok=false;
			for (unsigned int i=0;i<measurements.size();i++){
				unsigned int m=0;
				while (m < measurements[i]->meas.size()){
					if ((svn==measurements[i]->meas[m]->svn) && (measurements[i]->meas[m]->code == GNSSSystem::C1)){
						lasttow=currtow;
						lastmeas=currmeas;
						currmeas=measurements[i]->meas[m]->meas;
						currtow=measurements[i]->gpstow;

						DBGMSG(debugStream,TRACE,svn << " " << currtow << " " << currmeas << " ");
						if (first){
							ok = gnss->resolveMsAmbiguity(antenna,measurements[i],measurements[i]->meas[m],&corr);
							if (ok){
								first=false;
							}
						}
						else if (currtow - lasttow > MAX_GAP){
							DBGMSG(debugStream,TRACE,"gap " << svn << " " << lasttow << "," << lastmeas << "->" << currtow << "," << currmeas);
							ok = gnss->resolveMsAmbiguity(antenna,measurements[i],measurements[i]->meas[m],&corr);
						}
						else if (currtow > lasttow){
							if (fabs(currmeas-lastmeas) > CLOCKSTEP*SLOPPINESS){
								DBGMSG(debugStream,TRACE,"first/step " << svn << " " << lasttow << "," << lastmeas << "->" << currtow << "," << currmeas);
								ok = gnss->resolveMsAmbiguity(antenna,measurements[i],measurements[i]->meas[m],&corr);
							}
						}
						if (ok){
							measurements[i]->meas[m]->meas += corr;
							m++;
						}
						else{ // ambiguity correction failed, so drop the measurement
							nDropped++;
							measurements[i]->meas.erase(measurements[i]->meas.begin()+m); // FIXME memory leak
						}
						break;
					}
					else
						m++;

				} //
			} // for (unsigned int i=0;i<measurements.size();i++){
		}
	}

	DBGMSG(debugStream,INFO,"done: read " << linecount << " lines");
	DBGMSG(debugStream,INFO,measurements.size() << " measurements read");
	DBGMSG(debugStream,INFO,gps.ephemeris.size() << " GPS ephemeris entries read");
	DBGMSG(debugStream,INFO,nBadSawtoothCorrections << " bad sawtooth corrections");
	DBGMSG(debugStream,INFO,"dropped " << nDropped << " SV measurements (ms ambiguity failure)");

	return true;

}

GPS::EphemerisData* Ublox::decodeGPSEphemeris(string msg)
{
	U4 u4buf;
	HexToBin((char *) msg.substr(0*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
	GPS::EphemerisData* ed= new GPS::EphemerisData();
	ed->SVN=u4buf;
	DBGMSG(debugStream,TRACE,"Ephemeris for SV" << (int) ed->SVN);

	// cerr << "EPH" << endl;

	// To use the MID macro, LSB is bit 0, m is first bit, n is last bit
	#define LAST(k,n) ((k) & ((1<<(n))-1))
	#define MID(k,m,n) LAST((k)>>(m),((n)-(m)+1))

	// Data is in bits 0-23, parity bits are discarded by ublox
	// To translate from ICD numbering b24 (ICD) -> b0 (ublox)
	// subframe 1
	// word 3
	HexToBin((char *) msg.substr(8*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);

	ed->week_number = MID(u4buf,14,23);
	ed->SV_accuracy_raw=MID(u4buf,8,11);
	ed->SV_accuracy = GPS::URA[ed->SV_accuracy_raw];
	ed->SV_health=MID(u4buf,2,7);
	// IODC b23-b24 (upper bits)
	unsigned int hibits=MID(u4buf,0,1);
	hibits = hibits << 8;

	//fprintf(stderr,"%i %08x %i %i %i\n",(int) ed->SVN,u4buf, (int) ed->week_number,
	//	(int) ed->SV_accuracy, ed->SV_health);

	// word 7
	HexToBin((char *) msg.substr(24*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
	//Tgd b17-b24 (ICD) CHECKED
	signed char tGD = MID(u4buf,0,7); // signed, scaled by 2^-31
	ed->t_GD =  (double) tGD / (double) pow(2,31);

	//fprintf(stderr,"%08x %.12e\n",u4buf,ed->t_GD);

	// word 8
	HexToBin((char *) msg.substr(28*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
	// IODC b1-b8 (lower bits) // CHECKED
	unsigned int lobits = MID(u4buf,16,23);
	ed->IODC = hibits | lobits;
	// t_OC b9-b24 (ICD)
	ed->t_OC = 16*MID(u4buf,0,15);
	//fprintf(stderr,"%08x %e %i\n",u4buf,ed->t_OC,(int) ed->IODC);

	// word 9 a_f2 b1-b8, a_f1 b9-b24 // CHECKED a_f1
	HexToBin((char *) msg.substr(32*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
	signed char af2 = MID(u4buf,16,23);
	ed->a_f2 = af2/pow(2,55);
	signed short af1= MID(u4buf,0,15);
	ed->a_f1 = (double) af1/(double) pow(2,43);

	//fprintf(stderr,"%08x %.12e %.12e\n",u4buf,ed->a_f1,ed->a_f2);
	if (ed->a_f2 != 0.0) fprintf(stderr,"BING!\n");

	// word 10 a_f0 b1-b22 // CHECKED
	HexToBin((char *) msg.substr(36*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
	int tmp = (MID(u4buf,2,23) << 10);
	tmp = tmp >> 10;
	ed->a_f0 = (double) tmp /(double) pow(2,31); // signed, scaled by 2^-31
	//fprintf(stderr,"%08x %.12e\n",u4buf,ed->a_f0);

	// data frame 2
	// word 3
	// IODE b1-b8 // CHECKED
	HexToBin((char *) msg.substr(40*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
	ed->IODE = MID(u4buf,16,23);
	// C_rs b9-b24 // CHECKED
	signed short Crs= MID(u4buf,0,15);
	ed->C_rs= (double) Crs/ (double) 32.0;
	//fprintf(stderr,"%08x %i %.12e\n",u4buf,ed->IODE,ed->C_rs);

	// word 4
	// deltaN b1-b16 // CHECKED nb this is a SINGLE so differences in 7 or 8th digit in RINEX files
	HexToBin((char *) msg.substr(44*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
	signed short deltaN=MID(u4buf,8,23);
	ed->delta_N = ICD_PI*(double) deltaN/(double) pow(2,43); // GPS units are semi-circles/s, RINEX units are rad/s
	// M_0 (upper 8 bits) b17-b24
	hibits =  MID(u4buf,0,7) << 24;

	// word 5
	// M_0 (lower 24 bits) b1-b24 // CHECKED
	HexToBin((char *) msg.substr(48*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
	lobits = MID(u4buf,0,23);

	ed->M_0 = ICD_PI * ((double) ((int) (hibits | lobits)))/ (double) pow(2,31);
	//fprintf(stderr,"%08x %.12e %.12e\n",u4buf,ed->delta_N, ed->M_0);

	// word 6
	// C_uc b1-b16 // CHECKED
	HexToBin((char *) msg.substr(52*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
	signed short Cuc=MID(u4buf,8,23);
	ed->C_uc = (double) Cuc/(double) pow(2,29);
	// e b17-b24 (upper 8 bits)
	hibits =  MID(u4buf,0,7) << 24;

	// word 7
	// e b1-b24 (lower 24 bits) // CHECKED
	HexToBin((char *) msg.substr(56*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
	lobits = MID(u4buf,0,23);
	ed->e = ((double) (unsigned int)((hibits | lobits)))/ (double) pow(2,33);
	//fprintf(stderr,"%08x %.12e %.12e \n",u4buf,ed->C_uc,ed->e);

	// word 8
	// C_us b1-b16 // CHECKED
	HexToBin((char *) msg.substr(60*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
	signed short Cus=MID(u4buf,8,23);
	ed->C_us = (double) Cus/(double) pow(2,29);
	// sqrtA b1-b8 (upper bits)
	hibits =  MID(u4buf,0,7) << 24;

	// word 9
	//sqrtA b1-b24 (lower bits) // CHECKED
	HexToBin((char *) msg.substr(64*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
	lobits = MID(u4buf,0,23);
	ed->sqrtA = ((double) (unsigned int)((hibits | lobits)))/ (double) pow(2,19);
	//fprintf(stderr,"%08x %.12e %.12e\n",u4buf,ed->C_us,ed->sqrtA);

	// word 10
	// t_OE b1-b16 // CHECKED
	HexToBin((char *) msg.substr(68*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
	unsigned short toe=MID(u4buf,8,23);
	ed->t_oe = toe * 16;
	//fprintf(stderr,"%08x %.12e \n",u4buf,ed->t_oe);

	// data frame 3
	// word 3
	// C_ic b1-b16 // CHECKED
	HexToBin((char *) msg.substr(72*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
	signed short Cic=MID(u4buf,8,23);
	ed->C_ic = (double) Cic/(double) pow(2,29);
	// OMEGA_0 b17-b24 (upper bits)
	hibits =  MID(u4buf,0,7) << 24;

	// word 4
	// OMEGA_0 b1-b24 lower bits // CHECKED
	HexToBin((char *) msg.substr(76*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
	lobits = MID(u4buf,0,23);
	ed->OMEGA_0 = ICD_PI * ((double) (signed int)((hibits | lobits)))/ (double) pow(2,31);
	//fprintf(stderr,"%08x %.12e %.12e\n",u4buf,ed->C_ic,ed->OMEGA_0);

	// word 5
	// C_is b1-b16 // CHECKED
	HexToBin((char *) msg.substr(80*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
	signed short Cis=MID(u4buf,8,23);
	ed->C_is= (double) Cis/(double) pow(2,29);
	// i_0 b17-b24 (upper bits)
	hibits =  MID(u4buf,0,7) << 24;

	// word 6
	// i_0 b1-b24 (lower bits) // CHECKED
	HexToBin((char *) msg.substr(84*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
	lobits = MID(u4buf,0,23);
	ed->i_0 = ICD_PI * ((double) (signed int)((hibits | lobits)))/ (double) pow(2,31);
	//fprintf(stderr,"%08x %.12e %.12e\n",u4buf,ed->C_is,ed->i_0);

	// word 7
	// C_rc b1-b16 // CHECKED
	HexToBin((char *) msg.substr(88*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
	signed short Crc=MID(u4buf,8,23);
	ed->C_rc= (double) Crc/32.0;
	// OMEGA b17-b24 (upper bits)
	hibits =  MID(u4buf,0,7) << 24;

	// word 8
	// OMEGA b1-b24 (lower bits) // CHECKED
	HexToBin((char *) msg.substr(92*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
	lobits = MID(u4buf,0,23);
	ed->OMEGA = ICD_PI * ((double) (signed int)((hibits | lobits)))/ (double) pow(2,31);
	//fprintf(stderr,"%08x %.12e %.12e\n",u4buf,ed->C_rc,ed->OMEGA);

	// word 9
	// OMEGA_DOT b1-b24 // CHECKED
	HexToBin((char *) msg.substr(96*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
	int odot = (MID(u4buf,0,23)) << 8;
	odot = odot >> 8;
	ed->OMEGADOT = ICD_PI * (double) (odot)/ (double) pow(2,43);
	//fprintf(stderr,"%08x %.12e\n",u4buf,ed->OMEGADOT);

	// word 10
	// IODE b1-b8 (repeated to facilitate checking for data cutovers) ... but which one should I use ???
	// IDOT b9-b22 // CHECKED
	HexToBin((char *) msg.substr(100*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
	int idot = (MID(u4buf,2,15)) << 18;
	idot = idot >> 18;
	ed->IDOT = ICD_PI * (double) (idot)/ (double) pow(2,43);
	//fprintf(stderr,"%08x %.12e\n",u4buf,ed->IDOT);

	return ed;
}


/*
 * Copyright (c) 2020-2021 Adrian Georg Herrmann
 *
 * This contains code for the HEMS data types and structures.
 */

#include <algorithm>
#include <map>
#include <string>
#include <cmath>

#include <boost/date_time/posix_time/posix_time.hpp>

#include "hems/common/types.h"

namespace hems { namespace types {

    bool operator==(const settings_t& lhs, const settings_t& rhs) {
        if (lhs.longitude == rhs.longitude && lhs.latitude == rhs.latitude &&
            lhs.timezone == rhs.timezone && lhs.pv_uri == rhs.pv_uri &&
            lhs.station_intervals == rhs.station_intervals && lhs.station_uris == rhs.station_uris &&
            lhs.interval_energy_production == rhs.interval_energy_production &&
            lhs.interval_energy_consumption == rhs.interval_energy_consumption &&
            lhs.interval_automation == rhs.interval_automation) {
            return true;
        } else {
            return false;
        }
    }

    bool operator!=(const settings_t& lhs, const settings_t& rhs) {
        return !(lhs == rhs);
    }

    std::string to_string(const settings_t& entry) {
        std::string str = "("
            "longitude: " + std::to_string(entry.longitude) + ", " +
            "latitude: " + std::to_string(entry.latitude) + ", " +
            "timezone: " + std::to_string(entry.timezone) + ", " +
            "pv_uri: '" + entry.pv_uri + "', " +
            "station_intervals: (";
        if (entry.station_intervals.empty()) {
            str += "None";
        } else {
            for (const auto& station_interval : entry.station_intervals) {
                str +=
                    std::to_string(station_interval.first) + " -> " +
                    std::to_string(station_interval.second) + ", ";
            }
            str.pop_back();
            str.pop_back();
        }
        str += "), station_uris: (";
        if (entry.station_uris.empty()) {
            str += "None";
        } else {
            for (const auto& station_uris : entry.station_uris) {
                str +=
                    std::to_string(station_uris.first) + " -> " +
                    "'" + station_uris.second + "', ";
            }
            str.pop_back();
            str.pop_back();
        }
        str += "), ";
        str +=
            "interval_energy_production: " + std::to_string(entry.interval_energy_production) + ", " +
            "interval_energy_consumption: " + std::to_string(entry.interval_energy_consumption) + ", " +
            "interval_automation: " + std::to_string(entry.interval_automation);
        str += ")";
        return str;
    }

    const settings_t settings_undefined;

    bool is_undefined(const settings_t& entry) {
        return entry.longitude == 0 && entry.latitude == 0 && entry.timezone == 0;
    }

    bool operator==(const appliance_t& lhs, const appliance_t& rhs) {
        if (lhs.name != rhs.name || lhs.uri != rhs.uri || lhs.rating != rhs.rating ||
            lhs.duty_cycle != rhs.duty_cycle || lhs.schedules_per_week != rhs.schedules_per_week ||
            lhs.tasks.size() != rhs.tasks.size() || lhs.auto_profiles.size() != rhs.auto_profiles.size()) {
            return false;
        } else if (!std::is_permutation(lhs.tasks.begin(), lhs.tasks.end(), rhs.tasks.begin())) {
            return false;
        } else if (
            !std::is_permutation(lhs.auto_profiles.begin(), lhs.auto_profiles.end(), rhs.auto_profiles.begin())
        ) {
            return false;
        } else {
            return true;
        }
    }

    bool operator!=(const appliance_t& lhs, const appliance_t& rhs) {
        return !(lhs == rhs);
    }

    std::string to_string(const appliance_t& entry) {
        std::string str = "("
            "id: " + std::to_string(entry.id) + ", "
            "name: '" + entry.name + "', "
            "uri: '" + entry.uri + "', "
            "rating: " + std::to_string(entry.rating) + ", "
            "duty_cycle: " + std::to_string(entry.duty_cycle) + ", "
            "schedules_per_week: " + std::to_string(entry.schedules_per_week) + ", "
            "tasks: (";
        if (entry.tasks.empty()) {
            str += "None";
        } else {
            for (const auto& task : entry.tasks) {
                str += std::to_string(task) + ", ";
            }
            str.pop_back();
            str.pop_back();
        }
        str += "), auto_profiles: (";
        if (entry.auto_profiles.empty()) {
            str += "None";
        } else {
            for (const auto& auto_profile : entry.auto_profiles) {
                str += std::to_string(auto_profile) + ", ";
            }
            str.pop_back();
            str.pop_back();
        }
        str += "))";
        return str;
    }

    bool operator==(const task_t& lhs, const task_t& rhs) {
        if (lhs.name != rhs.name || lhs.start_time != rhs.start_time || lhs.end_time != rhs.end_time ||
            lhs.auto_profile != rhs.auto_profile || lhs.is_user_declared != rhs.is_user_declared ||
            lhs.appliances.size() != rhs.appliances.size()) {
            return false;
        } else if (!std::is_permutation(lhs.appliances.begin(), lhs.appliances.end(), rhs.appliances.begin())) {
            return false;
        } else {
            return true;
        }
    }

    bool operator!=(const task_t& lhs, const task_t& rhs) {
        return !(lhs == rhs);
    }

    std::string to_string(const task_t& entry) {
        std::string str = "("
            "id: " + std::to_string(entry.id) + ", "
            "name: '" + entry.name + "', "
            "start_time: " + boost::posix_time::to_simple_string(entry.start_time) + ", "
            "end_time: " + boost::posix_time::to_simple_string(entry.end_time) + ", "
            "auto_profile: " + std::to_string(entry.auto_profile) + ", "
            "is_user_declared: " + (entry.is_user_declared ? "true" : "false") + ", "
            "appliances: (";
        if (entry.appliances.empty()) {
            str += "None";
        } else {
            for (const auto& appliance : entry.appliances) {
                str += std::to_string(appliance) + ", ";
            }
            str.pop_back();
            str.pop_back();
        }
        str += "))";
        return str;
    }

    bool operator==(const auto_profile_t& lhs, const auto_profile_t& rhs) {
        if (lhs.name != rhs.name || lhs.profile != rhs.profile ||
            lhs.appliances.size() != rhs.appliances.size() || lhs.tasks.size() != rhs.tasks.size()) {
            return false;
        } else if (!std::is_permutation(lhs.appliances.begin(), lhs.appliances.end(), rhs.appliances.begin())) {
            return false;
        } else if (!std::is_permutation(lhs.tasks.begin(), lhs.tasks.end(), rhs.tasks.begin())) {
            return false;
        } else {
            return true;
        }
    }

    bool operator!=(const auto_profile_t& lhs, const auto_profile_t& rhs) {
        return !(lhs == rhs);
    }

    std::string to_string(const auto_profile_t& entry) {
        std::string str = "("
            "id: " + std::to_string(entry.id) + ", "
            "name: '" + entry.name + "', "
            "profile: '" + entry.profile + "', "
            "appliances: (";
        if (entry.appliances.empty()) {
            str += "None";
        } else {
            for (const auto& appliance : entry.appliances) {
                str += std::to_string(appliance) + ", ";
            }
            str.pop_back();
            str.pop_back();
        }
        str += "), tasks: (";
        if (entry.tasks.empty()) {
            str += "None";
        } else {
            for (const auto& task : entry.tasks) {
                str += std::to_string(task) + ", ";
            }
            str.pop_back();
            str.pop_back();
        }
        str += "))";
        return str;
    }

    bool operator==(const energy_consumption_t& lhs, const energy_consumption_t& rhs) {
        if (lhs.time == rhs.time && lhs.appliance_id == rhs.appliance_id && lhs.energy == rhs.energy) {
            return true;
        } else {
            return false;
        }
    }

    bool operator!=(const energy_consumption_t& lhs, const energy_consumption_t& rhs) {
        return !(lhs == rhs);
    }

    std::string to_string(const energy_consumption_t& entry) {
        std::string str = "("
            "time: " + boost::posix_time::to_simple_string(entry.time) + ", "
            "appliance_id: " + std::to_string(entry.appliance_id) + ", "
            "energy: " + std::to_string(entry.energy) +
        ")";
        return str;
    }

    bool operator==(const energy_production_t& lhs, const energy_production_t& rhs) {
        if (lhs.time == rhs.time && lhs.energy == rhs.energy) {
            return true;
        } else {
            return false;
        }
    }

    bool operator!=(const energy_production_t& lhs, const energy_production_t& rhs) {
        return !(lhs == rhs);
    }

    std::string to_string(const energy_production_t& entry) {
        std::string str = "("
            "time: " + boost::posix_time::to_simple_string(entry.time) + ", "
            "energy: " + std::to_string(entry.energy) +
        ")";
        return str;
    }

    bool operator==(const weather_t& lhs, const weather_t& rhs) {
        if (lhs.time == rhs.time && lhs.station == rhs.station && lhs.temperature == rhs.temperature &&
            lhs.humidity == rhs.humidity && lhs.pressure == rhs.pressure &&
            lhs.cloud_cover == rhs.cloud_cover && lhs.radiation == rhs.radiation) {
            return true;
        } else {
            return false;
        }
    }

    bool operator!=(const weather_t& lhs, const weather_t& rhs) {
        return !(lhs == rhs);
    }

    std::string to_string(const weather_t& entry) {
        std::string str = "("
            "time: " + boost::posix_time::to_simple_string(entry.time) + ", "
            "temperature: " + std::to_string(entry.temperature) + ", "
            "humidity: " + std::to_string(entry.humidity) + ", "
            "pressure: " + std::to_string(entry.pressure) + ", "
            "cloud cover: " + std::to_string(entry.cloud_cover) + ", "
            "radiation: " + std::to_string(entry.radiation) +
        ")";
        return str;
    }

    bool operator==(const sunlight_t& lhs, const sunlight_t& rhs) {
        if (lhs.time == rhs.time && lhs.angle == rhs.angle) {
            return true;
        } else {
            return false;
        }
    }

    bool operator!=(const sunlight_t& lhs, const sunlight_t& rhs) {
        return !(lhs == rhs);
    }

    std::string to_string(const sunlight_t& entry) {
        std::string str = "("
            "time: " + boost::posix_time::to_simple_string(entry.time) + ", "
            "angle: " + std::to_string(entry.angle) +
        ")";
        return str;
    }

    sunlight_t get_angle(ptime time, double latitude, double longitude) {
        auto get_radians = [](double degrees) {
            return (degrees * M_PI) / 180;
        };

        auto get_degrees = [](double radians) {
            return (radians * 180) / M_PI;
        };

        auto get_julian_day = [](ptime time) {
            auto y = 0;
            auto m = 0;
            if (time.date().month() > 2) {
                y = time.date().year();
                m = time.date().month();
            } else {
                y = time.date().year() - 1;
                m = time.date().month() + 12;
            }
            auto d =
                time.date().day() + time.time_of_day().hours() / 24.0 +
                time.time_of_day().minutes() / 1440.0 + time.time_of_day().seconds() / 86400.0;
            auto b = 2 - std::floor(y / 100) + std::floor(y / 400);

            auto jd = std::floor(365.25 * (y + 4716)) + std::floor(30.6001 * (m + 1)) + d + b - 1524.5;
            return jd;
        };

        /* Source: https://de.wikipedia.org/wiki/Sonnenstand#Genauere_Ermittlung_des_Sonnenstandes_f%C3%BCr_einen_Zeitpunkt */

        /* 1. Eclipctical coordinates of the sun */

        /* Julian day */
        auto jd = get_julian_day(time);

        auto n = jd - 2451545;

        /* Median ecliptic longitude of the sun */
        auto l = std::fmod(280.46 + 0.9856474 * n, 360);

        /* Median anomaly */
        auto g = std::fmod(357.528 + 0.9856003 * n, 360);

        /* Ecliptic longitude of the sun */
        auto lbd = l + 1.915 * std::sin(get_radians(g)) + 0.01997 * std::sin(get_radians(2*g));

        /* 2. Equatorial coordinates of the sun */

        /* Ecliptic */
        auto eps = 23.439 - 0.0000004 * n;

        /* Right ascension */
        auto alpha = get_degrees(std::atan(std::cos(get_radians(eps)) * std::tan(get_radians(lbd))));
        if (std::cos(get_radians(lbd)) < 0) {
            alpha += 180;
        }

        /* Declination */
        auto delta = get_degrees(std::asin(std::sin(get_radians(eps)) * std::sin(get_radians(lbd))));

        /* 3. Horizontal coordinates of the sun */

        ptime time_(time.date(), boost::posix_time::time_duration(0, 0, 0));
        auto t0 = (get_julian_day(time_) - 2451545) / 36525;

        /* Median sidereal time */
        auto theta_hg = std::fmod(
            6.697376 + 2400.05134 * t0 + 1.002738 * (time.time_of_day().hours() + time.time_of_day().minutes() / 60.0),
            24
        );

        auto theta_g = theta_hg * 15;
        auto theta = theta_g + longitude;

        /* Hour angle of the sun */
        auto tau = theta - alpha;

        /* Elevation angle */
        auto h = std::cos(get_radians(delta)) * std::cos(get_radians(tau)) * std::cos(get_radians(latitude));
        h += std::sin(get_radians(delta)) * std::sin(get_radians(latitude));
        h = get_degrees(std::asin(h));

        sunlight_t angle = {
            time    : time,
            angle   : h > 0 ? h : 0
        };
        return angle;
    }

}}


/**
* This file is part of Intrinsic3D.
*
* Copyright (c) 2019, NVIDIA CORPORATION. All rights reserved.
* Copyright (c) 2019, Technical University of Munich. 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 NVIDIA CORPORATION 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 "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.
*/

#include <nv/mesh/util.h>

#include <iostream>

#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_comparison.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>

namespace nv
{
namespace MeshUtil
{

    void removeLooseComponents(Mesh* mesh)
    {
        const std::vector<Vec3f> &verts = mesh->vertices;
        std::vector<Vec3i> &face_indices = mesh->face_vertices;
        size_t num_faces = face_indices.size();

        // collapse vertices that have the same 3D location and store their connected faces
        typedef boost::tuple<float, float, float> vec3;
        std::map<vec3, std::vector<size_t> > verts_collapsed;
        for (size_t i = 0; i < num_faces; ++i)
        {
            for (size_t j = 0; j < 3; ++j)
            {
                Vec3f v = verts[face_indices[i][j]];
                vec3 p = boost::tuples::make_tuple(v[0], v[1], v[2]);
                verts_collapsed[p].push_back(i);
            }
        }

        // build graph
        typedef boost::adjacency_list <boost::vecS, boost::vecS, boost::undirectedS> Graph;
        Graph G;
        for (std::map<vec3, std::vector<size_t> >::iterator itr = verts_collapsed.begin(); itr != verts_collapsed.end(); ++itr)
        {
            std::vector<size_t> &face_list = itr->second;
            for (size_t j = 0; j < face_list.size(); ++j)
            {
                boost::add_edge(face_list[j], face_list[(j + 1) % face_list.size()], G);
            }
        }

        // compute connected components
        std::vector<int> components(boost::num_vertices(G));
        int num_components = boost::connected_components(G, &components[0]);
        std::vector<int> component_sizes;
        component_sizes.resize(num_components, 0);
        //std::cout << "Total number of components: " << numComponents << std::endl;
        for (size_t i = 0; i != components.size(); ++i)
            component_sizes[components[i]]++;
        size_t largest = std::max_element(component_sizes.begin(), component_sizes.end()) - component_sizes.begin();
        //std::cout << "largest is " << largest << " with " << component_sizes[largest] << " out of " << num_faces << " triangles" << std::endl;

        // store old face indices and clear output
        std::vector<Vec3i> face_indices_old(face_indices);
        face_indices.clear();

        // only add faces from largest component to new output faces
        for (int i = 0; i < num_faces; ++i)
            if (components[i] == largest)
                face_indices.push_back(face_indices_old[i]);
        //std::cout << "triangles before: " << face_indices_old.size() << "; triangles after: " << face_indices.size() << std::endl;

        // remove unused vertices
        removeUnusedVertices(mesh);
    }

    void removeUnusedVertices(Mesh* mesh)
    {
        std::vector<Vec3f> &verts = mesh->vertices;
        std::vector<Vec3b> &colors = mesh->colors;
        std::vector<Vec3i> &face_indices = mesh->face_vertices;
        bool has_colors = !colors.empty();
        //std::cout << "Vertices before removing unused: " << verts.size() << std::endl;

        // detect used vertices
        std::vector<bool> verts_used;
        verts_used.resize(verts.size(), false);
        for (size_t i = 0; i < face_indices.size(); i++)
        {
            for (size_t t = 0; t < 3; ++t)
            {
                unsigned int v_idx = static_cast<unsigned int>(face_indices[i][t]);
                verts_used[v_idx] = true;
            }
        }

        // count unused vertices
        size_t num_unused = 0;
        for (size_t i = 0; i < verts_used.size(); ++i)
            if (!verts_used[i])
                ++num_unused;
        //std::cout << "Unused vertices: " << num_unused << std::endl;
        if (num_unused == 0)
            return;

        // remove unused vertices
        std::vector<Vec3f> verts_new;
        std::vector<Vec3b> colors_new;
        unsigned int idx = 0;
        std::vector<unsigned int> vert_indices;
        vert_indices.resize(verts.size());
        for (size_t i = 0; i < verts.size(); ++i)
        {
            if (verts_used[i])
            {
                verts_new.push_back(verts[i]);
                if (has_colors)
                    colors_new.push_back(colors[i]);
                vert_indices[i] = idx;
                ++idx;
            }
        }
        // set updated vertices
        mesh->vertices.clear();
        mesh->colors.clear();
        for (size_t i = 0; i < verts_new.size(); ++i)
        {
            mesh->vertices.push_back(verts_new[i]);
            if (has_colors)
                mesh->colors.push_back(colors_new[i]);
        }

        // update face indices (indices may have shifted)
        for (size_t i = 0; i < face_indices.size(); ++i)
        {
            for (size_t t = 0; t < 3; ++t)
            {
                unsigned int vIdx = static_cast<unsigned int>(face_indices[i][t]);
                face_indices[i][t] = vert_indices[vIdx];
            }
        }

        //std::cout << "Vertices after removing unused: " << mesh->vertices.size() << std::endl;
    }

    bool removeDegenerateFaces(Mesh* mesh)
    {
        if (!mesh)
            return false;

        // remove degenerate faces
        const std::vector<Vec3f> &verts = mesh->vertices;
        std::vector<Vec3i> faces = mesh->face_vertices;
        mesh->face_vertices.clear();
        for (size_t i = 0; i < faces.size(); ++i)
        {
            int v0 = faces[i][0];
            int v1 = faces[i][1];
            int v2 = faces[i][2];
            // check if two vertices have the same indices
            if ((v0 == v1) || (v0 == v2) || (v1 == v2))
                continue;
            // check if triangle area is zero
            Vec3f e0 = verts[v2] - verts[v0];
            Vec3f e1 = verts[v2] - verts[v1];
            double area = static_cast<double>((e0.cross(e1)).norm());
            if (area == 0.0 || std::isnan(area) || std::isinf(area))
                continue;
            mesh->face_vertices.push_back(Vec3i(v0, v1, v2));
        }
        return true;
    }

} // namespace MeshUtil
} // namespace nv