//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_CORE_FUNCTIONS_EXPR_RELATIVE_INDEX_HPP_INCLUDED
#define NT2_CORE_FUNCTIONS_EXPR_RELATIVE_INDEX_HPP_INCLUDED

#include <nt2/core/functions/relative_index.hpp>
#include <nt2/include/functions/run.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/core/functions/colon.hpp>
#include <nt2/core/container/extremum/category.hpp>
#include <nt2/core/container/colon/category.hpp>
#include <nt2/core/functions/details/colon.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>
#include <boost/simd/sdk/simd/meta/vector_of.hpp>

namespace nt2 { namespace ext
{
  //============================================================================
  // When indexing an expression, evaluate a type with right cardinal then convert
  //============================================================================
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::relative_index_, tag::cpu_
                            , (A0)(A1)(A2)(A3)(A4)
                            , ((ast_<A0, nt2::container::domain>))
                              (scalar_< unspecified_<A1> >)
                              (scalar_< unspecified_<A2> >)
                              (generic_< unspecified_<A3> >)
                              (target_< unspecified_<A4> >)
                            )
  {
    typedef typename A4::type result_type;
    typedef typename boost::simd::meta::
            vector_of< typename A0::value_type
                     , meta::cardinal_of<result_type>::value
                     >::type value_type;

    BOOST_DISPATCH_FORCE_INLINE result_type
    operator()(const A0& idx, const A1& bi, const A2&, const A3& p, const A4&) const
    {
      return nt2::splat<result_type>(nt2::run(idx,p,meta::as_<value_type>())-bi);
    }
  };

  //============================================================================
  // When indexing a scalar, evaluate as scalar type of expression then convert
  //============================================================================
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::relative_index_, tag::cpu_
                            , (A0)(Arity)(A1)(A2)(A3)(A4)
                            , ((expr_ < scalar_< unspecified_<A0> >
                                      , nt2::tag::terminal_
                                      , Arity
                                      >
                              ))
                              (scalar_< unspecified_<A1> >)
                              (scalar_< unspecified_<A2> >)
                              (generic_< unspecified_<A3> >)
                              (target_< unspecified_<A4> >)
                            )
  {
    typedef typename A4::type result_type;

    BOOST_DISPATCH_FORCE_INLINE result_type
    operator()(const A0& idx, const A1& bi, const A2&, const A3&, const A4&) const
    {
      return nt2::splat<result_type>(nt2::run(idx,0u,meta::as_<typename A0::value_type>())-bi);
    }
  };

  //============================================================================
  // When indexing begin_/end_ +/- i, evaluate to proper extremum
  //============================================================================
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::relative_index_, tag::cpu_
                            , (A0)(Arity)(A1)(A2)(A3)(A4)
                            , ((expr_ < extremum_< A0 >
                                      , nt2::tag::terminal_
                                      , Arity
                                      >
                              ))
                              (scalar_< unspecified_<A1> >)
                              (scalar_< unspecified_<A2> >)
                              (generic_< unspecified_<A3> >)
                              (target_< unspecified_<A4> >)
                            )
  {
    typedef typename A4::type result_type;

    BOOST_DISPATCH_FORCE_INLINE result_type
    operator()(const A0& idx, const A1& bi, const A2& sz, const A3&, const A4&) const
    {
      return nt2::splat<result_type>(boost::proto::value(idx).index(0u,sz));
    }
  };

  //============================================================================
  // When indexing on _, return the consecutive positions
  //============================================================================
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::relative_index_, tag::cpu_
                            , (A0)(Arity)(A1)(A2)(A3)(A4)
                            , ((expr_ < colon_< A0 >
                                      , nt2::tag::terminal_
                                      , Arity
                                      >
                              ))
                              (scalar_< unspecified_<A1> >)
                              (scalar_< unspecified_<A2> >)
                              (generic_< unspecified_<A3> >)
                              (target_< unspecified_<A4> >)
                            )
  {
    typedef typename A4::type result_type;

    BOOST_DISPATCH_FORCE_INLINE result_type
    operator()(const A0&, const A1&, const A2&, const A3& p, const A4&) const
    {
      return p;
    }
  };

  //============================================================================
  // When indexing on _(a, b) with an extremum, return the current offset
  //============================================================================
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::relative_index_, tag::cpu_
                            , (A0)(A1)(A2)(A3)(A4)
                            , ((node_ < A0
                                      , nt2::tag::relative_colon_
                                      , boost::mpl::long_<1>
                                      ,nt2::container::domain>
                              ))
                              (scalar_< unspecified_<A1> >)
                              (scalar_< unspecified_<A2> >)
                              (generic_< unspecified_<A3> >)
                              (target_< unspecified_<A4> >)
                            )
  {
    typedef typename A4::type result_type;

    BOOST_DISPATCH_FORCE_INLINE result_type
    operator()(const A0& i, const A1& b, const A2& s, const A3& p, const A4&) const
    {
      return  details::
              unity_colon_value ( boost::proto::
                                  value(boost::proto::child_c<0>(i)).lower(b,s)
                                , p
                                , meta::as_<result_type>()
                                ) - b;
    }
  };

  //============================================================================
  // When indexing on _(a, s, b) with an extremum, return the current offset
  //============================================================================
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::relative_index_, tag::cpu_
                            , (A0)(A1)(A2)(A3)(A4)
                            , ((node_ < A0
                                      , nt2::tag::relative_colon_
                                      , boost::mpl::long_<2>
                                      ,nt2::container::domain>
                              ))
                              (scalar_< unspecified_<A1> >)
                              (scalar_< unspecified_<A2> >)
                              (generic_< unspecified_<A3> >)
                              (target_< unspecified_<A4> >)
                            )
  {
    typedef typename A4::type result_type;

    BOOST_DISPATCH_FORCE_INLINE result_type
    operator()(const A0& i, const A1& b, const A2& s, const A3& p, const A4&) const
    {
      return  details::
              colon_value ( boost::proto::
                            value(boost::proto::child_c<0>(i)).lower(b,s)
                          , boost::proto::value(boost::proto::child_c<1>(i))
                          , p
                          , meta::as_<result_type>()
                          ) - b;
    }
  };
} }

#endif


/*! *****************************************************************************
 *   \file libSampler.cpp
 *   \author moennen
 *   \brief
 *   \date 2018-04-06
 *   *****************************************************************************/

#include "sampleBuffDataset/libBuffDatasetSampler.h"

#include "utils/cv_utils.h"
#include "utils/Hop.h"

#include <boost/filesystem.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>

#include <glm/glm.hpp>

#include <fstream>
#include <string>
#include <vector>
#include <array>

#include <ctime>
#include <iostream>
#include <map>

using namespace std;
using namespace cv;
using namespace glm;

#define TRACE cout << __PRETTY_FUNCTION__ << "@" << __LINE__ << endl;

namespace
{
struct Sampler final
{
   boost::random::mt19937 _rng;
   boost::random::uniform_int_distribution<> _pathGen;
   boost::random::uniform_real_distribution<> _transGen;

   std::vector<std::string> _paths;
   const ivec3 _sampleSz;
   const bool _toLinear;
   const float _downsample;

   Sampler(
       const char* dataSetPath,
       const char* dataPath,
       const float downsampleFactor,
       const ivec3 sampleSz,
       const bool toLinear,
       const int seed )
       : _rng( seed ),
         _transGen( 0.0, 1.0 ),
         _sampleSz( sampleSz ),
         _toLinear( toLinear ),
         _downsample( downsampleFactor )
   {
      HOP_PROF_FUNC();

      const boost::filesystem::path rootPath( dataPath );
      std::ifstream ifs( dataSetPath );
      if ( ifs.is_open() )
      {
         _paths.reserve( 75000 );
         std::string line;
         while ( ifs.good() )
         {
            getline( ifs, line );
            const boost::filesystem::path f( rootPath / boost::filesystem::path( line ) );
            if ( boost::filesystem::is_regular_file( f ) ) _paths.emplace_back( f.string() );
         }
         _paths.shrink_to_fit();
      }
      _pathGen = boost::random::uniform_int_distribution<>( 0, _paths.size() - 1 );
   }

   bool sample( float* buff )
   {
      HOP_PROF_FUNC();

      const unsigned buffSz = _sampleSz.z * _sampleSz.y * 3;
      float* currBuffHD = buff;
      float* currBuffLD = buff + buffSz * _sampleSz.x;

      for ( size_t s = 0; s < _sampleSz.x; ++s )
      {
         const std::string& iname = _paths[_pathGen( _rng )];

         Mat inputImg = cv_utils::imread32FC3( iname, _toLinear );
         ivec2 imgSz( inputImg.cols, inputImg.rows );

         // ignore too small samples
         if ( ( imgSz.x < _sampleSz.y ) || ( imgSz.y < _sampleSz.z ) )
         {
            --s;
            continue;
         }

         // random rescale
         const float ds =
             mix( 1.0f,
                  std::max( (float)_sampleSz.z / imgSz.y, (float)_sampleSz.y / imgSz.x ),
                  _transGen( _rng ) );
         resize( inputImg, inputImg, Size(), ds, ds, CV_INTER_AREA );
         imgSz = ivec2( inputImg.cols, inputImg.rows );

         // random translate
         const ivec2 trans(
             std::floor( _transGen( _rng ) * ( imgSz.x - _sampleSz.y ) ),
             std::floor( _transGen( _rng ) * ( imgSz.y - _sampleSz.z ) ) );

         // crop
         inputImg = inputImg( Rect( trans.x, trans.y, _sampleSz.y, _sampleSz.z ) );

         // random small blur to remove artifacts
         GaussianBlur( inputImg, inputImg, Size( 3, 3 ), 0.5 * _transGen( _rng ) );

         // bgr 2 rgb
         Mat sampleImg( _sampleSz.z, _sampleSz.y, CV_32FC3, currBuffHD );
         cvtColor( inputImg, sampleImg, COLOR_BGR2RGB );

         // downsample / upsample
         Mat sampleDsImg( _sampleSz.z, _sampleSz.y, CV_32FC3, currBuffLD );
         Mat tmpDsImg;
         resize( sampleImg, tmpDsImg, Size(), _downsample, _downsample, CV_INTER_AREA );
         resize( tmpDsImg, sampleDsImg, Size( _sampleSz.y, _sampleSz.z ), 0, 0, CV_INTER_LINEAR );

         currBuffHD += buffSz;
         currBuffLD += buffSz;
      }

      return true;
   }

