#pragma once
#define exprtk_disable_string_capabilities
#include "keyboard_optimizer/concepts.hpp"
#include "keyboard_optimizer/layout.hpp"
#include "keyboard_optimizer/utils.hpp"
#include <exprtk/exprtk.hpp>
#include <boost/container/small_vector.hpp>
#include <string>
#include <tbb/enumerable_thread_specific.h>

namespace KeyboardOptimizer {

using PressEffortMap = PressMap<Effort>;

struct StandartEffortModelGrade final {
  double Linear = 0.0;
  double FingerPair1 = 0.0;
  double Horizontal1 = 0.0;
  double Vertical1 = 0.0;
  double FingerPair2 = 0.0;
  double Horizontal2 = 0.0;
  double Vertical2 = 0.0;
  double HandsRhythm = 0.0;
  double Path = 0.0;
  double Result = 0.0;
  constexpr operator Effort() const noexcept { return Result; }
};

class StandartEffortModelGradeFormula final {
  using SymbolTable = exprtk::symbol_table<Effort>;
  using Expression = exprtk::expression<Effort>;
  using Parser = exprtk::parser<Effort>;
  using Error = MathParserError::Error;
  using Grade = StandartEffortModelGrade;
  using GradePtr = std::unique_ptr<Grade>;

  std::string expressionStr;
  GradePtr gradePtr;
  SymbolTable symbolTable;
  Expression expression;

public:
  StandartEffortModelGradeFormula(std::string &&expression_)
      : expressionStr{std::move(expression_)},
        gradePtr{std::make_unique<Grade>()}, symbolTable{InitSymbolTable(
                                                 *gradePtr)},
        expression{InitExpression(expressionStr, symbolTable)} {}
  StandartEffortModelGradeFormula(StandartEffortModelGradeFormula const &other)
      : expressionStr{other.expressionStr}, gradePtr{std::make_unique<Grade>()},
        symbolTable{InitSymbolTable(*gradePtr)},
        expression{InitExpression(expressionStr, symbolTable)} {}
  StandartEffortModelGradeFormula(
      StandartEffortModelGradeFormula &&other) noexcept {
    swap(other);
  }
  StandartEffortModelGradeFormula &
  operator=(StandartEffortModelGradeFormula const &other) & {
    auto this_ = StandartEffortModelGradeFormula(other);
    swap(this_);
    return *this;
  }
  StandartEffortModelGradeFormula &
      operator=(StandartEffortModelGradeFormula &&other) &
      noexcept {
    swap(other);
  }
  ~StandartEffortModelGradeFormula() = default;

  void Evaluate(Grade &grade_) {
    *gradePtr = grade_;
    grade_.Result = expression.value();
  }

  void swap(StandartEffortModelGradeFormula &other) noexcept {
    using std::swap;
    swap(expressionStr, other.expressionStr);
    swap(gradePtr, other.gradePtr);
    swap(symbolTable, other.symbolTable);
    swap(expression, other.expression);
  }

private:
  static SymbolTable InitSymbolTable(Grade &grade) {
    auto symbolTable = SymbolTable{};
    symbolTable.add_constants();
    symbolTable.add_variable("Linear", grade.Linear);
    symbolTable.add_variable("FingerPair1", grade.FingerPair1);
    symbolTable.add_variable("Horizontal1", grade.Horizontal1);
    symbolTable.add_variable("Vertical1", grade.Vertical1);
    symbolTable.add_variable("FingerPair2", grade.FingerPair2);
    symbolTable.add_variable("Horizontal2", grade.Horizontal2);
    symbolTable.add_variable("Vertical2", grade.Vertical2);
    symbolTable.add_variable("HandsRhythm", grade.HandsRhythm);
    symbolTable.add_variable("Path", grade.Path);
    return symbolTable;
  }

  static Expression InitExpression(std::string const &expressionStr,
                                   SymbolTable &symbolTable) {
    auto expression = Expression{};
    expression.register_symbol_table(symbolTable);
    auto parser = Parser{};
    if (!parser.compile(expressionStr, expression))
      throw MathParserError(parser);
    return expression;
  }
};

void swap(StandartEffortModelGradeFormula &left,
          StandartEffortModelGradeFormula &right) noexcept {
  left.swap(right);
}

struct StandartEffortModelParameters final {
  std::string gradeExpression = "Path";
};

class StandartEffortModel final {
  using GradeCombinerTLS =
      tbb::enumerable_thread_specific<StandartEffortModelGradeFormula>;

public:
  using Grade = StandartEffortModelGrade;

private:
  LayersMap<Modifier> modifiers;
  PressEffortMap pressEffortMap;
  PressFingerMap pressFingerMap;
  FingerPositions initialFingerPositions;
  StandartEffortModelParameters parameters;
  GradeCombinerTLS gradeCombiner;

public:
  StandartEffortModel(LayersMap<Modifier> &&modifiers,
                      PressFingerMap &&pressFingerMap,
                      PressEffortMap &&pressEffortMap,
                      FingerPositions &&initialFingerPositions,
                      StandartEffortModelParameters &&parameters =
                          StandartEffortModelParameters{})
      : modifiers{std::move(modifiers)},
        pressFingerMap{std::move(pressFingerMap)}, pressEffortMap{std::move(
                                                       pressEffortMap)},
        initialFingerPositions{std::move(initialFingerPositions)},
        parameters{std::move(parameters)}, gradeCombiner{
                                               GetGradeCombinerGenerator()} {
    // Check that provided expression is well-formed
    gradeCombiner.local();
  }

  // Thread-safe
  Grade Evaluate(Layout const &layout, SymStringViewConst text) {
    assert(text.size() > 10);
    auto fingerPositions = initialFingerPositions;
    auto textSize = text.size();
    auto p0 = layout[text[0]];
    auto p1 = layout[text[1]];
    auto p2 = layout[text[2]];

    auto grade = Grade{};

    for (auto symIt = text.begin() + 3, e = text.end(); symIt != e; ++symIt) {
      // clang-format off
      grade.Linear      += EvaluateLinear(p0);
      grade.FingerPair1 += EvaluateFingerPair(p0, p1);
      grade.Horizontal1 += EvaluateHorizontal(p0, p1);
      grade.Vertical1   += EvaluateVertical(p0, p1);
      grade.FingerPair2 += EvaluateFingerPair(p0, p2);
      grade.Horizontal2 += EvaluateHorizontal(p0, p2);
      grade.Vertical2   += EvaluateVertical(p0, p2);
      grade.HandsRhythm += EvaluateHandsRhythm(p0, p1);
      grade.Path        += EvaluatePath(p0, fingerPositions);
      // clang-format on
      UpdateFingerPositions(p2, fingerPositions);

      p0 = p1;
      p1 = p2;
      p2 = layout[*symIt];
    }

    // clang-format off
    grade.Linear      /= textSize;
    grade.FingerPair1 /= textSize;
    grade.Horizontal1 /= textSize;
    grade.Vertical1   /= textSize;
    grade.FingerPair2 /= textSize;
    grade.Horizontal2 /= textSize;
    grade.Vertical2   /= textSize;
    grade.HandsRhythm /= textSize;
    grade.Path        /= textSize;
    // clang-format on
    auto &&combiner = gradeCombiner.local();
    combiner.Evaluate(grade);
    return grade;
  }

private:
  double KeyDistance(Key key0, Key key1) const {
    return std::hypot(key0.row - key1.row, key0.x - key1.x);
  }
  double EvaluateLinear(Press const &p) const {
    auto effort = pressEffortMap.at(p);
    for (auto const &modifierPress : modifiers[p.layer])
      effort += pressEffortMap.at(modifierPress);
    return effort;
  }
  double EvaluateFingerPair(Press const &p1, Press const &p2) const {
    auto const &f1 = pressFingerMap.at(p1);
    auto const &f2 = pressFingerMap.at(p2);
    if (f1.hand != f2.hand)
      return 0.0;
    // Using same finger twice for different keys
    // implies a penalty
    if (f1.type == f2.type && !(p1.key == p2.key))
      return (pressEffortMap.at(p1) + pressEffortMap.at(p2)) / 2;
    return 0.0;
  }
  double EvaluateHorizontal(Press const &p1, Press const &p2) const {
    auto const &f1 = pressFingerMap.at(p1);
    auto const &f2 = pressFingerMap.at(p2);
    auto penalty = 0.0;
    if (f1.hand != f2.hand)
      return penalty;
    // Movement outwards implies a penalty
    if ((f1.hand == Hand::Right && p1.key.x < p2.key.x) ||
        (f1.hand == Hand::Left && p1.key.x > p2.key.x))
      penalty += (pressEffortMap.at(p1) + pressEffortMap.at(p2)) / 2;
    return penalty;
  }
  double EvaluateVertical(Press const &p1, Press const &p2) const {
    auto const &f1 = pressFingerMap.at(p1);
    auto const &f2 = pressFingerMap.at(p2);
    auto penalty = 0.0;
    if (f1.hand != f2.hand)
      return penalty;
    penalty += abs(p1.key.row - p2.key.row) *
               (pressEffortMap.at(p1) + pressEffortMap.at(p2)) / 2;
    return penalty;
  }
  double EvaluatePath(Press const &press,
                      FingerPositions const &fingerPositions) const {
    auto path = 0.0;
    auto curPos = fingerPositions[GetFingerIndex(pressFingerMap.at(press))];
    path += KeyDistance(curPos, press.key);
    for (auto const &modPress : modifiers[press.layer]) {
      auto curPos =
          fingerPositions[GetFingerIndex(pressFingerMap.at(modPress))];
      path += KeyDistance(curPos, modPress.key);
    }
    return path;
  }
  double EvaluateHandsRhythm(Press const &p1, Press const &p2) const {
    auto const &f1 = pressFingerMap.at(p1);
    auto const &f2 = pressFingerMap.at(p2);
    // Using same hand implies a penalty
    return f1.hand == f2.hand;
  }
  void UpdateFingerPositions(Press const &press,
                             FingerPositions &fingerPositions) const {
    auto finger = pressFingerMap.at(press);
    fingerPositions[GetFingerIndex(finger)] = press.key;
    for (auto const &modPress : modifiers[press.layer])
      fingerPositions[GetFingerIndex(pressFingerMap.at(modPress))] =
          modPress.key;
  }

  constexpr size_t GetFingerIndex(Finger finger) const {
    auto index =
        static_cast<size_t>(finger.hand) *
            static_cast<size_t>(KeyboardOptimizer::FingerType::NTypes) +
        static_cast<size_t>(finger.type);
    assert(index >= 0);
    assert(index < NFingers);
    return index;
  }

  std::function<StandartEffortModelGradeFormula()>
  GetGradeCombinerGenerator() const {
    return [expr = parameters.gradeExpression]() {
      return StandartEffortModelGradeFormula(std::string{expr});
    };
  }
};

} // namespace KeyboardOptimizer


 /**
 * @file
 *
 *   Unless noted otherwise, the portions of Isis written by the USGS are public
 *   domain. See individual third-party library and package descriptions for
 *   intellectual property information,user agreements, and related information.
 *
 *   Although Isis has been used by the USGS, no warranty, expressed or implied,
 *   is made by the USGS as to the accuracy and functioning of such software
 *   and related material nor shall the fact of distribution constitute any such
 *   warranty, and no responsibility is assumed by the USGS in connection
 *   therewith.
 *
 *   For additional information, launch
 *   $ISISROOT/doc//documents/Disclaimers/Disclaimers.html in a browser or see
 *   the Privacy & Disclaimers page on the Isis website,
 *   http://isis.astrogeology.usgs.gov, and the USGS privacy and disclaimers on
 *   http://www.usgs.gov/privacy.html.
 */