   size_t nSamples() const { return _paths.size(); }
   ivec3 sampleSizes() const { return _sampleSz; }
};

array<unique_ptr<Sampler>, 33> g_samplers;
};

extern "C" int getNbBuffers( const int /*sidx*/ ) { return 2; }

extern "C" int getBuffersDim( const int sidx, float* dims )
{
   HOP_PROF_FUNC();

   if ( !g_samplers[sidx].get() ) return ERROR_UNINIT;

   ivec3 sz = g_samplers[sidx]->sampleSizes();
   dims[0] = sz.y;
   dims[1] = sz.z;
   dims[2] = 3;
   dims[3] = sz.y;
   dims[4] = sz.z;
   dims[5] = 3;

   return SUCCESS;
}

extern "C" int initBuffersDataSampler(
    const int sidx,
    const char* datasetPath,
    const char* dataPath,
    const int nParams,
    const float* params,
    const int seed )
{
   HOP_PROF_FUNC();

   // check input
   if ( ( nParams < 4 ) || ( sidx > g_samplers.size() ) ) return ERROR_BAD_ARGS;

   // parse params
   const ivec3 sz( params[0], params[1], params[2] );
   const float downsampleFactor = params[3];
   const bool toLinear( nParams > 4 ? params[4] > 0.0 : false );
   g_samplers[sidx].reset(
       new Sampler( datasetPath, dataPath, downsampleFactor, sz, toLinear, seed ) );

   return g_samplers[sidx]->nSamples() ? SUCCESS : ERROR_BAD_DB;
}

extern "C" int getBuffersDataSample( const int sidx, float* buff )
{
   HOP_PROF_FUNC();

   if ( !g_samplers[sidx].get() ) return ERROR_UNINIT;

   // sample
   if ( !g_samplers[sidx]->sample( buff ) ) return ERROR_GENERIC;

   return SUCCESS;
}


/*!
 * Copyright (c) 2021 Microsoft Corporation. All rights reserved.
 * Licensed under the MIT License. See LICENSE file in the project root for
 * license information.
 */
#include "TransformProcessor.h"

#include <ctype.h>
#include <math.h>

#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/lexical_cast.hpp>
#include <cctype>
#include <chrono>
#include <codecvt>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <list>
#include <map>
#include <sstream>
#include <string>
#include <vector>

#include "common.h"
#include "log.h"

using namespace std;
using namespace DynamicRank;

size_t UIntParseFast(string value) {
  size_t num = 0;
  for (int index = 0; index < value.length(); ++index) {
    num = (size_t)(10 * (int)num + ((int)value[index] - 48));
  }
  return num;
}

double TransformProcessor::ConvertStrToDouble(string val) {
  if (val.empty() || val == "0" || val == "") return 0;
  if (val.find("E") == string::npos) return std::stod(val);
  // case scientific notation.
  vector<string> str_array;
  boost::split(str_array, val, boost::is_any_of("E"), boost::token_compress_on);
  double num1 = std::stod(str_array[0]);
  double num2 = pow(10.0, std::stod((str_array[1])));
  return num1 * num2;
}

bool TransformProcessor::CloseToZero(double val) {
  // Set an EPS to deal with the situation of "-0.00000" and make the result
  // consistent with the previous version.
  if ((val < EPS && val > -EPS) || (val < 0.001 && val > 0.0)) return true;
  return false;
}

vector<FeatureEvaluatorExtendedInfo> GetEvaluators(
    IniFileParserInterface *input_ini_interface,
    std::map<string, int> column_index_mapping) {
  vector<FeatureEvaluatorExtendedInfo> evaluator_extended_info_list;
  for (int i = 0; i < input_ini_interface->GetInputCount(); i++) {
    FeatureEvaluator *evaluator = new FeatureEvaluator(input_ini_interface, i);
    if (evaluator->IsRawFeatureEvaluator()) {
      int num = -1;
      string transform_str = "linear.";
      string key =
          input_ini_interface->GetInputName(i).substr(transform_str.length());
      if (column_index_mapping.find(key) != column_index_mapping.end()) {
        num = column_index_mapping.find(key)->second;
      } else {
        Log::Warning(
            "Feature cannot be found!\n\tFeature: %s\n\tInput expression: %s",
            key.c_str(), input_ini_interface->GetInputName(i).c_str());
      }

      FeatureEvaluatorExtendedInfo evaluator_extended_info;
      evaluator_extended_info.feature_evaluator_ = evaluator;
      evaluator_extended_info.featuremap_ = NULL;
      evaluator_extended_info.required_feature_mapped_indexes_ = vector<int>();
      evaluator_extended_info.index_ = num;
      evaluator_extended_info.israw_ = true;
      evaluator_extended_info_list.push_back(evaluator_extended_info);
    } else {
      string transform_str = "freeform2.";
      string key =
          input_ini_interface->GetInputName(i).substr(transform_str.length());
      IniFileParserInterface *from_freeform2 =
          IniFileParserInterface::Createfromfreeform2(key);
      FeatureEvaluator *evaluator = new FeatureEvaluator(from_freeform2, 0);
      FeatureMap *featuremap = new FeatureMap(from_freeform2);
      vector<int> intList;
      for (int i = 0; i < evaluator->GetRequiredRawFeatureIndices().size();
           i++) {
        int requiredRawFeatureIndex =
            evaluator->GetRequiredRawFeatureIndices()[i];
        int num = -1;
        string rawFeatureName =
            featuremap->GetRawFeatureName(requiredRawFeatureIndex);
        if (column_index_mapping.find(rawFeatureName) !=
            column_index_mapping.end()) {
          num = column_index_mapping.find(rawFeatureName)->second;
        } else {
          Log::Warning(
              "Feature cannot be found!\n\tFeature: %s\n\tInput expression: %s",
              rawFeatureName.c_str(), key.c_str());
        }
        intList.push_back(num);
      }
      FeatureEvaluatorExtendedInfo evaluator_extended_info;
      evaluator_extended_info.feature_evaluator_ = evaluator;
      evaluator_extended_info.featuremap_ = featuremap;
      evaluator_extended_info.required_feature_mapped_indexes_ = intList;
      evaluator_extended_info.index_ = -1;
      evaluator_extended_info.israw_ = false;
      evaluator_extended_info_list.push_back(evaluator_extended_info);
    }
  }
  return evaluator_extended_info_list;
}

TransformProcessor::TransformProcessor(const std::string &transform_str,
                                       const std::string &header_str,
                                       int label_id) {
  auto t0 = std::chrono::high_resolution_clock::now();
  this->_label_id = label_id;
  if (label_id < 0) {
    Log::Warning("Label id %d, found no label.", label_id);
  }
  this->_from_input_str =
      IniFileParserInterface::CreateFromInputStr(transform_str);
  // column Index mapping
  header_str = Common::Trim(header_str);
  string token;
  size_t pos = 0;
  size_t index = 0;
  string delimiter = "\t";
  while ((pos = header_str.find(delimiter)) != string::npos) {
    token = header_str.substr(0, pos);
    this->_column_index_mapping.insert(
        std::pair<string, int>(header_str.substr(0, pos), index));
    index++;
    header_str.erase(0, pos + delimiter.length());
  }
  this->_column_index_mapping.insert(std::pair<string, int>(header_str, index));
  this->_feature_evaluator_list =
      GetEvaluators(this->_from_input_str, this->_column_index_mapping);
  // time
  auto t1 = std::chrono::high_resolution_clock::now();
  Log::Info("Initialize transform time: %.2f seconds",
            std::chrono::duration<double, std::milli>(t1 - t0) * 1e-3);
}

void TransformProcessor::Parse(const char *str,
                               vector<string> *out_feature_strs,
                               double *out_label, string delimiter) {
  string data_line = str;
  data_line = Common::Trim(data_line);
  boost::split(*out_feature_strs, data_line, boost::is_any_of(delimiter));
  if (this->_label_id >= 0 and this->_label_id < out_feature_strs->size()) {
    *out_label = ConvertStrToDouble((*out_feature_strs)[this->_label_id]);
  }
}

void TransformProcessor::Apply(vector<string> *input_row,
                               vector<pair<int, double> > *out_features) {
  int num1 = 0;
  out_features->clear();
  for (vector<FeatureEvaluatorExtendedInfo>::iterator feature_evaluator =
           this->_feature_evaluator_list.begin();
       feature_evaluator != this->_feature_evaluator_list.end();
       feature_evaluator++) {
    double result = 0;
    if (feature_evaluator->israw_) {
      if (feature_evaluator->index_ > -1 &&
          feature_evaluator->index_ < input_row->size()) {
        result = ConvertStrToDouble((*input_row)[feature_evaluator->index_]);
      }
    } else {
      size_t inputsize =
          feature_evaluator->required_feature_mapped_indexes_.size();
      uint32_t *input = new uint32_t[inputsize];
      int index = 0;
      string str2 = "";
      for (int i = 0;
           i < feature_evaluator->required_feature_mapped_indexes_.size();
           i++) {
        input[index] = 0u;
        int feature_mapped_index =
            feature_evaluator->required_feature_mapped_indexes_[i];
        if (feature_mapped_index != -1 &&
            feature_mapped_index < input_row->size()) {
          str2 = (*input_row)[feature_mapped_index];
          if (str2.empty()) {
            input[index] = 0;
          }
          if (!str2.empty()) {
            input[index] = UIntParseFast(str2);
          }
        }
        ++index;
      }

      result = feature_evaluator->feature_evaluator_->Evaluate(&input[0]);
      delete[] input;

      if (std::isinf(result) && result < 0) {
        result = -3.40282346638529E+38;
      } else if (std::isinf(result) && result > 0) {
        result = 3.40282346638529E+38;
      } else if (std::isnan(result)) {
        result = 0.0;
      }
    }
    if (!CloseToZero(result)) {
      out_features->push_back(std::make_pair(num1, result));
    }
    num1++;
  }
}

int TransformProcessor::GetFeatureCount() {
  return this->_feature_evaluator_list.size();
}

vector<TransformedData> TransformProcessor::ApplyForFile(string data_path) {
  ifstream data_fin(data_path.c_str());
  string data_line;
  vector<TransformedData> dataset;
  while (std::getline(data_fin, data_line)) {
    double label;
    vector<string> input_row;
    vector<pair<int, double> > sparse_features;
    this->Parse(data_line.c_str(), &input_row, &label);
    this->Apply(&input_row, &sparse_features);
    dataset.push_back(TransformedData(label, sparse_features));
  }
  return dataset;
}