#include <cmath>
#include <iostream>

#include <boost/math/special_functions/legendre.hpp>

#include "Camera.h"
#include "CameraFocalPlaneMap.h"
#include "Constants.h"
#include "FunctionTools.h"
#include "IString.h"
#include "LineScanCameraDetectorMap.h"
#include "NewHorizonsMvicTdiCameraDistortionMap.h"

#include <QDebug>

using namespace boost::math;
using namespace std;
using namespace Isis;

namespace Isis {
  /** Camera distortion map constructor
   *
   * Create a camera distortion map.  This class maps between distorted
   * and undistorted focal plane x/y's.  The default mapping is the
   * identity, that is, the focal plane x/y and undistorted focal plane
   * x/y will be identical.
   *
   * @param parent                 the parent camera that will use this distortion map
   * @param zDirection             the direction of the focal plane Z-axis
   *                               (either 1 or -1)
   *
   * @param xDistortionCoeffs      distortion coefficients in x
   * @param yDistortionCoeffs      distortion coefficients in y
   * @param residualColDistCoeffs
   * @param residualRowDistCoeffs
   *
   */
  NewHorizonsMvicTdiCameraDistortionMap::NewHorizonsMvicTdiCameraDistortionMap(
      Camera *parent,
      vector<double> xDistortionCoeffs,
      vector<double> yDistortionCoeffs,
      vector<double> residualColDistCoeffs,
      vector<double> residualRowDistCoeffs) :
    CameraDistortionMap(parent, 1.0) {

    m_xDistortionCoeffs = xDistortionCoeffs;
    m_yDistortionCoeffs = yDistortionCoeffs;

    m_residualColDistCoeffs = residualColDistCoeffs;
    m_residualRowDistCoeffs = residualRowDistCoeffs;

    // half of detector in x is 32.5 mm
    m_focalPlaneHalf_x = 0.5 * p_camera->Samples() * p_camera->PixelPitch();
  }

  /** Destructor
   */
  NewHorizonsMvicTdiCameraDistortionMap::~NewHorizonsMvicTdiCameraDistortionMap() {
  }

  /** Compute undistorted focal plane x/y
   *
   * Compute undistorted focal plane x given a distorted focal plane x
   *
   * TODO: Modify this - Distortion in line direction currently not considered (MVIC TDI is treated as line scan sensor)
   *
   * In the event of any failure, undistorted focal plane values are set equal to the raw
   * (distorted) values
   *
   * @param dx distorted focal plane x in millimeters
   * @param dy distorted focal plane y in millimeters
   *
   * @return success/failure
   *
   * @see SetDistortion
   */
  // TODO: Don't really like this - we always return true, even in event of failures
  bool NewHorizonsMvicTdiCameraDistortionMap::SetFocalPlane(const double dx, const double dy) {

    p_focalPlaneX = dx;
    p_focalPlaneY = dy;

    // initialize undistorted focal plane coordinates to the distorted
    // coordinate values
    p_undistortedFocalPlaneX = dx;
    p_undistortedFocalPlaneY = dy;

    // if x lies outside of the detector, do NOT apply distortion
    // set undistorted focal plane values to be identical to raw values
    if ((fabs(dx) > m_focalPlaneHalf_x)) {
      return true;
    }

    // scale x to lie in the range -1.0 to +1.0
    // this is required for Legendre Polynomials, man
    double xscaled = -dx / m_focalPlaneHalf_x;

    // compute distortion corrections in x using Legendre Polynomials
    // these corrections are also in the -1.0 to +1.0 range
    double deltax1 = 0.0;
    if (!computeDistortionCorrections(xscaled, 0.0, deltax1)) {
      return true;
    }

    // now compute residual distortion corrections (per Jason Cook)
    // TODO: implementation not complete
    double deltax2 = 0.0;
    double deltay2 = 0.0;
    computeResidualDistortionCorrections(dx, deltax2, deltay2);

    // apply the corrections
    xscaled += deltax1;

    // scale back from range of '-1.0 to +1.0' to the detector, '-32.5 to 32.5 mm'
//    p_undistortedFocalPlaneX = -xscaled * m_focalPlaneHalf_x;
//    p_undistortedFocalPlaneY = p_focalPlaneY;
    p_undistortedFocalPlaneX = -xscaled * m_focalPlaneHalf_x + deltax2;
    p_undistortedFocalPlaneY = p_focalPlaneY + deltay2;

    return true;
  }

  /** Compute distorted focal plane x/y
   *
   * Compute distorted focal plane x/y given an undistorted focal plane x/y.
   *
   * This is an iterative procedure as computing the inverse of the distortion equations used by New
   * Horizons MVIC is difficult.
   *
   * @param ux undistorted focal plane x in millimeters
   * @param uy undistorted focal plane y in millimeters
   *
   * @return if the conversion was successful
   * @see SetDistortion
   */
  bool NewHorizonsMvicTdiCameraDistortionMap::SetUndistortedFocalPlane(const double ux, const double uy) {

    // image coordinates prior to introducing distortion
    p_undistortedFocalPlaneX = ux;
    p_undistortedFocalPlaneY = uy;

    double xt = ux;
    double yt = uy;

//    p_focalPlaneX = p_undistortedFocalPlaneX;
//    p_focalPlaneY = p_undistortedFocalPlaneY;
//    return true;

    // scale undistorted coordinates to range of -1.0 to +1.0
    double uxScaled = -ux/m_focalPlaneHalf_x;
    double xtScaled;
    double xprevious, yprevious;
    double scaledDeltax;
    double deltax2 = 0.0;
    double deltay2 = 0.0;

    xprevious = 1000000.0;
    yprevious = 1000000.0;

    double tolerance = 0.000001;

    bool bConverged = false;

    // iterating to introduce distortion...
    // we stop when the difference between distorted coordinates
    // in successive iterations is at or below the given tolerance
    for ( int i = 0; i < 50; i++ ) {

      xtScaled = -xt / m_focalPlaneHalf_x;

      // Changed this failure mode from a throw to return false.  These functions should never
      // throw, the callers are not catching, so bad things happen.  The callers should be checking
      // the return status
      if (fabs(xtScaled) > 1.0) {
        return bConverged;
      }

      // compute scaled distortion in x (scaled to -1.0 - +1.0) using Legendre Polynomials
      computeDistortionCorrections(xtScaled, 0.0, scaledDeltax);

      // compute residual distortion in unscaled focal plane coordinates
      computeResidualDistortionCorrections(xt, deltax2, deltay2);

      // update unscaled coordinates
      xt = -(uxScaled-scaledDeltax) * m_focalPlaneHalf_x - deltax2;
      yt = uy - deltay2;

      // check for convergenceyScaledDistortion
      if ((fabs(xt - xprevious) <= tolerance) && (fabs(yt - yprevious) <= tolerance)) {
        bConverged = true;
        break;
      }

      xprevious = xt;
      yprevious = yt;
    }

    if (bConverged) {
      // scale x coord inate back to detector (-32.5 to +32.5)
      // xtScaled *= -m_focalPlaneHalf_x;

      // set distorted coordinates
      p_focalPlaneX = xt;
      p_focalPlaneY = yt;
    }

    return bConverged;
  }

  /** Compute distortion corrections in x and y direction
   *
   * For Legendre Polynomials, see ...
   *
   * http://mathworld.wolfram.com/LegendrePolynomial.html
   * http://www.boost.org/doc/libs/1_36_0/libs/math/doc/sf_and_dist/html/math_toolkit/special/sf_poly/legendre.html
   *
   * @param xscaled focal plane x scaled to range of 1- to 1 for Legendre Polynomials
   * @param yscaled focal plane y scaled to range of 1- to 1 for Legendre Polynomials
   * @param deltax focal plane distortion correction to x in millimeters
   * @param deltay focal plane distortion correction to y in millimeters
   *
   * @return success/failure
   */
  bool NewHorizonsMvicTdiCameraDistortionMap::computeDistortionCorrections(
      const double xscaled,
      const double yscaled,
      double &deltax) {

    double lpx0, lpx1, lpx2, lpx3, lpx4, lpx5;
    double lpy0, lpy1, lpy2, lpy3, lpy4, lpy5;

    // Legendre polynomials
    try {
      lpx0 = legendre_p(0,xscaled);
      lpx1 = legendre_p(1,xscaled);
      lpx2 = legendre_p(2,xscaled);
      lpx3 = legendre_p(3,xscaled);
      lpx4 = legendre_p(4,xscaled);
      lpx5 = legendre_p(5,xscaled);
      lpy0 = legendre_p(0,yscaled);
      lpy1 = legendre_p(1,yscaled);
      lpy2 = legendre_p(2,yscaled);
      lpy3 = legendre_p(3,yscaled);
      lpy4 = legendre_p(4,yscaled);
      lpy5 = legendre_p(5,yscaled);
    }
    catch (const std::exception& e) {
      return false;
    }

    deltax =
       m_xDistortionCoeffs[0] * lpx0  * lpy1 +
       m_xDistortionCoeffs[1] * lpx1 * lpy0 +
       m_xDistortionCoeffs[2] * lpx0 * lpy2 +
       m_xDistortionCoeffs[3] * lpx1 * lpy1 +
       m_xDistortionCoeffs[4] * lpx2 * lpy0 +
       m_xDistortionCoeffs[5] * lpx0 * lpy3 +
       m_xDistortionCoeffs[6] * lpx1 * lpy2 +
       m_xDistortionCoeffs[7] * lpx2 * lpy1 +
       m_xDistortionCoeffs[8] * lpx3 * lpy0 +
       m_xDistortionCoeffs[9] * lpx0 * lpy4 +
      m_xDistortionCoeffs[10] * lpx1 * lpy3 +
      m_xDistortionCoeffs[11] * lpx2 * lpy2 +
      m_xDistortionCoeffs[12] * lpx3 * lpy1 +
      m_xDistortionCoeffs[13] * lpx4 * lpy0 +
      m_xDistortionCoeffs[14] * lpx0 * lpy5 +
      m_xDistortionCoeffs[15] * lpx1 * lpy4 +
      m_xDistortionCoeffs[16] * lpx2 * lpy3 +
      m_xDistortionCoeffs[17] * lpx3 * lpy2 +
      m_xDistortionCoeffs[18] * lpx4 * lpy1 +
      m_xDistortionCoeffs[19] * lpx5 * lpy0;

    return true;
  }