// Default: precision=5
string TransformedData::ToString(int precision) {
  string output_str = "";
  output_str += to_string((int)this->Label()) + " ";
  for (auto f : this->Features()) {
    std::stringstream ss;
    ss << f.first << ":" << setiosflags(ios::fixed)
       << std::setprecision(precision) << f.second << " ";
    output_str += ss.str();
  }
  return output_str;
}

// Default: precision=5
vector<string> TransformedData::ToString(vector<TransformedData> dataset,
                                         int precision) {
  vector<string> output_vector;
  for (auto d : dataset) output_vector.push_back(d.ToString(precision));
  return output_vector;
}

const double TransformProcessor::EPS = 1E-9;


//==================================================================================================
/*!
  @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_SIMD_FUNCTION_DIV_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_DIV_HPP_INCLUDED

#include <boost/simd/function/divides.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/detail/dispatch/hierarchy.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD_IF ( div_
                          , (typename T, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<T>, X>
                          , bs::pack_<bd::arithmetic_<T>, X>
                          )
  {
    BOOST_MAYBEINLINE T operator()(T const& a, T const& b ) const BOOST_NOEXCEPT
    {
      return divides(a, b);
    }
  };

} } }

#endif


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

/// \copyright Copyright 2021 Apex.AI, Inc.
/// All rights reserved.

#include <state_estimation_nodes/kalman_filter_wrapper.hpp>

#include <common/types.hpp>
#include <kalman_filter/esrcf.hpp>
#include <motion_model/constant_acceleration.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/time.hpp>
#include <state_estimation_nodes/measurement.hpp>
#include <state_estimation_nodes/measurement_typedefs.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <algorithm>
#include <cstdint>
#include <limits>

namespace
{
constexpr auto kCovarianceMatrixRows = 6U;
constexpr auto kIndexX = 0U;
constexpr auto kIndexY = kCovarianceMatrixRows + 1U;
constexpr auto kCovarianceMatrixRowsSquared = kCovarianceMatrixRows * kCovarianceMatrixRows;
static_assert(
  std::tuple_size<
    geometry_msgs::msg::PoseWithCovariance::_covariance_type>::value ==
  kCovarianceMatrixRowsSquared, "We expect the covariance matrix to have 36 entries.");

/// Convert a chrono timepoint to ros time.
rclcpp::Time to_ros_time(const std::chrono::system_clock::time_point & time_point)
{
  using std::chrono::duration_cast;
  using std::chrono::nanoseconds;
  return rclcpp::Time{duration_cast<nanoseconds>(time_point.time_since_epoch()).count()};
}

}  // namespace

namespace autoware
{
namespace prediction
{

using motion::motion_model::ConstantAcceleration;
using common::types::float64_t;
using common::types::bool8_t;

template<typename MotionModelT, std::int32_t kNumOfStates, int32_t kProcessNoiseDim>
void KalmanFilterWrapper<MotionModelT, kNumOfStates, kProcessNoiseDim>::add_reset_event_to_history(
  const VectorT<kNumOfStates> & state,
  const SquareMatrixT<kNumOfStates> & initial_covariance_chol,
  const std::chrono::system_clock::time_point & event_timestamp)
{
  m_history->emplace_event(event_timestamp, ResetEvent<FilterT>{state, initial_covariance_chol});
  m_time_grid = SteadyTimeGrid{event_timestamp, m_expected_prediction_period};
}

template<typename MotionModelT, std::int32_t kNumOfStates, int32_t kProcessNoiseDim>
// cppcheck-suppress syntaxError
template<typename MeasurementT>
void KalmanFilterWrapper<MotionModelT, kNumOfStates, kProcessNoiseDim>::add_reset_event_to_history(
  const MeasurementT & measurement)
{
  add_reset_event_to_history(
    measurement.get_values_in_full_state(m_motion_model.get_state()),
    m_initial_covariance_factor,
    measurement.get_acquisition_time());
}

template<typename MotionModelT, std::int32_t kNumOfStates, int32_t kProcessNoiseDim>
common::types::bool8_t  // We cannot use an alias here as Doxygen thinks its a different signature.
KalmanFilterWrapper<MotionModelT, kNumOfStates,
  kProcessNoiseDim>::add_next_temporal_update_to_history()
{
  if (!is_initialized()) {return false;}
  const auto next_prediction_timestamp =
    m_time_grid.get_next_timestamp_after(m_history->get_last_timestamp());
  m_history->emplace_event(next_prediction_timestamp, PredictionEvent{});
  return true;
}

template<typename MotionModelT, std::int32_t kNumOfStates, int32_t kProcessNoiseDim>
// cppcheck-suppress syntaxError
template<typename MeasurementT>
common::types::bool8_t  // We cannot use an alias here as Doxygen thinks its a different signature.
KalmanFilterWrapper<MotionModelT, kNumOfStates, kProcessNoiseDim>::add_observation_to_history(
  const MeasurementT & measurement)
{
  if (!is_initialized()) {return false;}
  m_history->emplace_event(measurement.get_acquisition_time(), measurement);
  return true;
}

template<typename MotionModelT, std::int32_t kNumOfStates, int32_t kProcessNoiseDim>
nav_msgs::msg::Odometry KalmanFilterWrapper<MotionModelT, kNumOfStates,
  kProcessNoiseDim>::get_state() const
{
  static_assert(
    sizeof(MotionModelT) == -1,
    "You have to have a specialization for get_state() function!");
  // We only throw here because otherwise the linter complaints there is no return value.
  throw std::runtime_error("You have to have a specialization for get_state() function!");
}

template<>
nav_msgs::msg::Odometry ConstantAccelerationFilter::get_state() const
{
  using std::chrono::duration_cast;
  using std::chrono::nanoseconds;
  nav_msgs::msg::Odometry msg{};
  if (!is_initialized()) {
    throw std::runtime_error("Filter not is_initialized, cannot get state.");
  }
  const auto last_event = m_history->get_last_event();
  msg.header.stamp = rclcpp::Time{to_ros_time(m_history->get_last_timestamp())};
  msg.header.frame_id = m_frame_id;
  // Fill state.
  const auto state = last_event.stored_state();
  msg.pose.pose.position.x =
    static_cast<float64_t>(state[ConstantAcceleration::States::POSE_X]);
  msg.pose.pose.position.y =
    static_cast<float64_t>(state[ConstantAcceleration::States::POSE_Y]);
  msg.twist.twist.linear.x =
    static_cast<float64_t>(state[ConstantAcceleration::States::VELOCITY_X]);
  msg.twist.twist.linear.y =
    static_cast<float64_t>(state[ConstantAcceleration::States::VELOCITY_Y]);

  const auto rotation_around_z = std::atan2(msg.twist.twist.linear.y, msg.twist.twist.linear.x);
  tf2::Quaternion rotation;
  rotation.setRPY(0.0, 0.0, rotation_around_z);
  msg.pose.pose.orientation = tf2::toMsg(rotation);

  // Fill covariances.
  const auto & covariance_factor = last_event.stored_covariance_factor();
  const auto covariance = covariance_factor * covariance_factor.transpose();
  msg.pose.covariance[kIndexX] = static_cast<double>(covariance(
      ConstantAcceleration::States::POSE_X, ConstantAcceleration::States::POSE_X));
  msg.pose.covariance[kIndexY] = static_cast<double>(covariance(
      ConstantAcceleration::States::POSE_Y, ConstantAcceleration::States::POSE_Y));
  msg.twist.covariance[kIndexX] = static_cast<double>(covariance(
      ConstantAcceleration::States::VELOCITY_X, ConstantAcceleration::States::VELOCITY_X));
  msg.twist.covariance[kIndexY] = static_cast<double>(covariance(
      ConstantAcceleration::States::VELOCITY_Y, ConstantAcceleration::States::VELOCITY_Y));
  // TODO(igor): do we need elements off the diagonal?
  return msg;
}

/// Excplicit class instantiation.
template class KalmanFilterWrapper<ConstantAcceleration, 6, 2>;

template bool8_t KalmanFilterWrapper<ConstantAcceleration, 6, 2>::add_observation_to_history<>(
  const MeasurementPose &);
template bool8_t KalmanFilterWrapper<ConstantAcceleration, 6, 2>::add_observation_to_history<>(
  const MeasurementSpeed &);
template bool8_t KalmanFilterWrapper<ConstantAcceleration, 6, 2>::add_observation_to_history<>(
  const MeasurementPoseAndSpeed &);

//! @cond Doxygen_Suppress Doxygen is confused about explicit template instantiation.
template void KalmanFilterWrapper<ConstantAcceleration, 6, 2>::add_reset_event_to_history<>(
  const MeasurementPose &);
template void KalmanFilterWrapper<ConstantAcceleration, 6, 2>::add_reset_event_to_history<>(
  const MeasurementSpeed &);
template void KalmanFilterWrapper<ConstantAcceleration, 6, 2>::add_reset_event_to_history<>(
  const MeasurementPoseAndSpeed &);
//! @endcond

}  // namespace prediction
}  // namespace autoware


// -*- Mode: C++; c-basic-offset: 2; indent-tabs-mode: nil; tab-width: 8 -*-

#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/miller/match_multi_indices.h>
#include <boost/python/class.hpp>

namespace cctbx { namespace miller { namespace boost_python {

namespace {

  struct match_multi_indices_wrappers
  {
    typedef match_multi_indices w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("match_multi_indices", no_init)
        .def(init<af::shared<index<> > const&,
                  af::shared<index<> > const&>(
                    (arg_("miller_indices_unique"),
                     arg_("miller_indices"))))
        .def("number_of_matches", &w_t::number_of_matches,
             arg_("i_array"))
        .def("have_singles", &w_t::have_singles)
        .def("pairs", &w_t::pairs)
        .def("singles", &w_t::singles,
             arg_("i_array"))
        .def("pair_selection", &w_t::pair_selection,
             arg_("i_array"))
        .def("single_selection", &w_t::single_selection,
             arg_("i_array"))
        .def("paired_miller_indices", &w_t::paired_miller_indices,
             arg_("i_array"));
    }
  };

} // namespace <anoymous>

  void wrap_match_multi_indices()
  {
    match_multi_indices_wrappers::wrap();
  }

}}} // namespace cctbx::miller::boost_python


/*
 * This file is a part of
 *
 * ============================================
 * ###   Pteros molecular modeling library  ###
 * ============================================
 *
 * (C) 2009-2018, Semen Yesylevskyy
 *
 * All works, which use Pteros, should cite the following papers:
 *
 *  1.  Semen O. Yesylevskyy, "Pteros 2.0: Evolution of the fast parallel
 *      molecular analysis library for C++ and python",
 *      Journal of Computational Chemistry, 2015, 36(19), 1480–1488.
 *      doi: 10.1002/jcc.23943.
 *
 *  2.  Semen O. Yesylevskyy, "Pteros: Fast and easy to use open-source C++
 *      library for molecular analysis",
 *      Journal of Computational Chemistry, 2012, 33(19), 1632–1636.
 *      doi: 10.1002/jcc.22989.
 *
 * This is free software distributed under Artistic License:
 * http://www.opensource.org/licenses/artistic-license-2.0.php
 *
*/