  /** Compute residual distortion corrections in row and column direction
   *  TODO: Implementati plete
   *
   * @param dx
   * @param dy
   * @param residualDeltax
   * @param residualDeltay
   *
   * @return success/failure
   */
  void NewHorizonsMvicTdiCameraDistortionMap::computeResidualDistortionCorrections(const double dx,
                                                             double &residualDeltax,
                                                             double &residualDeltay) {

    double s = 2500.5 - dx/0.013;

    residualDeltax = s * m_residualColDistCoeffs[1] +
                       pow(s,2) * m_residualColDistCoeffs[2] +
                       pow(s,3) * m_residualColDistCoeffs[3] +
                       pow(s,4) * m_residualColDistCoeffs[4] +
                       pow(s,5) * m_residualColDistCoeffs[5];

    residualDeltay = s * m_residualRowDistCoeffs[1] +
                       pow(s,2) * m_residualRowDistCoeffs[2] +
                       pow(s,3) * m_residualRowDistCoeffs[3] +
                       pow(s,4) * m_residualRowDistCoeffs[4] +
                       pow(s,5) * m_residualRowDistCoeffs[5];

    // convert to mm (correction for negative x to the left)
    residualDeltax *= -p_camera->PixelPitch();
    residualDeltay *= p_camera->PixelPitch();
  }

///**
// * Testing method to output corrections in x and y at pixel centers for entire focal plane.
// * Output in csv format for viewing/plotting in Excel.
// */
//bool NewHorizonsMvicTdiCameraDistortionMap::outputResidualDeltas() {
//
//  QString ofname("mvic_tdi_residual_deltas.csv");
//  std::ofstream fp_out(ofname.toLatin1().data(), std::ios::out);
//  if (!fp_out)
//    return false;
//
//  char buf[1056];
//
//  double residualColDelta;
//
//  for (int s=0; s <= 5000; s++) {  // loop in sample direction
//    residualColDelta =            m_residualColDistCoeffs[0] +
//                              s * m_residualColDistCoeffs[1] +
//                       pow(double(s),2) * m_residualColDistCoeffs[2] +
//                       pow(double(s),3) * m_residualColDistCoeffs[3] +
//                       pow(double(s),4) * m_residualColDistCoeffs[4] +
//                       pow(double(s),5) * m_residualColDistCoeffs[5];
//    sprintf(buf, "%d,%lf\n", s, residualColDelta);
//    fp_out << buf;
//  }
//
//  fp_out.close();
//
//  return true;
//}

}


#pragma once

#include <boost/python.hpp>
#include <boost/python/numpy.hpp>

#include "corridor/basic_types.h"
#include "corridor/corridor.h"
#include "corridor/corridor_assignment/corridor_assignment.h"
#include "corridor/corridor_assignment/corridor_related_semantics.h"
#include "corridor/internal/math.h"

namespace py = boost::python;

// /////////////////////////////////////////////////////////////////////////////
// Utilities
// /////////////////////////////////////////////////////////////////////////////

struct FlatCorridorRelatedFeatures {
  corridor::RealType l;         //< arc-length of the object (mutable)
  corridor::RealType d;         //< deviation from the reference line (mutable)
  corridor::RealType sigma_l;   //< standard deviation of the arc-length
  corridor::RealType sigma_d;   //< standard deviation of the deviation
  corridor::RealType vl;        //< arc-length of the object (mutable)
  corridor::RealType vd;        //< deviation from the reference line (mutable)
  corridor::RealType sigma_vl;  //< standard deviation of the arc-length
  corridor::RealType sigma_vd;  //< standard deviation of the deviation
  corridor::RealType obj_width_ratio;   //< from the absolute corridor width
  corridor::RealType obj_length_ratio;  //< from the absolute corridor length

  // Corridor features
  corridor::RealType corridor_width;   //< at projection point
  corridor::RealType corridor_length;  //< from start to end
};

corridor::CorridorRelatedFeatures From(
    const FlatCorridorRelatedFeatures& flat_features) {
  using namespace corridor;
  CorridorRelatedFeatures features;

  const FrenetStateVector2D state_vector(flat_features.l, flat_features.d,
                                         flat_features.vl, flat_features.vd);
  const FrenetStateCovarianceMatrix2D state_cov_mat(
      flat_features.sigma_l * flat_features.sigma_l,
      flat_features.sigma_d * flat_features.sigma_d,
      flat_features.sigma_vl * flat_features.sigma_vl,
      flat_features.sigma_vd * flat_features.sigma_vd);

  features.frenet_state = FrenetState2D(InvalidId, state_vector, state_cov_mat);

  features.corridor_width = flat_features.corridor_width;
  features.corridor_length = flat_features.corridor_length;
  features.corridor_center_offset = 0.0;

  features.frenet_obb = OrientedBoundingBox(
      0.0, flat_features.corridor_length * flat_features.obj_length_ratio,
      flat_features.corridor_width * flat_features.obj_width_ratio);

  return features;
};

corridor::RealType LateralAssignmentConfidence(
    const FlatCorridorRelatedFeatures& flat_features) {
  return corridor::LateralAssignmentConfidence(From(flat_features));
};

corridor::RealType LongitudinalAssignmentConfidence(
    const FlatCorridorRelatedFeatures& flat_features) {
  return corridor::LongitudinalAssignmentConfidence(From(flat_features));
};

corridor::RealType evaluateIntegralLineWidthGaussian(
    const corridor::RealType m, const corridor::RealType b,
    const corridor::RealType x, const corridor::RealType sigma_original,
    const corridor::RealType lower_bound,
    const corridor::RealType upper_bound) {
  return corridor::math::evaluateIntegralLineWidthGaussian(
      m, b, x, sigma_original, lower_bound, upper_bound);
};

corridor::RealType MovingConfidence(const corridor::RealType abs_velocity,
                                    const corridor::RealType std_velocity,
                                    const corridor::RealType sigma_band) {
  return corridor::MovingConfidence({abs_velocity, std_velocity}, sigma_band);
}

py::dict RelativeDirectionConfidence(const corridor::RealType heading_mean,
                                     const corridor::RealType heading_std,
                                     const corridor::RealType sigma_band) {
  using namespace corridor;

  py::dict result;
  result["downstream"] = RelativeOrientationConfidence(
      0.0, {heading_mean, heading_std}, sigma_band);
  result["upstream"] = RelativeOrientationConfidence(
      M_PI, {heading_mean, heading_std}, sigma_band);
  result["towards_left"] = RelativeOrientationConfidence(
      M_PI_2, {heading_mean, heading_std}, sigma_band);
  result["towards_right"] = RelativeOrientationConfidence(
      -M_PI_2, {heading_mean, heading_std}, sigma_band);

  return result;
}


#define STATES 16   // Number of states of Kalman Filter
#define INPUTS 6    // Number of inputs of Kalman Filter
#define MSRMTS 10   // Number of measurements of Kalman Filter

#include <ros/ros.h>
#include <sstream>
#include <chrono>

#include <std_msgs/String.h>
#include <tf/LinearMath/Quaternion.h>
#include <tf/transform_broadcaster.h>
#include <tf/LinearMath/Vector3.h>
#include <geometry_msgs/TransformStamped.h>
#include <Eigen/Dense>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Vector3.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/TwistWithCovariance.h>
#include <nav_msgs/Odometry.h>

#include <cpp_test/NavFilter.h>

int main(int argc, char **argv){

  ros::init(argc, argv, "my_nav_filter");

  Eigen::Matrix<double,STATES,1> state_prediction;

  int node_freq = 10;   // Hz

  NavFilter filter;

  ROS_INFO("::: NAV. NODE INITIALISED");

  ros::Rate loop_rate(node_freq);

  ros::spin();

  /*while(ros::ok()){

    //state_prediction = filter.propagateFilter();
    //std::cout << state_prediction.transpose() << std::endl;
    //pubTFframe(state_prediction);
    //filter.publishState();

    ROS_INFO("Callbacks in last second - Position:%2d, Velocity:%2d, IMU:%3d", filter.posCallbacks, filter.velCallbacks, filter.imuCallbacks);
    filter.posCallbacks = 0;
    filter.velCallbacks = 0;
    filter.imuCallbacks = 0;

    ros::spinOnce();
    loop_rate.sleep();
  }
  */
  return 0;
}


/* Copyright © 2019 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */

#include <algorithm>
#include <cmath>
#include <limits>
#include <vector>
#include <random>

#include <boost/gil/gil_all.hpp>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <image/image_util_impl.hpp>

#include <unity/toolkits/object_detection/object_detector.hpp>
#include <unity/toolkits/object_detection/one_shot_object_detection/one_shot_object_detector.hpp>
#include <unity/toolkits/object_detection/one_shot_object_detection/util/color_convert.hpp>
#include <unity/toolkits/object_detection/one_shot_object_detection/util/mapping_function.hpp>
#include <unity/toolkits/object_detection/one_shot_object_detection/util/parameter_sampler.hpp>
#include <unity/toolkits/object_detection/one_shot_object_detection/util/quadrilateral_geometry.hpp>
#include <unity/toolkits/object_detection/one_shot_object_detection/util/superposition.hpp>

namespace turi {
namespace one_shot_object_detection {
namespace data_augmentation {

flex_dict build_annotation( ParameterSampler &parameter_sampler,
                            size_t object_width,
                            size_t object_height,
                            long seed) {

  parameter_sampler.sample(seed);

  size_t original_top_left_x = 0;
  size_t original_top_left_y = 0;
  size_t original_top_right_x = object_width;
  size_t original_top_right_y = 0;
  size_t original_bottom_left_x = 0;
  size_t original_bottom_left_y = object_height;
  size_t original_bottom_right_x = object_width;
  size_t original_bottom_right_y = object_height;

  Eigen::Vector3f top_left_corner(3)   , top_right_corner(3);
  Eigen::Vector3f bottom_left_corner(3), bottom_right_corner(3);
  top_left_corner     << original_top_left_x    , original_top_left_y    , 1;
  top_right_corner    << original_top_right_x   , original_top_right_y   , 1;
  bottom_left_corner  << original_bottom_left_x , original_bottom_left_y , 1;
  bottom_right_corner << original_bottom_right_x, original_bottom_right_y, 1;

  auto normalize = [](Eigen::Vector3f corner) {
    corner[0] /= corner[2];
    corner[1] /= corner[2];
    corner[2] = 1.0;
    return corner;
  };

  Eigen::Matrix<float, 3, 3> mat = parameter_sampler.get_transform();

  const std::vector<Eigen::Vector3f> warped_corners = {
                                          normalize(mat * top_left_corner)   ,
                                          normalize(mat * top_right_corner)  ,
                                          normalize(mat * bottom_left_corner),
                                          normalize(mat * bottom_right_corner)
                                         };
  parameter_sampler.set_warped_corners(warped_corners);

  float min_x = std::numeric_limits<float>::max();
  float max_x = std::numeric_limits<float>::min();
  float min_y = std::numeric_limits<float>::max();
  float max_y = std::numeric_limits<float>::min();
  for (auto corner: warped_corners) {
    min_x = std::min(min_x, corner[0]);
    max_x = std::max(max_x, corner[0]);
    min_y = std::min(min_y, corner[1]);
    max_y = std::max(max_y, corner[1]);
  }
  float center_x = (min_x + max_x) / 2;
  float center_y = (min_y + max_y) / 2;
  float bounding_box_width  = max_x - min_x;
  float bounding_box_height = max_y - min_y;

  flex_dict coordinates = {std::make_pair("x", center_x),
                           std::make_pair("y", center_y),
                           std::make_pair("width", bounding_box_width),
                           std::make_pair("height", bounding_box_height)
                          };
  flex_dict annotation = {std::make_pair("coordinates", coordinates),
                          std::make_pair("label", "placeholder")
                         };
  return annotation;
}

static std::map<std::string,size_t> generate_column_index_map(
    const std::vector<std::string>& column_names) {
    std::map<std::string,size_t> index_map;
    for (size_t k=0; k < column_names.size(); ++k) {
        index_map[column_names[k]] = k;
    }
    return index_map;
}

gl_sframe augment_data(const gl_sframe &data,
                       const std::string& image_column_name,
                       const std::string& target_column_name,
                       const gl_sarray &backgrounds,
                       long long seed) {
  auto column_index_map = generate_column_index_map(data.column_names());
  std::vector<flexible_type> annotations;
  std::vector<flexible_type> images;
  for (const auto& row: data.range_iterator()) {
    flex_image object = row[column_index_map[image_column_name]].to<flex_image>();
    std::string label = row[column_index_map[target_column_name]].to<flex_string>();
    if (!(object.is_decoded())) {
      decode_image_inplace(object);
    }
    int row_number = -1;
    for (const auto& background_ft: backgrounds.range_iterator()) {
      row_number++;
      flex_image flex_background = background_ft.to<flex_image>();
      if (!(flex_background.is_decoded())) {
        decode_image_inplace(flex_background);
      }
      size_t background_width = flex_background.m_width;
      size_t background_height = flex_background.m_height;
      size_t background_channels = flex_background.m_channels;

      ParameterSampler parameter_sampler = ParameterSampler(
                                              background_width,
                                              background_height,
                                              (background_width-object.m_width)/2,
                                              (background_height-object.m_height)/2);

      flex_dict annotation = build_annotation(parameter_sampler,
                                              object.m_width, object.m_height,
                                              seed+row_number);

      DASSERT_TRUE(flex_background.get_image_data() != nullptr);
      DASSERT_TRUE(object.is_decoded());
      DASSERT_TRUE(flex_background.is_decoded());

      boost::gil::rgb8_image_t::view_t background_view = interleaved_view(
        background_width,
        background_height,
        (boost::gil::rgb8_pixel_t*)(flex_background.get_image_data()),
        background_channels * background_width // row length in bytes
        );

      images.push_back(
        create_synthetic_image(background_view, parameter_sampler, object)
      );
      annotations.push_back(annotation);
    }
  }

  const std::map<std::string, std::vector<flexible_type> >& augmented_data = {
    {"annotation", annotations},
    {"image", images}
  };
  gl_sframe augmented_data_out = gl_sframe(augmented_data);
  return augmented_data_out;
}

} // data_augmentation

one_shot_object_detector::one_shot_object_detector() {
  model_.reset(new turi::object_detection::object_detector());
}

gl_sframe one_shot_object_detector::augment(const gl_sframe &data,
                                            const std::string& target_column_name,
                                            const gl_sarray &backgrounds,
                                            std::map<std::string, flexible_type> &options){

  // TODO: Automatically infer the image column name, or throw error if you can't
  // This should just happen on the Python side.
  std::string image_column_name = "image";
  gl_sframe augmented_data = data_augmentation::augment_data(data,
                                                             image_column_name,
                                                             target_column_name,
                                                             backgrounds,
                                                             options["seed"]);
  // TODO: Call object_detector::train from here once we incorporate mxnet into
  // the C++ Object Detector.
  return augmented_data;
}

} // one_shot_object_detection
} // turi


#include <Configuration.h> // Package Path
#include <avatar_locomanipulation/enable_pinocchio_with_hpp_fcl.h> // Enable HPP FCL
// Multibody
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/multibody/geometry.hpp"
// Parsers
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/srdf.hpp"
// Algorithms
#include "pinocchio/algorithm/geometry.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
// Spatial
#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
// Standard
#include <math.h>
#include <iostream>
#include <iomanip>
#include <vector>
#include <boost/shared_ptr.hpp>

pinocchio::fcl::Quaternion3f makeQuat(double w, double x, double y, double z){
  pinocchio::fcl::Quaternion3f q;
  q.w() = w;
  q.x() = x;
  q.y() = y;
  q.z() = z;
  return q;
}

int main(int argc, char ** argv){

//------------------- Load Robot and give initial configuration
  std::string filename = (argc<=1) ? THIS_PACKAGE_PATH"models/valkyrie_simplified.urdf" : argv[1];
  //const std::string srdf_filename = PINOCCHIO_SOURCE_DIR"/models/romeo/romeo_description/srdf/romeo.srdf";
  std::vector < std::string > packageDirs;
  std::string meshDir  = THIS_PACKAGE_PATH"../val_model/";
  packageDirs.push_back(meshDir);

  // build URDF model
  pinocchio::Model model;
  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);

  // build Geometry model
  pinocchio::GeometryModel geomModel;
  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, packageDirs );

  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq); // Config
  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); // Config Velocity
  Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv); // Config Acceleration

  // floating base joints: x, y, z
  q[0] = 0.0;  q[1] = 0.0;  q[2] = 0.0;

  // floating base quaternion: qx, qy, qz, qw
  double theta = 0;//M_PI/4.0;
  Eigen::AngleAxis<double> aa(theta, Eigen::Vector3d(0.0, 0.0, 1.0)); // yaw pi/4 to the left
  Eigen::Quaternion<double> quat_init; quat_init =  aa;

  q[3] = quat_init.x();// 0.0;
  q[4] = quat_init.y(); //0.0;
  q[5] = quat_init.z(); //sin(theta/2.0);
  q[6] = quat_init.w(); //cos(theta/2.0);

//------------------- End Load Robot and give initial configuration

//----------------------------- Add Box to GeometryModel and give initial configuration
  static double pi = M_PI;
  pinocchio::fcl::Transform3f tf1 (makeQuat (0, 0, 1, 0), pinocchio::fcl::Vec3f (0, 0, 0));
  pinocchio::SE3 placement = pinocchio::toPinocchioSE3(tf1);

  pinocchio::Model::JointIndex idx;
  idx = model.addJoint(model.getJointId("universe"),pinocchio::JointModelPlanar(),pinocchio::SE3::Identity(),"planar1_joint");
  model.addJointFrame(idx);
  model.appendBodyToJoint(idx,pinocchio::Inertia::Random(),pinocchio::SE3::Identity());
  model.addBodyFrame("planar1_body", idx, pinocchio::SE3::Identity());

  boost::shared_ptr<pinocchio::fcl::Box> sample(new pinocchio::fcl::Box(1, 1, 1));
  pinocchio::Model::FrameIndex box_id = model.getBodyId("planar1_body");
  pinocchio::Model::JointIndex box_parent = model.frames[box_id].parent;

  pinocchio::GeometryModel::GeomIndex box_geom_id = geomModel.addGeometryObject(pinocchio::GeometryObject("box",
                                                                           model.getBodyId("planar1_body"),box_parent,
                                                                           sample,placement, "", Eigen::Vector3d::Ones())
                                                            );
  geomModel.geometryObjects[box_geom_id].parentJoint = model.frames[box_id].parent;

  Eigen::VectorXd p(model.nq);
  p <<  q,
  		0, 0, 1, 0;

  std::cout << "q.transpose" << q.transpose() << std::endl;
  std::cout << "p.transpose" << p.transpose() << std::endl;

//----------------------------- End Add Box to GeometryModel and give initial configuration

  geomModel.addAllCollisionPairs();
  pinocchio::Data data(model);
  pinocchio::GeometryData geomData(geomModel);
  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, p);

  int j, i, k;
  pinocchio::fcl::CollisionResult result;
  pinocchio::fcl::DistanceResult dresult;
  std::vector<pinocchio::fcl::Contact> contacts;

  pinocchio::computeCollisions(model,data,geomModel,geomData,p);

  for(j=0; j<geomData.collisionResults.size(); j++)
  {
  	result = geomData.collisionResults[j];
  	pinocchio::CollisionPair idx = geomModel.collisionPairs[j];
  	std::cout << geomModel.getGeometryName(idx.first) << std::endl;
  	result.getContacts(contacts);
      	if(contacts.size() != 0)
      	{
      		for(k=0; k<contacts.size(); k++)
      		{
      			std::cout << "Contact Found Between: " << geomModel.getGeometryName(idx.first) << " and " << geomModel.getGeometryName(idx.second) << std::endl;
        		std::cout << "position: " << contacts[k].pos << std::endl;
        		std::cout << "-------------------" << std::endl;
      		}
      	}
      	// else
      	// {
      	// 	pinocchio::computeDistance(geomModel, geomData, geomModel.findCollisionPair(idx));
      	// 	dresult = geomData.distanceResults[j];
      	// 	std::cout << "Minimum Distance Between: " << geomModel.getGeometryName(idx.first) << " and " << geomModel.getGeometryName(idx.second) << " = " << dresult.min_distance << std::endl;
      	// }
  }