#include "pteros/extras/membrane.h"
#include "pteros/core/pteros_error.h"
#include "pteros/core/distance_search.h"
#include "pteros/core/utilities.h"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <fstream>
#include "voro++.hh"

#ifndef M_PI
    #define M_PI 3.14159265358979323846
#endif

#ifndef M_PI_2
    #define M_PI_2 9.86960440108935712052951
#endif

using namespace std;
using namespace pteros;
using namespace Eigen;

Membrane::Membrane(System *sys, const std::vector<Lipid_descr> &species): system(sys), lipid_species(species)
{
    log = create_logger("membrane");

    Selection all_mid_sel(*system);

    // Creating selections
    for(auto& sp: lipid_species){
        vector<Selection> res;
        system->select(sp.whole_sel_str).split_by_residue(res);
        log->info("Lipid {}: {}", sp.name,res.size());
        for(auto& lip: res){
            auto mol = Lipid(lip,sp);
            mol.set_markers();
            all_mid_sel.append(mol.mid_sel.index(0));
            lipids.push_back(mol);
            index_map[mol.mid_sel.index(0)] = lipids.size()-1;
        }
    }

    // Compute connectivity
    std::vector<Selection> leafs;
    all_mid_sel.split_by_connectivity(2.0,leafs,true);
    leaflets.resize(leafs.size());
    leaflets_sel.resize(leafs.size());
    // Set lipid leaflets
    for(int i=0;i<leafs.size();++i){
        leaflets_sel[i].set_system(*system);
        for(int a=0; a<leafs[i].size(); ++a){
            int ind = leafs[i].index(a);
            lipids[index_map[ind]].leaflet = i;
            leaflets[i].push_back(index_map[ind]);
            leaflets_sel[i].append(ind);
        }
    }

    // Print statictics
    log->info("Total number of lipids: {}",lipids.size());
    log->info("Number of leaflets: {}",leafs.size());
}

void fit_quad_surface(const MatrixXf& coord,
                      Matrix<float,6,1>& res,
                      Vector3f& smoothed_first_point,
                      float& rms){
    int N = coord.cols();
    // We fit with polynomial fit = A*x^2 + B*y^2 + C*xy + D*x + E*y + F
    // Thus we need a linear system of size 6
    Matrix<float,6,6> m;
    Matrix<float,6,1> rhs; // Right hand side and result

    // Functions returning powers after coefficients (x^2, y^2, x*y, x, y, 1)
    vector<std::function<float(int)> > coef(6);
    coef[0] = [&coord](int j){return coord.col(j)(0)*coord.col(j)(0);};
    coef[1] = [&coord](int j){return coord.col(j)(1)*coord.col(j)(1);};
    coef[2] = [&coord](int j){return coord.col(j)(0)*coord.col(j)(1);};
    coef[3] = [&coord](int j){return coord.col(j)(0);};
    coef[4] = [&coord](int j){return coord.col(j)(1);};
    coef[5] = [&coord](int j){return 1.0;};

    // Now form the matrix
    m.fill(0.0);
    rhs.fill(0.0);

    for(int r=0;r<6;++r){ //rows
        for(int c=0;c<6;++c){ //columns
            m(r,c) = 0.0;
            for(int j=0;j<N;++j){
                m(r,c) += coef[r](j)*coef[c](j);
            }
        }
        // Now rhs
        for(int j=0;j<N;++j){
            rhs(r) += coef[r](j)*coord.col(j)(2);
        }
    }

    // Now solve
    res = m.colPivHouseholderQr().solve(rhs);

    // Compute RMS of fitting
    rms = 0.0;
    for(int j=0;j<N;++j){
        float fit = 0.0;
        for(int r=0;r<6;++r) fit += coef[r](j)*res[r];
        rms += pow(coord.col(j)(2)-fit,2);
    }
    rms = sqrt(rms/float(N));

    // Get smoothed surface point
    smoothed_first_point.fill(0.0);
    for(int r=0;r<6;++r) smoothed_first_point(2) += coef[r](0)*res[r];
}

float compute_area(const Eigen::MatrixXf& coord, double dist, std::vector<int>& neib){
    // Perform Voronoi computation in the tangent plane
    using namespace voro;

    voronoicell_neighbor c;

    // Z dimension of cotainer is 1, so volume=area
    container con(-dist,dist,
                  -dist,dist,
                  -0.5,0.5,
                  10,10,10,false,false,false,8);

    particle_order po;
    // Cycle over particles, first particle is our target
    for(int i=0;i<coord.cols();++i){
        con.put(po, i, coord.col(i)(0), coord.col(i)(1), 0.0); // Z is zero
    }
    c_loop_order clo(con,po);
    if(! clo.start()) return -1.0; // Get first point, which is our target
    if(! con.compute_cell(c,clo) ) return -1.0;

    // Check if any of vertices is on outer bounding box and skip cell if they are
    vector<double> vert;
    c.vertices(0,0,0,vert);
    for(int i=0; i<vert.size(); i+=3){
        //     X                         Y
        if(abs(abs(vert[i])-dist)<=1.0e-5 || abs(abs(vert[i+1])-dist)<=1.0e-5){
            //cout << "| " << vert[i] << " " << vert[i+1] << " " << vert[i+2] << " " << abs(vert[i])-dist << endl;
            return -1.0;
        }
    }

    // Extract neighbors of this particle
    std::vector<int> n;
    c.neighbors(n);
    // Filter out negatives
    neib.clear();
    for(int i: n)
        if(i>0) neib.push_back(i);

/*
    cout << "Npoints: " << coord.cols() << endl;
    // Output the particle positions in gnuplot format
    con.draw_particles("points.gnu");

    // Output the Voronoi cells in gnuplot format
    con.draw_cells_gnuplot("points_v.gnu");
*/
    return c.volume();
}

void get_curvature(const Matrix<float,6,1>& coeffs, float& gaussian_curvature, float& mean_curvature){
    /* Compute the curvatures

        First fundamental form:  I = E du^2 + 2F du dv + G dv^2
        E= r_u dot r_u, F= r_u dot r_v, G= r_v dot r_v

        For us parametric variables (u,v) are just (x,y) in local space.
        Derivatives:
            r_u = {1, 0, 2Ax+Cy+D}
            r_v = {0, 1, 2By+Cx+E}

        In central point x=0, y=0 so:
            r_u={1,0,D}
            r_v={0,1,E}

        Thus: E_ =1+D^2; F_ = D*E; G_ = 1+E^2;

        Second fundamental form: II = L du2 + 2M du dv + N dv2
        L = r_uu dot n, M = r_uv dot n, N = r_vv dot n

        Normal is just  n = {0, 0, 1}
        Derivatives:
            r_uu = {0, 0, 2A}
            r_uv = {0 ,0, C}
            r_vv = {0, 0, 2B}

        Thus: L_ = 2A; M_ = C; N_ = 2B;
        */

    float E_ = 1.0+coeffs[3]*coeffs[3];
    float F_ = coeffs[3]*coeffs[4];
    float G_ = 1.0+coeffs[4]*coeffs[4];

    float L_ = 2.0*coeffs[0];
    float M_ = coeffs[2];
    float N_ = 2.0*coeffs[1];

    //Curvatures:
    gaussian_curvature = (L_*N_-M_*M_)/(E_*G_-F_*F_);
    mean_curvature = 0.5*(E_*N_-2.0*F_*M_+G_*L_)/(E_*G_-F_*F_);
}