//   for(i=0; i<geomModel.ngeoms-1; i++)
//   {
//   	for(j=i+1; j<geomModel.ngeoms; j++)
//   	{
//   		geomModel.addCollisionPair(pinocchio::CollisionPair(i,j));
//   		pinocchio::Index idx = geomModel.findCollisionPair(pinocchio::CollisionPair(i,j));
// // --------- Collision Detection
//   		pinocchio::computeCollision(geomModel, geomData, idx);
//   		result = geomData.collisionResults[idx];
//       	result.getContacts(contacts);
//       	if(contacts.size() != 0)
//       	{
//       		for(k=0; k<contacts.size(); k++)
//       		{
//       			std::cout << "Contact Found Between: " << geomModel.getGeometryName(i) << " and " << geomModel.getGeometryName(j) << std::endl;
//         		std::cout << "position: " << contacts[k].pos << std::endl;
//         		std::cout << "-------------------" << std::endl;
//       		}
//       	}
// // --------- End Collision Detection
// // If no collision, compute distance
//       	else
//       	{
//       		pinocchio::computeDistance(geomModel, geomData, idx);
//       		dresult = geomData.distanceResults[idx];
//       		std::cout << "Minimum Distance Between: " << geomModel.getGeometryName(i) << " and " << geomModel.getGeometryName(j) << " = " << dresult.min_distance << std::endl;
//       	}
//   	}
//   }

  // std::cout << "box_geom_id: " << box_geom_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex pelvis_id = geomModel.getGeometryId("pelvis_0");
  // std::cout << "pelvis_id: " << pelvis_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftHipYawLink_id = geomModel.getGeometryId("leftHipYawLink_0");
  // std::cout << "leftHipYawLink_id: " << leftHipYawLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftHipRollLink_id = geomModel.getGeometryId("leftHipRollLink_0");
  // std::cout << "leftHipRollLink_id: " << leftHipRollLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftHipPitchLink_id = geomModel.getGeometryId("leftHipPitchLink_0");
  // std::cout << "leftHipPitchLink_id: " << leftHipPitchLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftKneePitchLink_id = geomModel.getGeometryId("leftKneePitchLink_0");
  // std::cout << "leftKneePitchLink_id: " << leftKneePitchLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftAnklePitchLink_id = geomModel.getGeometryId("leftAnklePitchLink_0");
  // std::cout << "leftAnklePitchLink_id: " << leftAnklePitchLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftFoot_id = geomModel.getGeometryId("leftFoot_0");
  // std::cout << "leftFoot_id: " << leftFoot_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex rightHipYawLink_id = geomModel.getGeometryId("rightHipYawLink_0");
  // std::cout << "rightHipYawLink_id: " << rightHipYawLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex rightHipRollLink_id = geomModel.getGeometryId("rightHipRollLink_0");
  // std::cout << "rightHipRollLink_id: " << rightHipRollLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex rightHipPitchLink_id = geomModel.getGeometryId("rightHipPitchLink_0");
  // std::cout << "rightHipPitchLink_id: " << rightHipPitchLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex rightKneePitchLink_id = geomModel.getGeometryId("rightKneePitchLink_0");
  // std::cout << "rightKneePitchLink_id: " << rightKneePitchLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex rightAnklePitchLink_id = geomModel.getGeometryId("rightAnklePitchLink_0");
  // std::cout << "rightAnklePitchLink_id: " << rightAnklePitchLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex rightFoot_id = geomModel.getGeometryId("rightFoot_0");
  // std::cout << "rightFoot_id: " << rightFoot_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex torsoYawLink_id = geomModel.getGeometryId("torsoYawLink_0");
  // std::cout << "torsoYawLink_id: " << torsoYawLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex torsoPitchLink_id = geomModel.getGeometryId("torsoPitchLink_0");
  // std::cout << "torsoPitchLink_id: " << torsoPitchLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex torso_id = geomModel.getGeometryId("torso_0");
  // std::cout << "torso_id: " << torso_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftShoulderPitchLink_id = geomModel.getGeometryId("leftShoulderPitchLink_0");
  // std::cout << "leftShoulderPitchLink_id: " << leftShoulderPitchLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftShoulderRollLink_id = geomModel.getGeometryId("leftShoulderRollLink_0");
  // std::cout << "leftShoulderRollLink_id: " << leftShoulderRollLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftElbowPitchLink_id = geomModel.getGeometryId("leftElbowPitchLink_0");
  // std::cout << "leftElbowPitchLink_id: " << leftElbowPitchLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftForearmLink_id = geomModel.getGeometryId("leftForearmLink_0");
  // std::cout << "leftForearmLink_id: " << leftForearmLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftWristRollLink_id = geomModel.getGeometryId("leftWristRollLink_0");
  // std::cout << "leftWristRollLink_id: " << leftWristRollLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftPalm_id = geomModel.getGeometryId("leftPalm_0");
  // std::cout << "leftPalm_id: " << leftPalm_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftIndexFingerPitch1Link_id = geomModel.getGeometryId("leftIndexFingerPitch1Link_0");
  // std::cout << "leftIndexFingerPitch1Link_id: " << leftIndexFingerPitch1Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftIndexFingerPitch2Link_id = geomModel.getGeometryId("leftIndexFingerPitch2Link_0");
  // std::cout << "leftIndexFingerPitch2Link_id: " << leftIndexFingerPitch2Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftIndexFingerPitch3Link_id = geomModel.getGeometryId("leftIndexFingerPitch3Link_0");
  // std::cout << "leftIndexFingerPitch3Link_id: " << leftIndexFingerPitch3Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftMiddleFingerPitch1Link_id = geomModel.getGeometryId("leftMiddleFingerPitch1Link_0");
  // std::cout << "leftMiddleFingerPitch1Link_id: " << leftMiddleFingerPitch1Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftMiddleFingerPitch2Link_id = geomModel.getGeometryId("leftMiddleFingerPitch2Link_0");
  // std::cout << "leftMiddleFingerPitch2Link_id: " << leftMiddleFingerPitch2Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftMiddleFingerPitch3Link_id = geomModel.getGeometryId("leftMiddleFingerPitch3Link_0");
  // std::cout << "leftMiddleFingerPitch3Link_id: " << leftMiddleFingerPitch3Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftPinkyPitch1Link_id = geomModel.getGeometryId("leftPinkyPitch1Link_0");
  // std::cout << "leftPinkyPitch1Link_id: " << leftPinkyPitch1Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftPinkyPitch2Link_id = geomModel.getGeometryId("leftPinkyPitch2Link_0");
  // std::cout << "leftPinkyPitch2Link_id: " << leftPinkyPitch2Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftPinkyPitch3Link_id = geomModel.getGeometryId("leftPinkyPitch3Link_0");
  // std::cout << "leftPinkyPitch3Link_id: " << leftPinkyPitch3Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftThumbRollLink_id = geomModel.getGeometryId("leftThumbRollLink_0");
  // std::cout << "leftThumbRollLink_id: " << leftThumbRollLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftThumbPitch1Link_id = geomModel.getGeometryId("leftThumbPitch1Link_0");
  // std::cout << "leftThumbPitch1Link_id: " << leftThumbPitch1Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftThumbPitch2Link_id = geomModel.getGeometryId("leftThumbPitch2Link_0");
  // std::cout << "leftThumbPitch2Link_id: " << leftThumbPitch2Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftThumbPitch3Link_id = geomModel.getGeometryId("leftThumbPitch3Link_0");
  // std::cout << "leftThumbPitch3Link_id: " << leftThumbPitch3Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex lowerNeckPitchLink_id = geomModel.getGeometryId("lowerNeckPitchLink_0");
  // std::cout << "lowerNeckPitchLink_id: " << lowerNeckPitchLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex neckYawLink_id = geomModel.getGeometryId("neckYawLink_0");
  // std::cout << "neckYawLink_id: " << neckYawLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex upperNeckPitchLink_id = geomModel.getGeometryId("upperNeckPitchLink_0");
  // std::cout << "upperNeckPitchLink_id: " << upperNeckPitchLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex head_id = geomModel.getGeometryId("head_0");
  // std::cout << "head_id: " << head_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex head_1_id = geomModel.getGeometryId("head_1");
  // std::cout << "head_1_id: " << head_1_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex hokuyo_link_id = geomModel.getGeometryId("hokuyo_link_0");
  // std::cout << "hokuyo_link_id: " << hokuyo_link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex hokuyo_link_1_id = geomModel.getGeometryId("hokuyo_link_1");
  // std::cout << "hokuyo_link_1_id: " << hokuyo_link_1_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex rightShoulderPitchLink_id = geomModel.getGeometryId("rightShoulderPitchLink_0");
  // std::cout << "rightShoulderPitchLink_id: " << rightShoulderPitchLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex rightShoulderRollLink_id = geomModel.getGeometryId("rightShoulderRollLink_0");
  // std::cout << "rightShoulderRollLink_0: " << rightShoulderRollLink_0 << std::endl;

  // pinocchio::GeometryModel::GeomIndex rightElbowPitchLink_id = geomModel.getGeometryId("rightElbowPitchLink_0");
  // std::cout << "rightElbowPitchLink_id: " << rightElbowPitchLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex rightForearmLink_id = geomModel.getGeometryId("rightForearmLink_0");
  // std::cout << "rightForearmLink_id: " << rightForearmLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex rightWristRollLink_id = geomModel.getGeometryId("rightWristRollLink_0");
  // std::cout << "rightWristRollLink_id: " << rightWristRollLink_id << std::endl;

  // //-/////////////////////////////////////////////////////////////////

  // pinocchio::GeometryModel::GeomIndex leftPalm_id = geomModel.getGeometryId("leftPalm_0");
  // std::cout << "leftPalm_id: " << leftPalm_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftIndexFingerPitch1Link_id = geomModel.getGeometryId("leftIndexFingerPitch1Link_0");
  // std::cout << "leftIndexFingerPitch1Link_id: " << leftIndexFingerPitch1Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftIndexFingerPitch2Link_id = geomModel.getGeometryId("leftIndexFingerPitch2Link_0");
  // std::cout << "leftIndexFingerPitch2Link_id: " << leftIndexFingerPitch2Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftIndexFingerPitch3Link_id = geomModel.getGeometryId("leftIndexFingerPitch3Link_0");
  // std::cout << "leftIndexFingerPitch3Link_id: " << leftIndexFingerPitch3Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftMiddleFingerPitch1Link_id = geomModel.getGeometryId("leftMiddleFingerPitch1Link_0");
  // std::cout << "leftMiddleFingerPitch1Link_id: " << leftMiddleFingerPitch1Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftMiddleFingerPitch2Link_id = geomModel.getGeometryId("leftMiddleFingerPitch2Link_0");
  // std::cout << "leftMiddleFingerPitch2Link_id: " << leftMiddleFingerPitch2Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftMiddleFingerPitch3Link_id = geomModel.getGeometryId("leftMiddleFingerPitch3Link_0");
  // std::cout << "leftMiddleFingerPitch3Link_id: " << leftMiddleFingerPitch3Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftPinkyPitch1Link_id = geomModel.getGeometryId("leftPinkyPitch1Link_0");
  // std::cout << "leftPinkyPitch1Link_id: " << leftPinkyPitch1Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftPinkyPitch2Link_id = geomModel.getGeometryId("leftPinkyPitch2Link_0");
  // std::cout << "leftPinkyPitch2Link_id: " << leftPinkyPitch2Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftPinkyPitch3Link_id = geomModel.getGeometryId("leftPinkyPitch3Link_0");
  // std::cout << "leftPinkyPitch3Link_id: " << leftPinkyPitch3Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftThumbRollLink_id = geomModel.getGeometryId("leftThumbRollLink_0");
  // std::cout << "leftThumbRollLink_id: " << leftThumbRollLink_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftThumbPitch1Link_id = geomModel.getGeometryId("leftThumbPitch1Link_0");
  // std::cout << "leftThumbPitch1Link_id: " << leftThumbPitch1Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftThumbPitch2Link_id = geomModel.getGeometryId("leftThumbPitch2Link_0");
  // std::cout << "leftThumbPitch2Link_id: " << leftThumbPitch2Link_id << std::endl;

  // pinocchio::GeometryModel::GeomIndex leftThumbPitch3Link_id = geomModel.getGeometryId("leftThumbPitch3Link_0");
  // std::cout << "leftThumbPitch3Link_id: " << leftThumbPitch3Link_id << std::endl;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2017 John Maddock
//  Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef BOOST_MATH_HYPERGEOMETRIC_1F1_SCALED_SERIES_HPP
#define BOOST_MATH_HYPERGEOMETRIC_1F1_SCALED_SERIES_HPP

#include <boost/array.hpp>

  namespace boost{ namespace math{ namespace detail{

     template <class T, class Policy>
     T hypergeometric_1F1_scaled_series(const T& a, const T& b, T z, const Policy& pol, const char* function)
     {
        BOOST_MATH_STD_USING
        //
        // Result is returned scaled by e^-z.
        // Whenever the terms start becoming too large, we scale by some factor e^-n
        // and keep track of the integer scaling factor n.  At the end we can perform
        // an exact subtraction of n from z and scale the result:
        //
        T sum(0), term(1), upper_limit(sqrt(boost::math::tools::max_value<T>())), diff;
        unsigned n = 0;
        long long log_scaling_factor = 1 - lltrunc(boost::math::tools::log_max_value<T>());
        T scaling_factor = exp(T(log_scaling_factor));
        boost::intmax_t current_scaling = 0;

        do
        {
           sum += term;
           if (sum >= upper_limit)
           {
              sum *= scaling_factor;
              term *= scaling_factor;
              current_scaling += log_scaling_factor;
           }
           term *= (((a + n) / ((b + n) * (n + 1))) * z);
           if (n > boost::math::policies::get_max_series_iterations<Policy>())
              return boost::math::policies::raise_evaluation_error(function, "Series did not converge, best value is %1%", sum, pol);
           ++n;
           diff = fabs(term / sum);
        } while (diff > boost::math::policies::get_epsilon<T, Policy>());

        z = -z - current_scaling;
        while (z < log_scaling_factor)
        {
           z -= log_scaling_factor;
           sum *= scaling_factor;
        }
        return sum * exp(z);
     }

  } } } // namespaces

#endif // BOOST_MATH_HYPERGEOMETRIC_1F1_SCALED_SERIES_HPP


#include <armadillo>
#include <boost/program_options.hpp>
#include <json.hpp>
#include <HMM.hpp>
#include <ProMPs_emission.hpp>

using namespace arma;
using namespace hsmm;
using namespace robotics;
using namespace std;
namespace po = boost::program_options;