void Membrane::compute_properties(float d, bool use_external_normal, Vector3f_const_ref external_pivot, Vector3i_const_ref external_dist_dim)
{
    // Clear everything
    neighbor_pairs.clear();

    Eigen::Matrix3f tr, tr_inv;

    // Set markers for all lipids
    // This unwraps each lipid
    for(auto& l: lipids) l.set_markers();

    // Compute per leaflet properties
    for(int l=0;l<leaflets.size();++l){
        // Get connectivity in this leaflet
        vector<Vector2i> bon;
        search_contacts(d,leaflets_sel[l],bon,false,true);

        // Convert the list of bonds to convenient form
        // atom ==> 1 2 3...
        vector<vector<int> > conn(leaflets_sel[l].size());
        for(int i=0;i<bon.size();++i){
            conn[bon[i](0)].push_back(bon[i](1));
            conn[bon[i](1)].push_back(bon[i](0));
        }

        // Process lipids from this leaflet
        for(int i=0;i<leaflets[l].size();++i){
            // Find current lipid
            Lipid& lip = lipids[leaflets[l][i]];

            // Save local selection for this lipid
            lip.local_sel = leaflets_sel[l].select(conn[i]);

            if(lip.local_sel.size()==0){
                log->warn("Empty locality of lipid {} in leaflet {}! Skipped.",i,l);
                continue;
            }

            // Local coordinate axes
            Matrix3f axes;

            //-----------------------------------
            // Compute normal
            //-----------------------------------

            Vector3f normal;

            if(use_external_normal){
                // Compute external normal as a vector from pivot to COM of mid_sel (set as marker currently)
                // over specified dimensions
                axes.col(2) = (lip.mid_marker-external_pivot).array()*external_dist_dim.cast<float>().array();

                // Find two vectors perpendicular to normal
                if( axes.col(2).dot(Vector3f(1,0,0)) != 1.0 ){
                    axes.col(0) = axes.col(2).cross(Vector3f(1,0,0));
                } else {
                    axes.col(0) = axes.col(2).cross(Vector3f(0,1,0));
                }
                axes.col(1) = axes.col(2).cross(axes.col(0));
            } else {
                // Compute normals from inertia axes
                // Create selection for locality of this lipid including itself
                Selection local_self(lip.local_sel);

                local_self.append(leaflets_sel[l].index(i)); // Add central atom

                // Get inertial axes
                Vector3f moments;
                local_self.inertia(moments,axes,fullPBC); // Have to use periodic variant
                // axes.col(2) will be a normal
            }

            normal = axes.col(2);

            // transformation matrix to local basis
            for(int j=0;j<3;++j) tr.col(j) = axes.col(j).normalized();
            tr_inv = tr.inverse();

            // Need to check direction of the normal
            float ang = angle_between_vectors(normal, lip.head_marker-lip.tail_marker);
            if(ang < M_PI_2){
                lip.normal = normal;
                lip.tilt = ang;
            } else {
                lip.normal = -normal;
                lip.tilt = M_PI-ang;
            }
            lip.normal.normalized();

            //-----------------------------------
            // Smooth and find local curvatures
            //-----------------------------------

            // Create array of local points in local basis
            MatrixXf coord(3,lip.local_sel.size()+1);
            Vector3f c0 = lip.mid_marker; // Real coord of central point - the marker
            coord.col(0) = Vector3f::Zero(); // Local coord of central point is zero
            for(int j=0; j<lip.local_sel.size(); ++j)
                coord.col(j+1) = tr_inv * system->box(0).shortest_vector(c0,lip.local_sel.xyz(j));

            // Fit a quad surface
            Matrix<float,6,1> res;
            Vector3f sm; // smoothed coords of central point
            fit_quad_surface(coord,res,sm,lip.quad_fit_rms);
            // Compute curvatures using quad fit coeefs
            get_curvature(res, lip.gaussian_curvature, lip.mean_curvature);

            // Get smoothed surface point in lab coords
            lip.smoothed_mid_xyz = tr*sm + lip.mid_marker;

            //-----------------------------------
            // Area and neighbours
            //-----------------------------------

            vector<int> neib;
            lip.area = compute_area(coord,10.0,neib);
            // Save coordination number of the lipid
            lip.coord_number = neib.size();

            // Add neighbor pairs. Only use nearest neighbor lipids from area computation
            // use only i<j to avoid adding duplicates
            // If area is -1 skip since this lipid has weird surrounding
            if(lip.area>0){
                int cur_ind = index_map[leaflets_sel[l].index(i)];
                for(int j=0; j<neib.size(); ++j){
                    int n = index_map[lip.local_sel.index(neib[j]-1)];
                    if(cur_ind<n) neighbor_pairs.emplace_back(cur_ind,n);
                }
            }

        } // Over lipids in leaflet

    } // over leaflets

    //-----------------------------------
    // Splay and triangilation
    //-----------------------------------

    // Go over neighbor pairs and compute mean splay
    // Also form neigbhor array as i ==> 1,2,3...
    splay.resize(neighbor_pairs.size());
    neighbors.resize(lipids.size());
    for(int i=0;i<neighbor_pairs.size();++i){
        neighbors[neighbor_pairs[i](0)].push_back(neighbor_pairs[i](1));
        neighbors[neighbor_pairs[i](1)].push_back(neighbor_pairs[i](0));
        // We have to use periodic distance since atoms could be from different images
        Lipid& lip1 = lipids[neighbor_pairs[i](0)];
        Lipid& lip2 = lipids[neighbor_pairs[i](1)];
        Vector3f& n1 = lip1.normal;
        Vector3f& n2 = lip2.normal;

        if(n1.dot(n2)<0) continue;

        auto x= system->box(0).shortest_vector(lip1.mid_marker, lip2.mid_marker);
        float d = x.norm();
        x = x-x.dot(n1)*n1;
        x.normalize();
        Vector3f v1 = (lip1.head_marker-lip1.tail_marker).normalized();
        Vector3f v2 = (lip2.head_marker-lip2.tail_marker).normalized();

        splay[i] = {
                    neighbor_pairs[i](0),
                    neighbor_pairs[i](1),
                    (v2.dot(x)-v1.dot(x)-n2.dot(x)+n1.dot(x))/d
                   };
    }

    //-----------------------------------
    // Order parameter
    //-----------------------------------

    for(auto& lip: lipids){
        // Compute Sz order parameter if the tails are provided
        for(int t=0; t<lip.tail_carbon_indexes.size(); ++t){
            // Go over atoms in tail t
            for(int at=1; at<lip.tail_carbon_indexes[t].size()-1; ++at){
                // Vector from at+1 to at-1
                auto coord1 = system->xyz(lip.tail_carbon_indexes[t][at+1]);
                auto coord2 = system->xyz(lip.tail_carbon_indexes[t][at-1]);
                float ang = angle_between_vectors(coord1-coord2,lip.normal);
                lip.order[t][at-1] = 1.5*pow(cos(ang),2)-0.5;
            }
        }
    }

}

string tcl_arrow(Vector3f_const_ref p1, Vector3f_const_ref p2, float r, string color){
    stringstream ss;
    Vector3f p = (p2-p1)*0.8+p1;
    ss << "draw color " << color << endl;

    ss << "draw cylinder \"" << p1.transpose()*10.0 << "\" ";
    ss << "\"" << p.transpose()*10.0 << "\" radius " << r << endl;

    ss << "draw cone \"" << p.transpose()*10.0 << "\" ";
    ss << "\"" << p2.transpose()*10.0 << "\" radius " << r*3.0 << endl;

    return ss.str();
}

void Membrane::write_vmd_arrows(const string &fname)
{
    ofstream f(fname);
    for(auto& lip: lipids){
        f << tcl_arrow(lip.mid_sel.center(true),lip.mid_sel.center(true)+lip.normal,0.2,"green");
        f << tcl_arrow(lip.tail_sel.center(true),lip.head_sel.center(true),0.2,"red");
    }
    f.close();

}

void Membrane::write_smoothed(const string& fname){
    System out;
    for(auto& l: lipids){
        auto s = out.append(l.mid_sel(0,0));
        s.name(0) = "M";
        s = out.append(l.head_sel(0,0));
        s.xyz(0) = l.smoothed_mid_xyz;
        s.name(0) = "S";
    }
    out().write(fname);
}

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

Lipid::Lipid(const Selection &sel, const Lipid_descr &descr){
    name = descr.name;
    whole_sel = sel;
    head_sel = whole_sel(descr.head_sel_str);
    tail_sel = whole_sel(descr.tail_sel_str);
    mid_sel = whole_sel(descr.mid_sel_str);
    // Fill tail indexes if any
    tail_carbon_indexes.resize(descr.tail_carbon_sels.size());
    order.resize(descr.tail_carbon_sels.size());
    for(int t=0; t<descr.tail_carbon_sels.size(); ++t){
        tail_carbon_indexes[t] = whole_sel(descr.tail_carbon_sels[t]).get_index();
        // Allocate array for order
        order[t].resize(tail_carbon_indexes[t].size()-2);
    }
}

void Lipid::set_markers()
{
    // Unwrap this lipid with leading index of position[0]
    whole_sel.unwrap(fullPBC, mid_sel.index(0)-whole_sel.index(0));

    // Set markers to COM
    head_marker = head_sel.center(true);
    tail_marker = tail_sel.center(true);
    mid_marker = mid_sel.center(true);
}


#include <iostream>
#include <boost/python.hpp>
#include <boost/cstdint.hpp>
#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION
#include <numpy/arrayobject.h>
#include <numpy/ndarrayobject.h>
#include <boost/python/extract.hpp>
#include <boost/python/raw_function.hpp>
#include <boost/numpy.hpp>
#include <Eigen/Eigen>
#include "eigen_numpy.h"
#include "densecrf.h"
#include "objective.h"
#include "optimization.h"
#include "labelcompatibility.h"
#include "crfenergy.h"
#include "pairwise.h"
#include "common.h"

namespace py=boost::python;
namespace np=boost::numpy;

void (DenseCRF::*addPairwiseEnergy)(const Eigen::MatrixXf &,PottsCompatibility *,KernelType,NormalizationType) =  &DenseCRF::addPairwiseEnergy;
void (DenseCRF2D::*addPairwiseEnergy2D)(const Eigen::MatrixXf &,PottsCompatibility *,KernelType,NormalizationType) =  &DenseCRF2D::addPairwiseEnergy;

void (DenseCRF2D::*addPairwiseGaussianPotts)( float , float , PottsCompatibility * , KernelType , NormalizationType ) = &DenseCRF2D::addPairwiseGaussian;
void (DenseCRF2D::*addPairwiseGaussianMatrix)( float , float , MatrixCompatibility * , KernelType , NormalizationType ) = &DenseCRF2D::addPairwiseGaussian;

void (DenseCRF2D::*addPairwiseBilateralPotts)( float , float ,  float , float, float, const unsigned char *,PottsCompatibility * , KernelType , NormalizationType ) = &DenseCRF2D::addPairwiseBilateral;
void (DenseCRF2D::*addPairwiseBilateralMatrix)( float , float ,  float , float,float, const unsigned char*, MatrixCompatibility * , KernelType , NormalizationType ) = &DenseCRF2D::addPairwiseBilateral;

void (DenseCRF::*setUnaryEnergy)(const Eigen::MatrixXf &) = &DenseCRF::setUnaryEnergy;
void (DenseCRF::*setUnaryEnergyLogistic)(const Eigen::MatrixXf &,const Eigen::MatrixXf &) = &DenseCRF::setUnaryEnergy;

void (DenseCRF2D::*setUnaryEnergy2D)(const Eigen::MatrixXf &) = &DenseCRF2D::setUnaryEnergy;
void (DenseCRF2D::*setUnaryEnergyLogistic2D)(const Eigen::MatrixXf &,const Eigen::MatrixXf &) = &DenseCRF2D::setUnaryEnergy;

static const int X = Eigen::Dynamic;

template <typename CType>
py::object addPairwiseBilateralWrapper(py::tuple args, py::dict kwargs)
{
    DenseCRF2D& dcrf = py::extract<DenseCRF2D&>(args[0]);
    np::ndarray img = py::extract<np::ndarray>(args[6]);
    uint8_t * img_data = reinterpret_cast<uint8_t*>(img.get_data());

    dcrf.addPairwiseBilateral(py::extract<float>(args[1]),
                              py::extract<float>(args[2]),
                              py::extract<float>(args[3]),
                              py::extract<float>(args[4]),
                              py::extract<float>(args[5]),
                              img_data,
                              py::extract<CType*>(args[7]),
                              py::extract<KernelType>(args[8]),
                              py::extract<NormalizationType>(args[9]));

    return py::object();
}