field<mat> fromMatToField(const mat& obs) {
    field<mat> ret(obs.n_cols);
    for(int i = 0; i < obs.n_cols; i++)
        ret(i) = obs.col(i);
    return ret;
}

// Running the Viterbi algorithm.
void ViterbiAlgorithm(HSMM& promp_hsmm, const field<field<mat>>& seq_obs,
        string filename) {
    int nseq = seq_obs.n_elem;
    for(int s = 0; s < nseq; s++) {
        const field<mat>& obs = seq_obs(s);
        int nstates = promp_hsmm.nstates_;
        int nobs = obs.n_elem;
        imat psi_duration(nstates, nobs, fill::zeros);
        imat psi_state(nstates, nobs, fill::zeros);
        mat delta(nstates, nobs, fill::zeros);
        cout << "Before pdf" << endl;
        cube log_pdf = promp_hsmm.computeEmissionsLogLikelihood(obs);
        cout << "After pdf" << endl;
        Viterbi(promp_hsmm.transition_, promp_hsmm.pi_, promp_hsmm.duration_,
                log_pdf, delta, psi_duration, psi_state,
                promp_hsmm.min_duration_, nobs);
        cout << "Delta last column" << endl;
        cout << delta.col(nobs - 1) << endl;
        ivec viterbiStates, viterbiDurations;
        viterbiPath(psi_duration, psi_state, delta, viterbiStates,
                viterbiDurations);
        cout << "Viterbi states and durations" << endl;
        imat states_and_durations = join_horiz(viterbiStates, viterbiDurations);
        cout << states_and_durations << endl;
        cout << "Python states list representation" << endl;
        cout << "[";
        for(int i = 0; i < viterbiStates.n_elem; i++)
            cout << viterbiStates[i] <<
                    ((i + 1 == viterbiStates.n_elem)? "]" : ",");
        cout << endl;
        cout << "Python duration list representation" << endl;
        cout << "[";
        for(int i = 0; i < viterbiDurations.n_elem; i++)
            cout << viterbiDurations[i] <<
                    ((i + 1 == viterbiDurations.n_elem)? "]" : ",");
        cout << endl;

        // Saving the matrix of joint states and durations.
        string viterbi_filename(filename);
        if (nseq > 1)
            viterbi_filename += string(".") + to_string(s);
        states_and_durations.save(viterbi_filename, raw_ascii);
    }
}

int main(int argc, char *argv[]) {
    po::options_description desc("Options");
    desc.add_options()
        ("help,h", "Produce help message")
        ("input,i", po::value<string>(), "Path to the input obs")
        ("output,o", po::value<string>(), "Path to the json output params")
        ("nstates,s", po::value<int>(), "Number of hidden states")
        ("commitid", po::value<string>(), "Git commit id of the experiment")
        ("labels", po::value<string>(), "Path to the provided labels")
        ("viterbi,v", po::value<string>(), "Path to the output viterbi file")
        ("nfiles", po::value<int>()->default_value(1),
                "Number of files (sequences) to process");
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
    if (vm.count("help")) {
        cout << desc << endl;
        return 0;
    }
    if (!vm.count("input") || !vm.count("output") || !vm.count("nstates")) {
        cerr << "Error: You should provide input files, output files and "
                "number of hidden states" << endl;
        return 1;
    }
    string input_filename = vm["input"].as<string>();
    string output_filename = vm["output"].as<string>();
    field<field<mat>> seq_obs(1);
    field<Labels> seq_labels(1);
    int njoints;
    int nseq = vm["nfiles"].as<int>();
    seq_obs.set_size(nseq);
    seq_labels.set_size(nseq);
    for(int i = 0; i < seq_obs.n_elem; i++) {
        string name = input_filename;
        if (nseq != 1)
            name += string(".") + to_string(i);
        mat obs;
        obs.load(name, raw_ascii);
        njoints = obs.n_rows;
        int nobs = obs.n_cols;
        cout << "Time series shape: (" << njoints << ", " << nobs <<
            ")." << endl;
        seq_obs(i) = {obs};

        // Reading labels for different obs.
        if (!vm.count("labels"))
            continue;
        string labels_name = vm["labels"].as<string>();
        if (nseq != 1)
            labels_name += string(".") + to_string(i);
        mat labels_mat;
        labels_mat.load(labels_name);
        for(int j = 0; j < labels_mat.n_rows; j++)
            seq_labels(i).setLabel(labels_mat(j, 0), labels_mat(j, 1),
                    labels_mat(j, 2));
    }

    int nstates = vm["nstates"].as<int>();
    mat transition(nstates, nstates);
    transition.fill(1.0 / nstates );
    vec pi(nstates);
    pi.fill(1.0/nstates);

    // Setting a combination of polynomial and rbf basis functions.
    auto rbf = shared_ptr<ScalarGaussBasis>(new ScalarGaussBasis(
                {0.25,0.5,0.75},0.25));
    auto poly = make_shared<ScalarPolyBasis>(1);
    auto comb = shared_ptr<ScalarCombBasis>(new ScalarCombBasis({rbf, poly}));
    int n_basis_functions = comb->dim();

    // Instantiating as many ProMPs as hidden states.
    vector<FullProMP> promps;
    for(int i = 0; i < nstates; i++) {
        vec mu_w(n_basis_functions * njoints);
        mu_w.randn();
        mat Sigma_w = eye<mat>(n_basis_functions * njoints,
                    n_basis_functions * njoints);
        mat Sigma_y = 0.001 * eye<mat>(njoints, njoints);
        ProMP promp(mu_w, Sigma_w, Sigma_y);
        FullProMP poly(comb, promp, njoints);
        promps.push_back(poly);
    }

    // Creating the ProMP emission.
    shared_ptr<ProMPsEmissionHMM> ptr_emission(new ProMPsEmissionHMM(promps));

    // Creating a prior for Sigma_w.
    mat Phi = 0.5 * eye<mat>(n_basis_functions * njoints,
            n_basis_functions * njoints);
    NormalInverseWishart iw_prior(Phi, Phi.n_rows + 2);
    ptr_emission->set_Sigma_w_Prior(iw_prior);

    HMM promp_hsmm(std::static_pointer_cast<AbstractEmission>(ptr_emission),
            transition, pi);

    // Saving the model in a json file.
    std::ofstream initial_params(output_filename);
    nlohmann::json initial_model = promp_hsmm.to_stream();
    if (vm.count("commitid"))
        initial_model["git_commit_id"] = vm["commitid"].as<string>();
    initial_params << std::setw(4) << initial_model << std::endl;
    initial_params.close();

    for(int i = 0; i < 10; i++) {

        // Reading the current parameters.
        std::ifstream current_params_stream(output_filename);
        nlohmann::json current_params;
        current_params_stream >> current_params;
        promp_hsmm.from_stream(current_params);
        bool convergence_reached = promp_hsmm.fit(seq_obs, seq_labels, 5, 1e-5);

        // Saving again the parameters after one training iteration.
        std::ofstream output_params(output_filename);
        current_params = promp_hsmm.to_stream();
        if (vm.count("commitid"))
            current_params["git_commit_id"] = vm["commitid"].as<string>();
        output_params << std::setw(4) << current_params << std::endl;
        output_params.close();

        if (vm.count("viterbi"))
            ViterbiAlgorithm(promp_hsmm, seq_obs, vm["viterbi"].as<string>());

        if (convergence_reached)
            break;

    }
    return 0;
}


#include "phase_limiter/auto_mastering.h"

#include <cmath>
#include <iostream>
#include <vector>
#include <string>
#include <stdexcept>
#include <algorithm>
#include <fstream>
#include <mutex>
#include "gflags/gflags.h"
#include "picojson.h"
#include "tbb/tbb.h"
#include <optim.hpp>
#include <boost/archive/binary_iarchive.hpp>

#include "bakuage/sound_quality2.h"
#include "bakuage/ms_compressor_filter.h"
#include "bakuage/utils.h"
#include "bakuage/fir_design.h"
#include "bakuage/fir_filter2.h"

DECLARE_string(sound_quality2_cache);
DECLARE_string(mastering5_optimization_algorithm);
DECLARE_int32(mastering5_optimization_max_eval_count);
DECLARE_double(mastering5_mastering_level);
DECLARE_string(mastering5_mastering_reference_file);

typedef float Float;
using namespace bakuage;

namespace {
    // compress(x) -> wet_gain -> output
    // x -> dry_gain -> output
    class LoudnessMapping {
    public:
        LoudnessMapping() {}
        LoudnessMapping(Float original_mean, Float relative_threshold,
                        Float wet_gain, Float relative_dry_gain, Float ratio):
        original_mean_(original_mean),
        target_mean_(original_mean + wet_gain),
        threshold_(original_mean + relative_threshold),
        dry_gain_(wet_gain + relative_dry_gain), inv_ratio_(1.0 / ratio) {}

        Float operator () (Float x) const {
            static const float log10_div_20 = std::log(10) / 20;
            Float w = std::max(threshold_, x);
            Float gain = (w - original_mean_) * inv_ratio_ + target_mean_ - w;
            Float y = x + gain;
            Float z = x + dry_gain_;
            return 20 * std::log10(1e-37 + 0.5 * std::exp(log10_div_20 * y) + 0.5 * std::exp(log10_div_20 * z));
        }

        Float threshold() const { return threshold_; }
    private:
        Float original_mean_;
        Float target_mean_;
        Float threshold_;
        Float dry_gain_;
        Float inv_ratio_;
    };
    typedef MsCompressorFilter<Float, LoudnessMapping, LoudnessMapping> Compressor;

    typedef arma::vec EffectParams;

    struct BandEffect {
        LoudnessMapping loudness_mapping;
        LoudnessMapping ms_loudness_mapping;
    };

    // 空間は重要。最適化のしやすさに関わる (あきらかに同じ値を返すような点が複数あると効率が悪い)
    // param変換。param in [a, b]。0で恒等変換になるようにする
    // 両側に均等なものはparam in [-1, 1]だが、そうでないものは[0, 1]とかもありえる
    Float ToRelThreshold(Float x) {
        return 20 * x;
    }
    Float ToWetGain(Float x) {
        return 10 * x;
    }
    Float ToRelativeDryGain(Float x) {
        return 10 * x;
    }
    Float ToRatio(Float x) {
        return std::pow(5, x);
    }

    // すべてゼロで恒等変換になるようにする
    struct Effect {
        Effect(const Eigen::VectorXd &original_mean, const EffectParams& params) {
            const int band_count = params.size() / 8;
            band_effects.resize(band_count);
            for (int i = 0; i < band_count; i++) {
                band_effects[i].loudness_mapping = LoudnessMapping(original_mean[2 * i + 0], ToRelThreshold(params(8 * i + 0)), ToWetGain(params(8 * i + 1)), ToRelativeDryGain(params(8 * i + 2)), ToRatio(params(8 * i + 3)));
                band_effects[i].ms_loudness_mapping = LoudnessMapping(original_mean[2 * i + 1], ToRelThreshold(params(8 * i + 4)), ToWetGain(params(8 * i + 5)), ToRelativeDryGain(params(8 * i + 6)), ToRatio(params(8 * i + 7)));
            }
        }
        std::vector<BandEffect> band_effects;
    };

}

namespace phase_limiter {