NormalizationType identityNorm_(NormalizationType norm) { return norm;}

KernelType identityKernel_(KernelType k) { return k;}

BOOST_PYTHON_MODULE(Pydensecrf)
{
    boost::numpy::initialize();
    SetupEigenConverters();

    py::class_<DenseCRF>("DenseCRF", py::init<int, int>())
        .def("addPairwiseEnergy", addPairwiseEnergy)
        .def("setUnaryEnergy", setUnaryEnergy)
        .def("setUnaryEnergy", setUnaryEnergyLogistic)
        .def("inference", &DenseCRF::inference)
        .def("startInference", &DenseCRF::startInference)
        .def("stepInference", &DenseCRF::stepInference)
        .def("gradient", &DenseCRF::gradient)
        .def("unaryEnergy", &DenseCRF::unaryEnergy)
        .def("pairwiseEnergy", &DenseCRF::pairwiseEnergy)
        .def("klDivergence", &DenseCRF::klDivergence)
        .def("unaryParameters",&DenseCRF::unaryParameters)
        .def("setUnaryParameters",&DenseCRF::setUnaryParameters)
        .def("labelCompatibilityParameters", &DenseCRF::labelCompatibilityParameters)
        .def("setLabelCompatibilityParameters", &DenseCRF::setLabelCompatibilityParameters)
        .def("kernelParameters",&DenseCRF::kernelParameters)
        .def("setKernelParameters",&DenseCRF::setKernelParameters)
        ;

    py::class_<DenseCRF2D,py::bases<DenseCRF> >("DenseCRF2D", py::init<int, int,int>())
        .def("addPairwiseGaussian", addPairwiseGaussianPotts)
        .def("addPairwiseGaussian", addPairwiseGaussianMatrix)
        .def("addPairwiseBilateral",py::raw_function(&addPairwiseBilateralWrapper<PottsCompatibility>, 10))
        .def("addPairwiseBilateral", py::raw_function(&addPairwiseBilateralWrapper<MatrixCompatibility>, 10))
        .def("setUnaryEnergy", setUnaryEnergy2D)
        .def("setUnaryEnergy", setUnaryEnergyLogistic2D)
        .def("addPairwiseEnergy", addPairwiseEnergy2D)
        ;

    py::class_<EnergyFunction,boost::noncopyable>("EnergyFunction",py::no_init);

    py::class_<CRFEnergy,py::bases<EnergyFunction> >(
            "CRFEnergy",py::init<DenseCRF &,const ObjectiveFunction &, int, bool,bool,bool>())
        .def("setL2Norm",&CRFEnergy::setL2Norm)
        .def("gradient",&CRFEnergy::gradient);

    py::def("minimizeLBFGS",&minimizeLBFGS);
    py::def("numericGradient",&numericGradient);
    py::def("gradient",&gradient);
    py::def("gradCheck",&gradCheck);
    py::def("computeFunction",&computeFunction);

    py::enum_<NormalizationType>("NormalizationType")
        .value("NO_NORMALIZATION", NO_NORMALIZATION)
        .value("NORMALIZE_BEFORE",NORMALIZE_BEFORE)
        .value("NORMALIZE_AFTER",NORMALIZE_AFTER)
        .value("NORMALIZE_SYMMETRIC",NORMALIZE_SYMMETRIC);
    py::def("identityNorm",identityNorm_);

    py::enum_<KernelType>("KernelType")
        .value("CONST_KERNEL",CONST_KERNEL)
        .value("DIAG_KERNEL",DIAG_KERNEL)
        .value("FULL_KERNEL",FULL_KERNEL);
    py::def("identityKernel",identityKernel_);

    py::class_<LabelCompatibility,boost::noncopyable>("LabelCompatibility",py::no_init);

    py::class_<PottsCompatibility, py::bases<LabelCompatibility> >("PottsCompatibility",py::init<int>())
        .def("parameters",&PottsCompatibility::parameters);

    py::class_<MatrixCompatibility, py::bases<LabelCompatibility> >("MatrixCompatibility",py::init<const MatrixXf &>())
        .def("parameters",&MatrixCompatibility::parameters);

    py::class_<ObjectiveFunction,boost::noncopyable>("ObjectiveFunction",py::no_init);

    py::class_<IntersectionOverUnion, py::bases<ObjectiveFunction> >("IntersectionOverUnion",py::init<const VectorXs &>())
        .def("evaluate", &IntersectionOverUnion::evaluate);
    py::class_<Hamming, py::bases<ObjectiveFunction> >("Hamming",py::init<const VectorXs &,const VectorXf &>())
        .def("evaluate", &Hamming::evaluate);

    import_array();
}


/***************************************************************************
 *   Copyright (C) Preparatory Commission for the Comprehensive            *
 *   Nuclear-Test-Ban Treaty Organization (CTBTO).                         *
 *                                                                         *
 *   You can redistribute and/or modify this program under the             *
 *   terms of the SeisComP Public License.                                 *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   SeisComP Public License for more details.                             *
 ***************************************************************************/

#define SEISCOMP_COMPONENT Magnitudes/mb_idc

#include "mb_idc_private.h"

#include <seiscomp3/logging/log.h>
#include <seiscomp3/system/environment.h>

#include <boost/thread/mutex.hpp>

#include <cmath> // log10
#include <vector> // log10
#include <cfloat>
#include <fstream>

#define AMP_TYPE "A5/2"
#define MAG_TYPE "mb(IDC)"

#define MINIMUM_DISTANCE 20.0   // in degrees
#define MAXIMUM_DISTANCE 105.0  // in degrees

using namespace std;

namespace {

std::string ExpectedAmplitudeUnit = "nm";

Util::TabValues tableQ;
bool validTableQ = false;
bool readTableQ = false;
boost::mutex mutexTableQ;
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
Magnitude_mb_idc::Magnitude_mb_idc()
: MagnitudeProcessor(MAG_TYPE) {}
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
std::string Magnitude_mb_idc::amplitudeType() const {
	return AMP_TYPE;
}
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
bool Magnitude_mb_idc::setup(const Settings &settings) {
	_Q.reset();

	if ( !MagnitudeProcessor::setup(settings) )
		return false;

	try {
		string tablePath = Environment::Instance()->absolutePath(settings.getString("magnitudes.mb(IDC).Q"));
		size_t p;

		p = tablePath.find("{net}");
		if ( p != string::npos ) {
			tablePath.replace(p, 5, settings.networkCode);
		}

		p = tablePath.find("{sta}");
		if ( p != string::npos ) {
			tablePath.replace(p, 5, settings.stationCode);
		}

		p = tablePath.find("{loc}");
		if ( p != string::npos ) {
			tablePath.replace(p, 5, settings.locationCode);
		}

		SEISCOMP_DEBUG("Read station specific mb(IDC) Q table at %s",
		               tablePath.c_str());

		_Q = new Util::TabValues;
		bool validLocalTable = _Q->read(tablePath);
		if ( !validLocalTable ) {
			_Q.reset();
			SEISCOMP_ERROR("Failed to read A values from: %s", tablePath.c_str());
			return false;
		}
	}
	catch ( ... ) {}

	if ( !_Q ) {
		mutexTableQ.lock();
		if ( !readTableQ ) {
			string tablePath = Environment::Instance()->absolutePath("@DATADIR@/magnitudes/IDC/qfvc.mb");
			SEISCOMP_DEBUG("Read global mb(IDC) Q table at %s", tablePath.c_str());

			validTableQ = tableQ.read(tablePath);
			if ( !validTableQ ) {
				SEISCOMP_ERROR("Failed to read Q values from: %s", tablePath.c_str());
			}

			readTableQ = true;
		}
		else if ( !validTableQ ) {
			SEISCOMP_ERROR("Invalid Q value table");
		}
		mutexTableQ.unlock();
		return validTableQ;
	}

	return true;
}
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
MagnitudeProcessor::Status
Magnitude_mb_idc::computeMagnitude(double amplitude,
                                   const std::string &unit,
                                   double period, double,
                                   double delta, double depth,
                                   const DataModel::Origin *,
                                   const DataModel::SensorLocation *,
                                   const DataModel::Amplitude *,
                                   double &value) {
	if ( !validTableQ )
		return IncompleteConfiguration;

	if ( period <= 0 )
		return PeriodOutOfRange;

	if ( (delta < MINIMUM_DISTANCE) || (delta > MAXIMUM_DISTANCE) )
		return DistanceOutOfRange;

	if ( !convertAmplitude(amplitude, unit, ExpectedAmplitudeUnit) )
		return InvalidAmplitudeUnit;

	double x_1st_deriv, z_1st_deriv, x_2nd_deriv, z_2nd_deriv;
	double interpolated_value;
	int interp_err;

	if ( !tableQ.interpolate(interpolated_value, false, true,
	                         delta, depth, &x_1st_deriv, &z_1st_deriv,
	                         &x_2nd_deriv, &z_2nd_deriv, &interp_err) ) {
		return Error;
	}

	if ( interp_err ) {
		return Error;
	}

	value = correctMagnitude(log10(amplitude / period) + double(interpolated_value));

	return OK;
}
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
REGISTER_MAGNITUDEPROCESSOR(Magnitude_mb_idc, MAG_TYPE);
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>}
}


#ifndef animated_layout_hpp
#define animated_layout_hpp

#include <Eigen/Core>
#include <QLayout>
#include <chrono>
#include <memory>
#include <vector>

class QTimer;

class AnimatedLayout : public QLayout
{
public:
    AnimatedLayout();
    ~AnimatedLayout();

    void         addItem(QLayoutItem* item) override;
    void         addWidget(QWidget* widget);
    int          count() const override;
    QLayoutItem* itemAt(int index) const override;
    QLayoutItem* takeAt(int index) override;

    QSize sizeHint() const override;
    void  setGeometry(const QRect& rect) override;

    void play();
    void stop();

    void setRandomTargetPositions();

private:
    struct ItemWrapper
    {
        QLayoutItem*    item;
        Eigen::Vector2d target_position;
        Eigen::Vector2d current_position;
    };

    std::vector<ItemWrapper> m_item_wrappers;

    double                                m_elapsed_time;
    std::chrono::system_clock::time_point m_previous_time;
    std::chrono::system_clock::time_point m_origin_time;

    std::unique_ptr<QTimer> m_timer;
};

#endif /* animated_layout_hpp */


#include "audio.hpp"

#include <cmath>

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