    // audio_analyzer(CalculateMultibandLoudness2)の仕様に合わせて、mean, covを計算する。
    // エフェクトはloudness vector上でシミュレーションする
    // 基準ラウドネスの違いとかはホワイトノイズを処理して補正値を計算して補正する
    void AutoMastering5(std::vector<float> *_wave, const int sample_rate, const std::function<void(float)> &progress_callback) {
        const int frames = _wave->size() / 2;
        const int channels = 2;
        const float block_sec = 0.4;

        // initialize sound quality calculator
        bakuage::SoundQuality2Calculator calculator;
        {
            std::ifstream ifs(FLAGS_sound_quality2_cache);
            boost::archive::binary_iarchive ia(ifs);
            ia >> calculator;
        }
        const auto band_count = calculator.band_count();
        const auto bands = calculator.bands();

        // initialize reference
        bakuage::MasteringReference2 mastering_reference;
        if (!FLAGS_mastering5_mastering_reference_file.empty()) {
            Eigen::VectorXd mean;
            Eigen::MatrixXd cov;
            bakuage::SoundQuality2CalculatorUnit::ParseReference(bakuage::LoadStrFromFile(FLAGS_mastering5_mastering_reference_file.c_str()).c_str(), &mean, &cov);
            mastering_reference = bakuage::MasteringReference2(mean, cov);
        }

        // calculate original band loudness vectors
        std::vector<bakuage::AlignedPodVector<float>> band_loudnesses;
        {
            // 400ms block
            const int sample_freq = sample_rate;
            const int width = bakuage::CeilInt<int>(sample_freq * block_sec, 4);
            const int shift = width / 2; // 本当は75% overlapだけど、高速化のために50% overlap
            const int samples = frames;

            bakuage::AlignedPodVector<Float> filtered(channels * samples);
            for (int i = 0; i < channels; i++) {
                LoudnessFilter<float> filter(sample_freq);
                for (int j = 0; j < samples; j++) {
                    int k = channels * j + i;
                    filtered[k] = filter.Clock((*_wave)[k]);
                }
            }

            const int spec_len = width / 2 + 1;

            // FFTの正規化も行う (sqrt(hanning)窓)
            bakuage::AlignedPodVector<float> window(width);
            bakuage::CopyHanning(width, window.data(), 1.0 / std::sqrt(width));

            // 規格では最後のブロックは使わないけど、
            // 使ったほうが実用的なので使う
            band_loudnesses.resize(bakuage::CeilInt(samples, shift) / shift);
            tbb::parallel_for<int>(0, band_loudnesses.size(), [&](int pos_idx) {
                const int pos = pos_idx * shift;
                bakuage::AlignedPodVector<float> band_loudness(2 * band_count);
                int end = std::min<int>(pos + width, samples);

                auto &pool = bakuage::ThreadLocalDftPool<bakuage::RealDft<Float>>::GetThreadInstance();
                const auto dft = pool.Get(width);

                bakuage::AlignedPodVector<float> fft_input(width);
                std::vector<bakuage::AlignedPodVector<std::complex<float>>> fft_outputs(channels);
                for (int ch = 0; ch < channels; ch++) {
                    fft_outputs[ch].resize(spec_len);
                }

                // FFT
                for (int ch = 0; ch < channels; ch++) {
                    for (int i = 0; i < width; i++) {
                        fft_input[i] = pos + i < end ? filtered[channels * (pos + i) + ch] * window[i] : 0;
                    }
                    dft->Forward(fft_input.data(), (float *)fft_outputs[ch].data(), pool.work());
                }

                // binをbandに振り分けていく
                for (int band_index = 0; band_index < band_count; band_index++) {
                    int low_bin_index = std::floor(width * bands[band_index].low_freq / sample_freq);
                    int high_bin_index = std::min<int>(std::floor(width * (bands[band_index].high_freq == 0 ? 0.5 : bands[band_index].high_freq / sample_freq)), spec_len);

                    // mid
                    double sum = 0;
                    for (int i = low_bin_index; i < high_bin_index; i++) {
                        sum += std::norm(fft_outputs[0][i] + fft_outputs[1][i]) ;
                    }
                    band_loudness[2 * band_index + 0] = 10 * std::log10(1e-7//1e-37
                                                                     + sum / (0.5 * width));
                    // side
                    sum = 0;
                    for (int i = low_bin_index; i < high_bin_index; i++) {
                        sum += std::norm(fft_outputs[0][i] - fft_outputs[1][i]);
                    }
                    band_loudness[2 * band_index + 1] = 10 * std::log10(1e-7//1e-37
                                                                      + sum / (0.5 * width));
                }

                band_loudnesses[pos_idx] = std::move(band_loudness);
            });
        }
        progress_callback(0.1);

        // エフェクト補正値計算
        // ホワイトノイズに窓掛けFFTで上記処理をしたラウドネスと、
        // ホワイトノイズをコンプレッサーで解析したラウドネスを比較する
        // -> いったんなしで、差分を出力してみて大きいなら考える。

        // エフェクト適用シミュレーション (ms compressor filter)
        const auto apply_effect = [](const Effect &effect, const Float *input, Float *output) {
            for (int i = 0; i < effect.band_effects.size(); i++) {
                const auto &band_effect = effect.band_effects[i];
                // static const float sqrt_0_5 = std::sqrt(0.5); // 元のMsCompressorFilterはmsにするときにこれをかけていた。誤差が問題になるなら考える
                static const float log10_div_20 = std::log(10) / 20;

                Float input_m = input[2 * i + 0];
                Float input_s = input[2 * i + 1];
                Float rms_m = std::pow(10, 0.1 * input_m);
                Float rms_s = std::pow(10, 0.1 * input_s);

                Float total_loudness = -0.691 + 10 * std::log10(rms_m + rms_s + 1e-37);
                Float mapped_loudness = band_effect.loudness_mapping(total_loudness);

                Float mid_to_side_loudness = input_s - input_m;
                Float side_gain = std::exp(log10_div_20 * (band_effect.ms_loudness_mapping(mid_to_side_loudness) - mid_to_side_loudness));

                Float total_loudness_with_side_gain = -0.691 + 10 * std::log10(rms_m + rms_s * bakuage::Sqr(side_gain) + 1e-37);
                Float gain = std::exp(log10_div_20 * (mapped_loudness - total_loudness_with_side_gain));

                output[2 * i + 0] = 10 * std::log10(rms_m * bakuage::Sqr(gain));
                output[2 * i + 1] = 10 * std::log10(rms_s * bakuage::Sqr(side_gain * gain));
            }
        };

        // エフェクトパラメータからエフェクト適用後のmean, covと各バンドの平均誤差を計算する
        const auto calc_mean_cov = [apply_effect, band_count, &band_loudnesses](const Effect *effect, Eigen::VectorXd *mean_vec, Eigen::MatrixXd *cov, float *mse) {
            const auto relative_threshold_db = -20;
            mean_vec->resize(2 * band_count);
            cov->resize(2 * band_count, 2 * band_count);
            *mse = 0;

            // apply effect
            bakuage::AlignedPodVector<float> applied(2 * band_count);
            std::vector<bakuage::AlignedPodVector<float>> loudness_blocks(2 * band_count);
            for (int i = 0; i < 2 * band_count; i++) {
                loudness_blocks[i].resize(band_loudnesses.size());
            }
            for (int i = 0; i < band_loudnesses.size(); i++) {
                if (effect) {
                    apply_effect(*effect, band_loudnesses[i].data(), applied.data());
                } else {
                    bakuage::TypedMemcpy(applied.data(), band_loudnesses[i].data(), applied.size());
                }
                for (int j = 0; j < applied.size(); j++) {
                    *mse += bakuage::Sqr(band_loudnesses[i][j] - applied[j]);
                    loudness_blocks[j][i] = applied[j];
                }
            }
            *mse /= band_loudnesses.size() * applied.size();

            // calculate mean
            bakuage::AlignedPodVector<Float> thresholds(2 * band_count);
            for (int band_index = 0; band_index < 2 * band_count; band_index++) {
                const auto &band_blocks = loudness_blocks[band_index];

                double threshold = -1e10;//-70;
                for (int k = 0; k < 2; k++) {
                    Float count = 0;
                    Float sum = 0;
                    for (const auto &z: band_blocks) {
                        const bool valid = z >= threshold; // for easy optimization
                        count += valid;
                        sum += valid ? z : 0;
                    }

                    double mean = sum / (1e-37 + count);
                    if (k == 0) {
                        threshold = mean + relative_threshold_db;
                        thresholds[band_index] = threshold;
                    }
                    else if (k == 1) {
                        (*mean_vec)[band_index] = mean;
                    }
                }
            }

            // calculate covariance
            for (int band_index1 = 0; band_index1 < 2 * band_count; band_index1++) {
                for (int band_index2 = band_index1; band_index2 < 2 * band_count; band_index2++) {
                    const Float mean1 = (*mean_vec)[band_index1];
                    const Float mean2 = (*mean_vec)[band_index2];
                    const Float threshold1 = thresholds[band_index1];
                    const Float threshold2 = thresholds[band_index2];

                    const auto &band_blocks1 = loudness_blocks[band_index1];
                    const auto &band_blocks2 = loudness_blocks[band_index2];

                    Float v = 0;
                    Float c = 0;
                    for (int i = 0; i < band_blocks1.size(); i++) {
                        const auto x1 = band_blocks1[i];
                        const auto x2 = band_blocks2[i];
                        const bool valid = (x1 >= threshold1) & (x2 >= threshold2); // for easy optimization (not && instead &)
                        v += valid * (x1 - mean1) * (x2 - mean2);
                        c += valid;
                    }
                    v  /= (1e-37 + c);
                    (*cov)(band_index1, band_index2) = v;
                    (*cov)(band_index2, band_index1) = v;
                }
            }
        };

        // calculate original
        Eigen::VectorXd original_mean;
        Eigen::MatrixXd original_cov;
        float original_mse;
        calc_mean_cov(nullptr, &original_mean, &original_cov, &original_mse);

        // define bound
        arma::vec lower_bounds(8 * band_count);
        arma::vec upper_bounds(8 * band_count);
        for (int i = 0; i < band_count; i++) {
            lower_bounds(8 * i + 0) = -1; // rel threshold
            upper_bounds(8 * i + 0) = 0.01;
            lower_bounds(8 * i + 1) = -1; // wet gain
            upper_bounds(8 * i + 1) = 1;
            lower_bounds(8 * i + 2) = -1; // relative dry gain
            upper_bounds(8 * i + 2) = 0.01;
            lower_bounds(8 * i + 3) = -0.01; // ratio
            upper_bounds(8 * i + 3) = 1;
            lower_bounds(8 * i + 4) = -1;
            upper_bounds(8 * i + 4) = 0.01;
            lower_bounds(8 * i + 5) = -1;
            upper_bounds(8 * i + 5) = 1;
            lower_bounds(8 * i + 6) = -1;
            upper_bounds(8 * i + 6) = 0.01;
            lower_bounds(8 * i + 7) = -0.01;
            upper_bounds(8 * i + 7) = 1;
        }
        {
            const double scale = 1e-2 + FLAGS_mastering5_mastering_level;
            lower_bounds *= scale;
            upper_bounds *= scale;
        }

        // エフェクトパラメータから評価関数を計算する
        std::mutex eval_mtx;
        float min_eval = 1e100;
        int eval_count = 0;
        const auto calc_eval = [calc_mean_cov, &calculator, &original_mean, &min_eval, &eval_count, &eval_mtx, &progress_callback, &lower_bounds, &upper_bounds, &mastering_reference](const EffectParams &params) -> double {
            Eigen::VectorXd mean;
            Eigen::MatrixXd cov;
            float msp = 0;
            for (int i = 0; i < params.size(); i++) {
                msp += bakuage::Sqr(params(i));
            }
            msp /= params.size();
            float bound_error = 0;
            for (int i = 0; i < params.size(); i++) {
                bound_error += bakuage::Sqr(std::max<float>(0, lower_bounds[i] - params[i]));
                bound_error += bakuage::Sqr(std::max<float>(0, params[i] - upper_bounds[i]));
            }
            float mse;
            Effect effect(original_mean, params);
            calc_mean_cov(&effect, &mean, &cov, &mse);

            const bakuage::MasteringReference2 target(mean, cov);
            float main_eval = 0;
            if (FLAGS_mastering5_mastering_reference_file.empty()) {
                float sound_quality;
                calculator.CalculateSoundQuality(target, &sound_quality, nullptr);
                main_eval = -sound_quality;
            } else {
                main_eval = calculator.CalculateDistance(mastering_reference, target);
            }

            // 事前の実験でalphaをいろいろ変えて最適化してmseを見たところ
            // 最適解のところで sound_qualityの微分とmseの微分が一致していると仮定すると、
            // alpha = 0.02 / sqrt(mse)になった。
            // mseの許容量(target_mse)からalphaを逆算して決める。
            // target_mseはFLAGS_mastering5_mastering_levelを元に適当に決める
            // betaはエフェクトのパラメータを変えたときにどのくらいmseが変わるかを考えて、alphaに比例させて決める。(paramを1変化させて、特徴量が10dB変化するなら、beta = 10^2 * alpha -> いろいろ試して微調整)
            // 実際これでやってみてtarget_mseになるかどうかで妥当性検証可能
            const float target_mse = bakuage::Sqr(4 * (1e-2 + FLAGS_mastering5_mastering_level));
            const float alpha = 0.02 / std::sqrt(target_mse);
            const float beta = bakuage::Sqr(10.0) * alpha;
            const float eval = main_eval + alpha * mse + beta * msp + bound_error * 1e4;
            {
                std::lock_guard<std::mutex> lock(eval_mtx);
                eval_count++;
                if (eval_count % (FLAGS_mastering5_optimization_max_eval_count / 10) == 0 && eval_count < FLAGS_mastering5_optimization_max_eval_count) {
                    progress_callback(0.1 + 0.5 * eval_count / FLAGS_mastering5_optimization_max_eval_count);
                }
                if (min_eval > eval) {
                    min_eval = eval;
                    std::cerr << "optimization " << eval_count << "\t" << min_eval << "\t" << main_eval << "\t" << mse << "\t" << msp << std::endl;
                }
            }
            return eval;
        };

        // calc initial eval
        EffectParams zero_params(8 * band_count);
        for (int i = 0; i < zero_params.size(); i++) {
            zero_params(i) = 0;
        }
        const auto initial_eval = calc_eval(zero_params);
        std::cerr << "optimization initial_eval: " << initial_eval << std::endl;

        // エフェクトパラメータ探索
        const auto find_params = [calc_eval, band_count, &zero_params, initial_eval, &lower_bounds, &upper_bounds]() {
            optim::algo_settings_t settings;
#if 0
            // boundしているとうまく最適化できないので使わない。
            // grep inv_transform in optim
            // optimは有界な値を実数全体に変換して最適化している
            // 境界そのものはinfに変換されてしまうからうまく処理できない
            // 原点が境界そのものにならないように注意。
            // 最初の分布も重要。
            // boundして最適化すると境界付近の値になりやすい気がする (変換の形的に境界付近の存在確率が高まる？次元の呪い？)
            // 正則化強めだと、boundなしのほうが最適化性能が高い。正則化弱めだと少しだけboundありのほうが性能高いが大差無い
            // だから、boundなしで行こう。こっちのほうが安定している。
            // 評価関数側でなんとかする
            settings.lower_bounds = lower_bounds;
            settings.upper_bounds = upper_bounds;
            settings.vals_bound = true;
#endif
#if 1
            settings.de_initial_lb = lower_bounds;
            settings.de_initial_ub = upper_bounds;
            settings.pso_initial_lb = lower_bounds;
            settings.pso_initial_ub = upper_bounds;
#endif
            auto result = zero_params;
            bool success;
            // それぞれ最大繰り返し回数の指定方法が違うので注意。ソースコード参照
            if (FLAGS_mastering5_optimization_algorithm == "nm") {
                settings.iter_max = FLAGS_mastering5_optimization_max_eval_count / lower_bounds.size();
                success = optim::nm(result, [calc_eval](const arma::vec& vec, arma::vec* grad_out, void *opt_data) {
                    return calc_eval(vec);
                }, nullptr, settings);
            }
            else if (FLAGS_mastering5_optimization_algorithm == "pso") {
                settings.pso_n_gen = FLAGS_mastering5_optimization_max_eval_count / settings.pso_n_pop;
                success = optim::pso(result, [calc_eval](const arma::vec& vec, arma::vec* grad_out, void *opt_data) {
                    return calc_eval(vec);
                }, nullptr, settings);
            } else if (FLAGS_mastering5_optimization_algorithm == "pso_dv") {
                settings.pso_n_gen = FLAGS_mastering5_optimization_max_eval_count / settings.pso_n_pop;
                success = optim::pso_dv(result, [calc_eval](const arma::vec& vec, arma::vec* grad_out, void *opt_data) {
                    return calc_eval(vec);
                }, nullptr, settings);
            } else if (FLAGS_mastering5_optimization_algorithm == "de") {
                settings.de_max_fn_eval = FLAGS_mastering5_optimization_max_eval_count;
                success = optim::de(result, [calc_eval](const arma::vec& vec, arma::vec* grad_out, void *opt_data) {
                    return calc_eval(vec);
                }, nullptr, settings);
            } else if (FLAGS_mastering5_optimization_algorithm == "de_prmm") {
                settings.de_max_fn_eval = FLAGS_mastering5_optimization_max_eval_count;
                success = optim::de_prmm(result, [calc_eval](const arma::vec& vec, arma::vec* grad_out, void *opt_data) {
                    return calc_eval(vec);
                }, nullptr, settings);
            } else {
                throw std::logic_error(std::string("unknown FLAGS_mastering5_optimization_algorithm " + FLAGS_mastering5_optimization_algorithm));
            }
            const auto result_eval = calc_eval(result);
            std::cerr << "optimization success: " << success << std::endl;
            std::cerr << "optimization solution y: " << result_eval << std::endl;
            for (int i = 0; i < band_count; i++) {
                std::cerr << "optimization solution x " << i << "\t" << result(8 * i + 0) << "\t" << result(8 * i + 1) << "\t" << result(8 * i + 2) << "\t" << result(8 * i + 3) << std::endl;
                std::cerr << "optimization solution x ms " << i << "\t" << result(8 * i + 4) << "\t" << result(8 * i + 5) << "\t" << result(8 * i + 6) << "\t" << result(8 * i + 7) << std::endl;
            }
            if (result_eval < initial_eval) {
                return result;
            } else {
                return zero_params;
            }
        };

        const auto effect_params = find_params();
        const Effect effect(original_mean, effect_params);

        std::mutex result_mtx;
        std::mutex progression_mtx;
        std::vector<std::function<void ()>> tasks;
        std::vector<Float> result(_wave->size());
        bakuage::AlignedPodVector<Float> progressions(band_count);

        const auto update_progression = [&progressions, &progression_mtx, progress_callback](int i, Float p) {
            std::lock_guard<std::mutex> lock(progression_mtx);
            Float total = 0;
            progressions[i] = p;
            for (const auto &a : progressions) {
                total += a;
            }
            progress_callback(0.6 + 0.4 * total / progressions.size());
        };

        for (int band_index = 0; band_index < band_count; band_index++) {
            const auto &band = calculator.bands()[band_index];
            const auto update_progression_bound = std::bind(update_progression, band_index, std::placeholders::_1);
            const auto &band_effect = effect.band_effects[band_index];
            tasks.push_back([band, band_effect, sample_rate, frames, _wave, &result, &result_mtx, update_progression_bound]() {
                const float *wave_ptr = &(*_wave)[0];

                int fir_delay_samples;
                std::vector<Float> fir;
                {
                    fir_delay_samples = static_cast<int>(0.2 * sample_rate);
                    const int n = 2 * fir_delay_samples + 1;
                    Float freq1 = std::min<Float>(0.5, band.low_freq / sample_rate);
                    Float freq2 = std::min<Float>(0.5, band.high_freq == 0 ? 0.5 : band.high_freq / sample_rate);
                    fir = CalculateBandPassFir<Float>(freq1, freq2, n, 4);
                }
                update_progression_bound(0.1);

                const int len = frames + fir.size() - 1;
                bakuage::AlignedPodVector<float> filtered(channels * len);
                {
                    FirFilter2<Float> fir_filter(fir.begin(), fir.end());
                    bakuage::AlignedPodVector<Float> filter_temp_input(frames + fir_delay_samples);
                    bakuage::AlignedPodVector<Float> filter_temp_output(frames + fir_delay_samples);
                    for (int ch = 0; ch < channels; ch++) {
                        fir_filter.Clear();
                        for (int i = 0; i < frames; i++) {
                            filter_temp_input[i] = wave_ptr[channels * i + ch];
                        }
                        fir_filter.Clock(filter_temp_input.data(), filter_temp_input.data() + frames + fir_delay_samples, filter_temp_output.data());
                        for (int i = 0; i < frames; i++) {
                            filtered[channels * i + ch] = filter_temp_output[i + fir_delay_samples];
                        }
                    }
                }
                update_progression_bound(0.2);

                Compressor::Config compressor_config;
                compressor_config.loudness_mapping_func = band_effect.loudness_mapping;
                compressor_config.ms_loudness_mapping_func = band_effect.ms_loudness_mapping;
                compressor_config.max_mean_sec = 0.2;
                compressor_config.num_channels = channels;
                compressor_config.sample_rate = sample_rate;
                Compressor compressor(compressor_config);

                const int shift = compressor.delay_samples();
                const int len2 = frames + shift;
                filtered.resize(channels * len2);
                bakuage::AlignedPodVector<Float> temp_input(channels);
                bakuage::AlignedPodVector<Float> temp_output(channels);

                // filteredにin-placeで書き込んでから共有のresultに足しこむ
                for (int j = 0; j < len2; j++) {
                    for (int i = 0; i < channels; i++) {
                        temp_input[i] = filtered[channels * j + i];
                    }
                    compressor.Clock(&temp_input[0], &temp_output[0]);
                    for (int i = 0; i < channels; i++) {
                        filtered[channels * j + i] = temp_output[i];
                    }
                }
                update_progression_bound(0.8);

                // flush filtered (dry sound)
                {
                    std::lock_guard<std::mutex> lock(result_mtx);
                    const int len3 = frames * channels;
                    const int channels_shift = channels * shift;
                    bakuage::VectorAddInplace(filtered.data() + channels_shift, result.data(), len3);
                }
                update_progression_bound(1);
            });
        }

        tbb::parallel_for(0, (int)tasks.size(), [&tasks](int task_i) {
            tasks[task_i]();
        });

        *_wave = std::move(result);
    }

}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/function/is_flint.hpp>
#include <boost/simd/function/is_nltz.hpp>
#include <boost/simd/function/pow.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/assert.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( realpow_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG( is_nltz(a0) || is_flint(a1),
                        "realpow cannot produce complex result." );
      return  pow(a0, a1);
    }
  };
} } }

#endif