#include "../snail/audio.hpp"
#include "area.hpp"
#include "character.hpp"
#include "config.hpp"
#include "data/types/type_music.hpp"
#include "data/types/type_sound.hpp"
#include "game.hpp"
#include "log.hpp"
#include "map.hpp"
#include "variables.hpp"

namespace snl_sound = snail::audio::sound;
namespace snl_music = snail::audio::music;

namespace elona
{

namespace
{

constexpr int max_volume = MIX_MAX_VOLUME;

// Channels:
// 0-6: reserved for SEs used very frequently
// 7: temporary(default)
// 8-12: temporary
// 13-16: reserved for weather effect

constexpr int temporary_channels_head = 7;
constexpr int temporary_channels_size = 6;

std::vector<int> soundlist;

optional<data::InstanceId> envwprev = none;
data::InstanceId musicprev{};

void preload_sound_if_needed(const fs::path& filepath, int id)
{
    if (id < temporary_channels_head)
    {
        snl_sound::load(id, filepath);
    }
}

void preload_sounds()
{
    soundlist.resize(temporary_channels_size);

    for (const auto& se : the_sound_db.values())
    {
        preload_sound_if_needed(se.file, se.integer_id);
    }
}

void set_volume(int channel, int volume)
{
    snl_sound::set_volume(channel, volume * g_config.sound_volume() / 8);
}

void snd_inner(
    const SoundData& sound,
    short angle,
    unsigned char dist,
    bool loop,
    bool allow_duplicate)
{
    if (!g_config.sound())
        return;

    int channel = sound.integer_id;
    if (channel > temporary_channels_head)
    {
        if (loop)
        {
            switch (sound.integer_id)
            {
            case 78: channel = 14; break;
            case 79: channel = 15; break;
            case 80: channel = 16; break;
            default: channel = 13; break;
            }
        }
        else
        {
            channel = temporary_channels_head;
            bool found{};
            if (!allow_duplicate)
            {
                for (int i = temporary_channels_head;
                     i < temporary_channels_head + temporary_channels_size;
                     ++i)
                {
                    if (snl_sound::is_playing(i) &&
                        soundlist[i - temporary_channels_head] ==
                            sound.integer_id)
                    {
                        channel = i;
                        found = true;
                        break;
                    }
                }
            }
            if (!found)
            {
                for (int i = temporary_channels_head;
                     i < temporary_channels_head + temporary_channels_size;
                     ++i)
                {
                    if (!snl_sound::is_playing(i))
                    {
                        channel = i;
                        soundlist[i - temporary_channels_head] =
                            sound.integer_id;
                    }
                }
            }
        }
        snl_sound::load(channel, sound.file);
    }

    if (dist != 0)
    {
        snl_sound::set_position(channel, angle, dist);
    }
    else
    {
        // implicitly unregisters distance effect on mixer
        snl_sound::set_position(channel, 0, 0);
    }

    set_volume(channel, sound.volume);
    snl_sound::play(channel, loop ? snail::audio::loop_forever : 1);
}

std::pair<short, unsigned char>
calculate_position(int listener_x, int listener_y, int source_x, int source_y)
{
    // Larger means it takes more distance for sounds to become quiet.
    const constexpr double distance_factor = 7.0;

    double x = static_cast<double>(source_x - listener_x);
    double y = static_cast<double>(source_y - listener_y);

    double angle_raw = std::atan2(y, x);
    if (angle_raw < 0)
    {
        angle_raw += 2 * M_PI;
    }

    angle_raw += M_PI / 2;

    short angle = static_cast<short>(angle_raw * 180.0 / M_PI) % 360;
    int dist_raw = dist(listener_x, listener_y, source_x, source_y);
    double dist_norm =
        boost::math::gamma_p(distance_factor, static_cast<double>(dist_raw));
    unsigned char dist = static_cast<unsigned char>(dist_norm * 255.0);

    return {angle, dist};
}

std::pair<short, unsigned char> calculate_position(const Position& p)
{
    if (!config_get<bool>("core.screen.stereo_sound"))
    {
        return {0, 0};
    }
    if (cdata.player().state() == Character::State::empty)
    {
        return {0, 0};
    }

    return calculate_position(
        cdata.player().position.x, cdata.player().position.y, p.x, p.y);
}

data::InstanceId get_default_music()
{
    optional<data::InstanceId> music_id = none;

    if (area_data[game()->current_map].type == mdata_t::MapType::field)
    {
        return musicprev;
    }
    if (area_data[game()->current_map].type == mdata_t::MapType::town)
    {
        music_id = "core.mcTown1";
    }
    if (area_data[game()->current_map].type == mdata_t::MapType::player_owned)
    {
        music_id = "core.mcHome";
    }
    if (map_data.bgm != 0)
    {
        music_id = the_music_db.get_id_from_integer(map_data.bgm);
    }
    if (mdata_t::is_nefia(area_data[game()->current_map].type))
    {
        static const std::vector<data::InstanceId> choices{
            "core.mcDungeon1",
            "core.mcDungeon2",
            "core.mcDungeon3",
            "core.mcDungeon4",
            "core.mcDungeon5",
            "core.mcDungeon6",
        };
        music_id = choices[game_time().hour() % 6];
    }
    if (area_data[game()->current_map].id == mdata_t::MapId::random_dungeon ||
        area_data[game()->current_map].id == mdata_t::MapId::the_void)
    {
        if (game()->current_dungeon_level ==
            area_data[game()->current_map].deepest_level)
        {
            if (area_data[game()->current_map].has_been_conquered != -1)
            {
                music_id = "core.mcBoss";
            }
        }
    }
    if (game()->current_map == mdata_t::MapId::quest)
    {
        if (game()->executing_immediate_quest_type == 1001)
        {
            music_id = "core.mcBattle1";
        }
        if (game()->executing_immediate_quest_type == 1006)
        {
            music_id = "core.mcVillage1";
        }
        if (game()->executing_immediate_quest_type == 1009)
        {
            music_id = "core.mcCasino";
        }
        if (game()->executing_immediate_quest_type == 1008)
        {
            music_id = "core.mcBoss";
        }
        if (game()->executing_immediate_quest_type == 1010)
        {
            music_id = "core.mcArena";
        }
    }
    if (game()->current_map == mdata_t::MapId::arena)
    {
        music_id = "core.mcArena";
    }
    if (game()->current_map == mdata_t::MapId::larna)
    {
        music_id = "core.mcVillage1";
    }
    if (game()->current_map == mdata_t::MapId::port_kapul)
    {
        music_id = "core.mcTown2";
    }
    if (game()->current_map == mdata_t::MapId::lumiest)
    {
        music_id = "core.mcTown2";
    }
    if (game()->current_map == mdata_t::MapId::yowyn)
    {
        music_id = "core.mcVillage1";
    }
    if (game()->current_map == mdata_t::MapId::derphy)
    {
        music_id = "core.mcTown3";
    }
    if (game()->current_map == mdata_t::MapId::palmia)
    {
        music_id = "core.mcTown4";
    }
    if (game()->current_map == mdata_t::MapId::cyber_dome)
    {
        music_id = "core.mcTown5";
    }
    if (game()->current_map == mdata_t::MapId::noyel)
    {
        music_id = "core.mcTown6";
    }

    if (!music_id ||
        area_data[game()->current_map].type == mdata_t::MapType::world_map)
    {
        static const std::vector<data::InstanceId> choices = {
            "core.mcField1",
            "core.mcField2",
            "core.mcField3",
        };
        music_id = choices[game_date().day() % 3];
    }

    return *music_id;
}

void play_music_inner(data::InstanceId music_id, bool loop)
{
    if (musicprev == music_id)
        return;

    const auto music = the_music_db[music_id];
    if (!music)
    {
        ELONA_ERROR("Audio: cannot load music '" + music_id.get() + "'");
        return;
    }
    if (!fs::exists(music->file))
    {
        ELONA_ERROR(
            "Audio: cannot load file '" + music->file.to_u8string() +
            "' for music '" + music_id.get() + "'");
        return;
    }

    musicprev = music_id;
    stop_music();

    // Since we're using SDL mixer, this should load any file
    // format that mixer was compiled to support.
    snl_music::load(music->file);
    snl_music::set_volume(music->volume * g_config.music_volume() / 8);
    snl_music::play(loop ? snail::audio::loop_forever : 1);
}

} // namespace

/* Sound */

bool audio_init_sound()
{
    bool ok = snl_sound::init();
    if (!ok)
    {
        return false;
    }
    preload_sounds();
    return true;
}

void snd(data::InstanceId sound_id, bool loop, bool allow_duplicate)
{
    auto sound = the_sound_db[sound_id];

    if (!sound)
    {
        ELONA_ERROR("Audio: cannot load sound '" + sound_id.get() + "'");
        return;
    }

    snd_inner(*sound, 0, 0, loop, allow_duplicate);
}

void snd_at(
    data::InstanceId sound_id,
    const Position& p,
    bool loop,
    bool allow_duplicate)
{
    const auto [angle, dist] = calculate_position(p);

    auto sound = the_sound_db[sound_id];

    if (!sound)
    {
        ELONA_ERROR("Audio: cannot load sound '" + sound_id.get() + "'");
        return;
    }

    snd_inner(*sound, angle, dist, loop, allow_duplicate);
}

optional<data::InstanceId> sound_id_for_element(int element_id)
{
    switch (element_id)
    {
    case 50: return data::InstanceId{"core.atk_fire"};
    case 51: return data::InstanceId{"core.atk_ice"};
    case 52: return data::InstanceId{"core.atk_elec"};
    case 59: return data::InstanceId{"core.atk_chaos"};
    case 53: return data::InstanceId{"core.atk_dark"};
    case 58: return data::InstanceId{"core.atk_nerve"};
    case 57: return data::InstanceId{"core.atk_sound"};
    case 54: return data::InstanceId{"core.atk_mind"};
    case 55: return data::InstanceId{"core.atk_poison"};
    case 56: return data::InstanceId{"core.atk_hell"};
    case 63: return data::InstanceId{"core.atk_poison"};
    default: return none;
    }
}

void sound_kill(const Position& position)
{
    static const std::vector<data::InstanceId> sounds{
        "core.kill1",
        "core.kill2",
    };

    snd_at(choice(sounds), position, false, false);
}

void sound_pick_up()
{
    static const std::vector<data::InstanceId> sounds{
        "core.get1",
        "core.get2",
    };

    snd(choice(sounds));
}

void sound_footstep(int foot)
{
    switch (foot % 2)
    {
    case 0: snd("core.foot1a"); break;
    case 1: snd("core.foot1b"); break;
    }
}

void sound_footstep2(int foot)
{
    switch (foot % 3)
    {
    case 0: snd("core.foot2a"); break;
    case 1: snd("core.foot2b"); break;
    case 2: snd("core.foot2c"); break;
    }
}

/* Music */

bool audio_init_music()
{
    return snl_music::init();
}

void sound_play_environmental()
{
    optional<data::InstanceId> env = none;

    if (game()->weather == "core.rain")
    {
        env = "core.bg_rain";
    }
    if (game()->weather == "core.hard_rain")
    {
        env = "core.bg_thunder";
    }
    if (game()->weather == "core.etherwind")
    {
        env = "core.bg_wind";
    }
    if (env != envwprev)
    {
        envwprev = env;
        if (g_config.sound())
        {
            if (env)
            {
                snd(*env, true);
            }
            else
            {
                snl_sound::stop(13);
            }
        }
    }
    if (map_data.indoors_flag == 2)
    {
        set_volume(13, max_volume * 0.8);
    }
    else if (
        game()->current_dungeon_level == 1 ||
        game()->current_map == mdata_t::MapId::shelter_)
    {
        set_volume(13, max_volume * 0.2);
    }
    else
    {
        set_volume(13, max_volume);
    }
    if (game()->current_map == mdata_t::MapId::port_kapul)
    {
        snd("core.bg_sea", true);
    }
    else
    {
        snl_sound::stop(14);
    }
    if (map_data.type == mdata_t::MapType::town)
    {
        snd("core.bg_town", true);
    }
    else
    {
        snl_sound::stop(15);
    }
    if (map_data.play_campfire_sound == 1)
    {
        snd("core.bg_fire", true);
    }
    else
    {
        snl_sound::stop(16);
    }
}

void stop_music()
{
    snl_music::stop();
    snl_music::load(filesystem::dirs::sound() / "gm_on.mid");
    snl_music::play(1);
}

void play_music(bool loop)
{
    play_music(get_default_music(), loop);
}

void play_music(data::InstanceId music_id, bool loop)
{
    sound_play_environmental();

    if (!g_config.music())
        return;

    play_music_inner(music_id, loop);
}

} // namespace elona


#ifndef INCLUDED_RANGECLASSIFICATIONTEST
#include "RangeClassificationTest.h"
#define INCLUDED_RANGECLASSIFICATIONTEST
#endif

// External headers.
#ifndef INCLUDED_BOOST_SHARED_PTR
#include <boost/shared_ptr.hpp>
#define INCLUDED_BOOST_SHARED_PTR
#endif

#ifndef INCLUDED_BOOST_TEST_FLOATING_POINT_COMPARISON
#include <boost/test/floating_point_comparison.hpp>
#define INCLUDED_BOOST_TEST_FLOATING_POINT_COMPARISON
#endif

#ifndef INCLUDED_BOOST_TEST_TEST_TOOLS
#include <boost/test/test_tools.hpp>
#define INCLUDED_BOOST_TEST_TEST_TOOLS
#endif

#ifndef INCLUDED_BOOST_TEST_UNIT_TEST_SUITE
#include <boost/test/unit_test_suite.hpp>
#define INCLUDED_BOOST_TEST_UNIT_TEST_SUITE
#endif

// Project headers.

// Module headers.
#ifndef INCLUDED_RANGECLASSIFICATION
#include "RangeClassification.h"
#define INCLUDED_RANGECLASSIFICATION
#endif

/*!
  \file
  This file contains the implementation of the RangeClassificationTest class.
*/

namespace ag {

//------------------------------------------------------------------------------
// DEFINITION OF STATIC RANGECLASSIFICATIONTEST MEMBERS
//------------------------------------------------------------------------------

//! Suite.
boost::unit_test::test_suite* RangeClassificationTest::suite()
{
  boost::unit_test::test_suite* suite = BOOST_TEST_SUITE(__FILE__);
  boost::shared_ptr<RangeClassificationTest> instance(
         new RangeClassificationTest());
  suite->add(BOOST_CLASS_TEST_CASE(
         &RangeClassificationTest::testDefaultConstructor, instance));
  suite->add(BOOST_CLASS_TEST_CASE(
         &RangeClassificationTest::testLinear, instance));
  suite->add(BOOST_CLASS_TEST_CASE(
         &RangeClassificationTest::testLog10, instance));

  return suite;
}

//------------------------------------------------------------------------------
// DEFINITION OF RANGECLASSIFICATIONTEST MEMBERS
//------------------------------------------------------------------------------

//! Constructor.
RangeClassificationTest::RangeClassificationTest()
{
}

void RangeClassificationTest::testDefaultConstructor()
{
  {
    RangeClassification<float> classification;

    BOOST_REQUIRE_EQUAL(classification.size(), 11u);
    BOOST_REQUIRE_EQUAL(classification.nrClasses(), 10u);

    BOOST_CHECK_CLOSE(classification.minCutoff(), 0.0f, 0.001f);
    BOOST_CHECK_CLOSE(classification.maxCutoff(), 1.0f, 0.001f);

    BOOST_CHECK_CLOSE(classification[3], 0.3f, 0.001f);

    BOOST_CHECK_EQUAL(classification.index(0.3f), 3u);
    BOOST_CHECK_EQUAL(classification.index(0.29f), 2u);
    BOOST_CHECK_EQUAL(classification.index(0.31f), 3u);

    BOOST_CHECK_EQUAL(classification.index(0.0f), 0u);
    BOOST_CHECK_EQUAL(classification.index(1.0f), 10u);
  }
}

void RangeClassificationTest::testLinear()
{
  {
    RangeClassification<float> classification(10.0f, 20.0f, 10,
         RangeClassification<float>::Linear);

    BOOST_REQUIRE_EQUAL(classification.size(), 11u);
    BOOST_REQUIRE_EQUAL(classification.nrClasses(), 10u);

    BOOST_CHECK_CLOSE(classification.minCutoff(), 10.0f, 0.001f);
    BOOST_CHECK_CLOSE(classification.maxCutoff(), 20.0f, 0.001f);

    BOOST_CHECK_CLOSE(classification[3], 13.0f, 0.001f);

    BOOST_CHECK_EQUAL(classification.index(13.0f), 3u);
    BOOST_CHECK_EQUAL(classification.index(12.9f), 2u);
    BOOST_CHECK_EQUAL(classification.index(13.1f), 3u);

    BOOST_CHECK_EQUAL(classification.index(10.0f), 0u);
    BOOST_CHECK_EQUAL(classification.index(20.0f), 10u);
  }
}

void RangeClassificationTest::testLog10()
{
  {
    RangeClassification<float> classification(1.0f, 1000.0f, 4,
         RangeClassification<float>::Log10);

    BOOST_REQUIRE_EQUAL(classification.size(), 4u);
    BOOST_REQUIRE_EQUAL(classification.nrClasses(), 3u);

    BOOST_CHECK_CLOSE(classification.minCutoff(), 1.0f, 0.001f);
    BOOST_CHECK_CLOSE(classification.maxCutoff(), 1000.0f, 0.001f);

    BOOST_CHECK_CLOSE(classification[0], 1.0f, 0.001f);
    BOOST_CHECK_CLOSE(classification[1], 10.0f, 0.001f);
    BOOST_CHECK_CLOSE(classification[2], 100.0f, 0.001f);
    BOOST_CHECK_CLOSE(classification[3], 1000.0f, 0.001f);

    BOOST_CHECK_EQUAL(classification.index(50.0f), 1u);
    BOOST_CHECK_EQUAL(classification.index(5.0f), 0u);
    BOOST_CHECK_EQUAL(classification.index(500.0f), 2u);

    BOOST_CHECK_EQUAL(classification.index(1.0f), 0u);
    BOOST_CHECK_EQUAL(classification.index(1000.0f), 3u);
  }
}

} // namespace ag


#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <boost/program_options.hpp>
#include <v4r/camera_tracking_and_mapping/PoissonTriangulation.hh>

namespace bf = boost::filesystem;
namespace po = boost::program_options;

int main(int argc, char **argv) {
  typedef pcl::PointXYZRGBNormal PointT;
  bf::path input_file, out_dir = ".";
  int depth = 7;
  int samplesPerNode = 2;
  bool cropModel = true;

  po::options_description desc(
      "Generate mesh file from point cloud\n======================================\n**Allowed options");
  desc.add_options()("help,h", "produce help message");
  desc.add_options()("input_file,i", po::value<bf::path>(&input_file)->required(), "File path to input point cloud.");
  desc.add_options()("out_dir,o", po::value<bf::path>(&out_dir)->default_value(out_dir), "Output directory.");
  desc.add_options()("depth,d", po::value<int>(&depth)->default_value(depth), "");
  desc.add_options()("crop,c", po::value<bool>(&cropModel)->default_value(depth), "crop model");
  desc.add_options()("samples,s", po::value<bool>(&cropModel)->default_value(depth), "samples per node");
  po::positional_options_description p;
  p.add("input_file", 1);

  po::variables_map vm;
  po::parsed_options parsed = po::command_line_parser(argc, argv).options(desc).positional(p).run();
  po::store(parsed, vm);
  if (vm.count("help")) {
    std::cout << desc << std::endl;
  }
  try {
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "Error: " << e.what() << std::endl << std::endl << desc << std::endl;
  }

  typename pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
  pcl::io::loadPCDFile(input_file.string(), *cloud);
  v4r::PoissonTriangulation pt(depth, samplesPerNode, cropModel);
  pcl::PolygonMesh mesh;
  pt.reconstruct(cloud, mesh);
  pcl::io::savePLYFile((out_dir / "mesh.ply").string(), mesh);
}


//==================================================================================================
/*!
  @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_FUNCTION_IF_ZERO_ELSE_ALLBITS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IF_ZERO_ELSE_ALLBITS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-boolean
    Function object implementing if_zero_else_allbits capabilities

    Returns a mask of bits. All ones if the
    input element is @ref Zero else all zeros.

    This is a convenience alias of @ref genmaskc
  **/
  Value if_zero_else_allbits(Value const& c);

  //@overload
  Value if_zero_else_allbits(LogicalValue const& c);
} }
#endif

#include <boost/simd/function/scalar/genmaskc.hpp>
#include <boost/simd/function/simd/if_zero_else_allbits.hpp>

#endif