/*
 * Copyright 2017 MapD Technologies, 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.
 */

/**
 * @file		PopulateTableRandom.cpp
 * @author	Wei Hong <wei@map-d.com>
 * @brief		Populate a table with random data
 *
 * Copyright (c) 2014 MapD Technologies, Inc.  All rights reserved.
 **/

#include <boost/functional/hash.hpp>
#include <cfloat>
#include <cstdint>
#include <cstdlib>
#include <cstring>
#include <exception>
#include <iostream>
#include <memory>
#include <random>
#include <string>

#include "../Catalog/Catalog.h"
#include "../DataMgr/DataMgr.h"
#include "../Fragmenter/Fragmenter.h"
#include "../Shared/DateConverters.h"
#include "../Shared/measure.h"
#include "../Shared/sqltypes.h"
#include "Logger/Logger.h"

using namespace Catalog_Namespace;
using namespace Fragmenter_Namespace;

size_t random_fill_int16(int8_t* buf, size_t num_elems) {
  std::default_random_engine gen;
  std::uniform_int_distribution<int16_t> dist(INT16_MIN, INT16_MAX);
  auto p = reinterpret_cast<int16_t*>(buf);
  size_t hash = 0;
  for (size_t i = 0; i < num_elems; i++) {
    p[i] = dist(gen);
    boost::hash_combine(hash, p[i]);
  }
  return hash;
}

size_t random_fill_int32(int8_t* buf, size_t num_elems) {
  std::default_random_engine gen;
  std::uniform_int_distribution<int32_t> dist(INT32_MIN, INT32_MAX);
  auto p = reinterpret_cast<int32_t*>(buf);
  size_t hash = 0;
  for (size_t i = 0; i < num_elems; i++) {
    p[i] = dist(gen);
    boost::hash_combine(hash, p[i]);
  }
  return hash;
}

size_t random_fill_int64(int8_t* buf, size_t num_elems, int64_t min, int64_t max) {
  std::default_random_engine gen;
  std::uniform_int_distribution<int64_t> dist(min, max);
  auto p = reinterpret_cast<int64_t*>(buf);
  size_t hash = 0;
  for (size_t i = 0; i < num_elems; i++) {
    p[i] = dist(gen);
    boost::hash_combine(hash, p[i]);
  }
  return hash;
}

size_t random_fill_int64(int8_t* buf, size_t num_elems) {
  return random_fill_int64(buf, num_elems, INT64_MIN, INT64_MAX);
}

size_t random_fill_float(int8_t* buf, size_t num_elems) {
  std::default_random_engine gen;
  std::uniform_real_distribution<float> dist(FLT_MIN, FLT_MAX);
  auto p = reinterpret_cast<float*>(buf);
  size_t hash = 0;
  for (size_t i = 0; i < num_elems; i++) {
    p[i] = dist(gen);
    boost::hash_combine(hash, p[i]);
  }
  return hash;
}

size_t random_fill_double(int8_t* buf, size_t num_elems) {
  std::default_random_engine gen;
  std::uniform_real_distribution<double> dist(DBL_MIN, DBL_MAX);
  auto p = reinterpret_cast<double*>(buf);
  size_t hash = 0;
  for (size_t i = 0; i < num_elems; i++) {
    p[i] = dist(gen);
    boost::hash_combine(hash, p[i]);
  }
  return hash;
}

size_t random_fill_string(std::vector<std::string>& stringVec,
                          size_t num_elems,
                          int max_len,
                          size_t& data_volumn) {
  std::string chars("abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ1234567890");
  std::default_random_engine gen;
  std::uniform_int_distribution<> char_dist(0, chars.size() - 1);
  std::uniform_int_distribution<> len_dist(0, max_len);
  size_t hash = 0;
  std::hash<std::string> string_hash;
  for (size_t n = 0; n < num_elems; n++) {
    int len = len_dist(gen);
    std::string s(len, ' ');
    for (int i = 0; i < len; i++) {
      {
        s[i] = chars[char_dist(gen)];
      }
    }
    stringVec[n] = s;
    boost::hash_combine(hash, string_hash(s));
    data_volumn += len;
  }
  return hash;
}

size_t random_fill_int8array(std::vector<std::vector<int8_t>>& stringVec,
                             size_t num_elems,
                             int max_len,
                             size_t& data_volumn) {
  std::default_random_engine gen;
  std::uniform_int_distribution<int8_t> dist(INT8_MIN, INT8_MAX);
  std::uniform_int_distribution<> len_dist(0, max_len);
  size_t hash = 0;
  for (size_t n = 0; n < num_elems; n++) {
    int len = len_dist(gen);
    std::vector<int8_t> s(len);
    for (int i = 0; i < len; i++) {
      s[i] = dist(gen);
      boost::hash_combine(hash, s[i]);
    }
    stringVec[n] = s;
    data_volumn += len * sizeof(int8_t);
  }
  return hash;
}

size_t random_fill_int16array(std::vector<std::vector<int16_t>>& stringVec,
                              size_t num_elems,
                              int max_len,
                              size_t& data_volumn) {
  std::default_random_engine gen;
  std::uniform_int_distribution<int16_t> dist(INT16_MIN, INT16_MAX);
  std::uniform_int_distribution<> len_dist(0, max_len / 2);
  size_t hash = 0;
  for (size_t n = 0; n < num_elems; n++) {
    int len = len_dist(gen);
    std::vector<int16_t> s(len);
    for (int i = 0; i < len; i++) {
      s[i] = dist(gen);
      boost::hash_combine(hash, s[i]);
    }
    stringVec[n] = s;
    data_volumn += len * sizeof(int16_t);
  }
  return hash;
}

size_t random_fill_int32array(std::vector<std::vector<int32_t>>& stringVec,
                              size_t num_elems,
                              int max_len,
                              size_t& data_volumn) {
  std::default_random_engine gen;
  std::uniform_int_distribution<int32_t> dist(INT32_MIN, INT32_MAX);
  std::uniform_int_distribution<> len_dist(0, max_len / 4);
  size_t hash = 0;
  for (size_t n = 0; n < num_elems; n++) {
    int len = len_dist(gen);
    std::vector<int32_t> s(len);
    for (int i = 0; i < len; i++) {
      s[i] = dist(gen);
      boost::hash_combine(hash, s[i]);
    }
    stringVec[n] = s;
    data_volumn += len * sizeof(int32_t);
  }
  return hash;
}

size_t random_fill_dates(int8_t* buf, size_t num_elems) {
  constexpr int64_t kDateMin = -185542587187200;
  constexpr int64_t kDateMax = 185542587100800;
  std::default_random_engine gen;
  std::uniform_int_distribution<int64_t> dist(kDateMin, kDateMax);
  auto p = reinterpret_cast<int64_t*>(buf);
  size_t hash = 0;
  for (size_t i = 0; i < num_elems; i++) {
    p[i] = dist(gen);
    boost::hash_combine(hash, DateConverters::get_epoch_days_from_seconds(p[i]));
  }
  return hash;
}

#define MAX_TEXT_LEN 255

size_t random_fill(const ColumnDescriptor* cd,
                   DataBlockPtr p,
                   size_t num_elems,
                   size_t& data_volumn) {
  size_t hash = 0;
  switch (cd->columnType.get_type()) {
    case kSMALLINT:
      hash = random_fill_int16(p.numbersPtr, num_elems);
      data_volumn += num_elems * sizeof(int16_t);
      break;
    case kINT:
      hash = random_fill_int32(p.numbersPtr, num_elems);
      data_volumn += num_elems * sizeof(int32_t);
      break;
    case kBIGINT:
      hash = random_fill_int64(p.numbersPtr, num_elems, INT64_MIN, INT64_MAX);
      data_volumn += num_elems * sizeof(int64_t);
      break;
    case kNUMERIC:
    case kDECIMAL: {
      int64_t max = std::pow((double)10, cd->columnType.get_precision());
      int64_t min = -max;
      hash = random_fill_int64(p.numbersPtr, num_elems, min, max);
      data_volumn += num_elems * sizeof(int64_t);
      break;
    }
    case kFLOAT:
      hash = random_fill_float(p.numbersPtr, num_elems);
      data_volumn += num_elems * sizeof(float);
      break;
    case kDOUBLE:
      hash = random_fill_double(p.numbersPtr, num_elems);
      data_volumn += num_elems * sizeof(double);
      break;
    case kVARCHAR:
    case kCHAR:
      if (cd->columnType.get_compression() == kENCODING_NONE) {
        {
          hash = random_fill_string(
              *p.stringsPtr, num_elems, cd->columnType.get_dimension(), data_volumn);
        }
      }
      break;
    case kTEXT:
      if (cd->columnType.get_compression() == kENCODING_NONE) {
        {
          hash = random_fill_string(*p.stringsPtr, num_elems, MAX_TEXT_LEN, data_volumn);
        }
      }
      break;
    case kDATE:
    case kTIME:
    case kTIMESTAMP:
      hash = cd->columnType.get_type() == kDATE
                 ? random_fill_dates(p.numbersPtr, num_elems)
                 : random_fill_int64(p.numbersPtr, num_elems);
      data_volumn += num_elems * sizeof(int64_t);
      break;
    default:
      assert(false);
  }
  return hash;
}

std::vector<size_t> populate_table_random(const std::string& table_name,
                                          const size_t num_rows,
                                          const Catalog& cat) {
  const TableDescriptor* td = cat.getMetadataForTable(table_name);
  const auto cds = cat.getAllColumnMetadataForTable(td->tableId, false, false, false);
  InsertData insert_data;
  insert_data.databaseId = cat.getCurrentDB().dbId;
  insert_data.tableId = td->tableId;
  for (const auto& cd : cds) {
    insert_data.columnIds.push_back(cd->columnId);
    insert_data.is_default.push_back(false);
  }
  insert_data.numRows = num_rows;
  std::vector<std::vector<int8_t>> numbers_vec;
  std::vector<std::unique_ptr<std::vector<std::string>>> strings_vec;

  DataBlockPtr p{0};
  // now allocate space for insert data
  for (auto cd : cds) {
    if (cd->columnType.is_varlen()) {
      if (cd->columnType.get_compression() == kENCODING_NONE) {
        strings_vec.push_back(std::make_unique<std::vector<std::string>>(num_rows));
        p.stringsPtr = strings_vec.back().get();
      } else {
        CHECK(false);
      }
    } else {
      numbers_vec.emplace_back(num_rows * cd->columnType.get_logical_size());
      p.numbersPtr = numbers_vec.back().data();
    }
    insert_data.data.push_back(p);
  }

  // fill InsertData  with random data
  std::vector<size_t> col_hashs(
      cds.size());  // compute one hash per column for the generated data
  int i = 0;
  size_t data_volumn = 0;
  for (auto cd : cds) {
    col_hashs[i] = random_fill(cd, insert_data.data[i], num_rows, data_volumn);
    i++;
  }

  // now load the data into table
  auto ms = measure<>::execution([&]() { td->fragmenter->insertData(insert_data); });
  std::cout << "Loaded " << num_rows << " rows " << data_volumn << " bytes in " << ms
            << " ms. at " << (double)data_volumn / (ms / 1000.0) / 1e6 << " MB/sec."
            << std::endl;

  return col_hashs;
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/list/mcd.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/detail/minimal/list.hpp>
#include <boost/hana/detail/minimal/product.hpp>

#include <boost/hana/integral.hpp>
using namespace boost::hana;

constexpr auto prod = detail::minimal::product<>;

// partition requires the predicate to return an Integral boolean, so we need
// the comparison to be purely compile-time.
template <int i>
BOOST_HANA_CONSTEXPR_LAMBDA auto x = int_<i>;

BOOST_HANA_CONSTEXPR_LAMBDA auto neg = [](auto x) {
    return x < int_<0>;
};

template <typename mcd>
void test() {
    BOOST_HANA_CONSTEXPR_LAMBDA auto list = detail::minimal::list<mcd>;

    BOOST_HANA_CONSTANT_ASSERT(partition(list(), neg) == prod(list(), list()));
    BOOST_HANA_CONSTANT_ASSERT(partition(list(x<0>), neg) == prod(list(), list(x<0>)));
    BOOST_HANA_CONSTANT_ASSERT(partition(list(x<0>, x<1>), neg) == prod(list(), list(x<0>, x<1>)));
    BOOST_HANA_CONSTANT_ASSERT(partition(list(x<-1>), neg) == prod(list(x<-1>), list()));
    BOOST_HANA_CONSTANT_ASSERT(partition(list(x<-1>, x<0>, x<2>), neg) == prod(list(x<-1>), list(x<0>, x<2>)));
    BOOST_HANA_CONSTANT_ASSERT(partition(list(x<0>, x<-3>, x<2>, x<-5>, x<6>), neg) == prod(list(x<-3>, x<-5>), list(x<0>, x<2>, x<6>)));
    BOOST_HANA_CONSTANT_ASSERT(partition(list(x<-1>, x<2>, x<-3>, x<0>, x<-3>, x<4>), neg) == prod(list(x<-1>, x<-3>, x<-3>), list(x<2>, x<0>, x<4>)));
}

int main() {
    test<List::mcd<void>>();
    (void)neg;
}


// Copyright (C) 2017 Vicente J. Botet Escriba
//
//  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)

// <experimental/strong_counter.hpp>

/**
 * This example tries to see how close we can define std::chrono duration using strong_counter
 *
 * We need a specific Domain for durations to state how durations convert between them.
 */

#include <experimental/strong_counter.hpp>
#include <experimental/type_traits.hpp>
#include <ratio>
#include <chrono>
#include <utility>
#include <type_traits>
#include <experimental/fundamental/v3/strong/mixins/is_compatible_with.hpp>

namespace stdex = std::experimental;

// fixme: Shouldn't all the strong_counter domains require most of these conditions (except the Period related)?
// todo generalize to dimension_1_domain<duration, Period>
// todo generalize to dimensionless_domain<Tag>
// fixme Shouldn't those have a Tag parameter to make them strongly typed?
template<class Period>
struct duration_domain
{
  template <class T, class P, class U
          , typename std::enable_if <
              std::conjunction <
                std::is_convertible<U, T>, // no overflow
                std::disjunction<
                      std::chrono::treat_as_floating_point<T>,
                      std::conjunction<
                          std::integral_constant<bool, std::ratio_divide<P, Period>::den == 1>,
                          std::negation< std::chrono::treat_as_floating_point<U> >
                      >
                  >
              >::value
          >::type* = nullptr
  >
  static T inter_domain_convert(duration_domain<P>, U const& u)
  {
    return T(
        std::chrono::duration_cast<std::chrono::duration<T, Period>>(std::chrono::duration<U,P>(u)).count()
        );
  }
  template <class T, class P, class U>
  static T inter_domain_cast(duration_domain<P>, U const& u)
  {
    return T(
        std::chrono::duration_cast<std::chrono::duration<T, Period>>(std::chrono::duration<U,P>(u)).count()
    );
  }
  template <class T, class U
          , typename std::enable_if <
              std::conjunction <
                std::is_convertible<U, T>,
                std::disjunction<
                      std::chrono::treat_as_floating_point<T>,
                      std::negation< std::chrono::treat_as_floating_point<U> >
                >
              >::value
          >::type* = nullptr
          >
  static T intra_domain_convert(U const& u) { return T(u); }
};

template<
    class Rep,
    class Period = std::ratio<1>
>
using duration = stdex::strong_counter<duration_domain<Period>, Rep>;

// fixme: duration stream insertion/extraction can be more specialized.
// Wondering if streamable, should be part of strong_counter.

namespace std {
  template <class P1, class P2>
  struct common_type<duration_domain<P1>, duration_domain<P2>>
  {
     using CP = typename common_type<std::chrono::duration<int,P1>,std::chrono::duration<int,P2>>::type::period;
     using type = duration_domain<CP>;
  };

namespace experimental {
inline  namespace fundamental_v3{
namespace mixin {
  template <class Domain1, class Domain2>
  struct is_compatible_with<duration_domain<Domain1>, duration_domain<Domain2>> : std::true_type {};
}

  template <class Period>
  struct domain_converter<duration_domain<Period>> : duration_domain<Period>  {  };
}
}
}

static_assert(std::is_same<
                  std::common_type<
                    duration_domain<std::ratio<1,10>>,
                    duration_domain<std::ratio<1,100>>
                  >::type
                , duration_domain<std::ratio<1,100>>
              >::value, "");

#include <boost/detail/lightweight_test.hpp>
#include <iostream>

template <class T> struct check;
int main()
{
#if 0
  {
    using U = int;
    using T = int;
    using P = std::ratio<1,10>;
    using Period = std::ratio<1,10>;

  static_assert(
      stdex::conjunction <
                      std::is_convertible<U, T>, // no overflow
                      stdex::disjunction<
                            std::chrono::treat_as_floating_point<T>,
                            stdex::conjunction<
                                std::integral_constant<bool, std::ratio_divide<P, Period>::den == 1>,
                                stdex::negation< std::chrono::treat_as_floating_point<U> >
                            >
                        >
                    >::value
      , "");
  }

  auto x1 = duration_domain<std::ratio<1,10>>::inter_domain_convert<int>(duration_domain<std::ratio<1,10>>{}, 1);
  auto x2 = stdex::inter_domain_convert<duration_domain<std::ratio<1,10>>, int>(duration_domain<std::ratio<1,10>>{}, 1);
#endif

    {
      duration<int, std::ratio<1,10>> d1 {30};
      auto d2 = d1 ++;
      BOOST_TEST(d1 != d2);
      BOOST_TEST(d1.count() == 31);
      BOOST_TEST(d2.count() == 30);
    }
    {
      duration<int, std::ratio<1,10>> d1 {30};
      auto d2 = ++d1;
      BOOST_TEST(d1 == d2);
      BOOST_TEST(d1.count() == 31);
      BOOST_TEST(d2.count() == 31);
    }
    {
      duration<int, std::ratio<1,10>> d1 {30};
      duration<int, std::ratio<1,10>> d2{d1};
      BOOST_TEST(d2.count() == 30);
    }
    {
      duration<int, std::ratio<1,10>> d1 {30};
      duration<int, std::ratio<1,10>> d2 =d1;
      BOOST_TEST(d2.count() == 30);
    }
    {
      duration<int, std::ratio<1,10>> d1 {30};
      duration<int, std::ratio<1,10>> d2;
      d2 = d1;
      BOOST_TEST(d2.count() == 30);
    }
    {
      duration<int, std::ratio<1,10>> d1 {300};
      duration<int, std::ratio<1,100>> d2  = d1;

      static_assert(
          std::ratio_divide<std::ratio<1,10>, std::ratio<1,100>>::den == 1
          , "");

      static_assert(
          stdex::detail::is_inter_domain_convertible<duration_domain<std::ratio<1,100>>, int, int, duration_domain<std::ratio<1,10>>>::value
          , "");
      static_assert(
          std::ratio_divide<std::ratio<1,100>, std::ratio<1,10>>::den != 1
          , "");

      static_assert(
          ! stdex::detail::is_inter_domain_convertible<duration_domain<std::ratio<1,10>>, int, int, duration_domain<std::ratio<1,100>>>::value
          , "ERROR");

#if defined __clang__ && (__clang_major__ < 4)
      duration<int, std::ratio<1,10>> d3;
      // fixme: This shouldn't compile - clang-3.9.1 BUG
      std::cout << "------- \n";
      d3= d2; // calling move assignment :(
      std::cout << "------- \n";
#endif
#if ! defined __clang__
      // fixme: this doesn't work for clang on travis
      BOOST_TEST(d1 == d2);
#endif
      BOOST_TEST_EQ(d2.count() , 3000);
    }
    {
      duration<int, std::ratio<1,100>> d1 {303};
      duration<int, std::ratio<1,10>> d2  = stdex::strong_counter_cast<duration<int, std::ratio<1,10>>>(d1);
#if defined __clang__ && (__clang_major__ < 4) // clang-3.9.1 BUG
      // fixme: This should compile
      //BOOST_TEST(d1 != d2);
#endif
#if ! defined __clang__
      // fixme: this doesn't work for clang on travis
      BOOST_TEST(d1 != d2);
#endif
      BOOST_TEST(d2.count() == 30);
    }
    {
      duration<int, std::ratio<1,10>> d1 {300};
      duration<int, std::ratio<1,10>> d2  = d1;
      auto d = d1 + d2;
      BOOST_TEST(d.count() == 600);
    }
    {
      duration<int, std::ratio<1,10>> d1 {300};
      duration<int, std::ratio<1,10>> d2  = d1;
      auto d = d1 - d2;
      BOOST_TEST(d.count() == 0);
    }
    {
      duration<int, std::ratio<1,100>> d1 {3};
      duration<int, std::ratio<1,10>> d2{1};
      auto d =  d1 + d2;
      BOOST_TEST_EQ(d.count() , 13);
    }
    {
      duration<int, std::ratio<1,100>> d1 {3};
      duration<int, std::ratio<1,10>> d2{1};
      auto d =  d2 + d1;
      BOOST_TEST_EQ(d.count() , 13);
    }
    {
      duration<double, std::ratio<1,100>> d1 {3};
      duration<double, std::ratio<1,10>> d2{1};
      auto d =  d2 + d1;
      BOOST_TEST((d == duration<double, std::ratio<1,100>>(13)));
    }
    return ::boost::report_errors();
}


//==================================================================================================
/*!
  @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)
*/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/function/expocvt.hpp>
#include <boost/simd/function/is_even.hpp>
#include <boost/simd/function/complement.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/minf.hpp>

template <typename T, std::size_t N, typename Env>
void test(Env& $)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using p_t = bs::pack<T, N>;

  T a1[N];
  T b[N];
  for(std::size_t i = 0; i < N; ++i)
  {
     a1[i] = T(i+1);
     b[i]  = bs::expocvt(a1[i]);
   }
  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb(&b[0], &b[0]+N);
  STF_EQUAL(bs::expocvt(aa1), bb);
}

STF_CASE_TPL("Check expocvt on pack",STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  test<T, N>($);
  test<T, N/2>($);
  test<T, N*2>($);
}

  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

STF_CASE_TPL (" expocvt real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using iT = bd::as_integer_t<T>;
  using bs::expocvt;
  using p_t = bs::pack<T>;
  using r_t = decltype(expocvt(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

  // specific values tests

  for( iT i= bs::Minexponent<T>(); i < bs::Maxexponent<T>(); ++i)
  {
    p_t z1 = expocvt(p_t(i));
    STF_EQUAL(z1, p_t(std::ldexp(T(1), i)));
  }

} // end of test for floating_


#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>
#include <algorithm>
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include <array>

#include "train_features.h"

using namespace std;

double expit(double x) {
    if (x > 0) {
        return 1. / (1. + exp(-x));
    } else {
        return 1. - 1. / (1. + exp(x));
    }
}

double log1exp(double x) {
    if (x > 0) {
        return x + log1p(exp(-x));
    } else {
        return log1p(exp(x));
    }
}

void train_with_features(string data_file_path,
                         string features_file_path,
                         string noise_file_path,
                         int n_steps, double eta_0, double iter_power,
                         size_t l_dimensions,
                         string output_file_path) {
    cout << data_file_path << endl;
    cout << features_file_path << endl;
    cout << noise_file_path << endl;

    // Data loading
    fstream data_file_input;
    data_file_input.open(data_file_path, ios::in);
    string line;
    vector<string> tokens;
    getline(data_file_input, line);
    boost::split(tokens, line, boost::is_any_of(","));
    size_t data_size = stoul(tokens[0]);
    size_t n_samples = stoul(tokens[1]);

    vector<size_t> start_indexes(n_samples);
    vector<size_t> permutation(n_samples);
    vector<int> data(data_size + n_samples);
    long n_data = 0;
    long n_noise = 0;
    for (size_t i = 0, data_index = 0; i < n_samples; ++i) {
        getline(data_file_input, line);
        boost::split(tokens, line, boost::is_any_of(","));
        start_indexes[i] = data_index;
        permutation[i] = i;
        for (size_t j = 0; j < tokens.size(); ++j) {
            data[data_index + j] = stoi(tokens[j]);
        }
        if (data[data_index] == 0) {
            ++n_noise;
        } else {
            ++n_data;
        }
        data[data_index + tokens.size()] = -1;
        data_index += tokens.size() + 1;
    }

    fstream features_file_input;
    features_file_input.open(features_file_path, ios::in);
    getline(features_file_input, line);
    boost::split(tokens, line, boost::is_any_of(","));
    size_t n_items = stoul(tokens[0]);
    size_t m_features = stoul(tokens[1]);

    auto index_features = [m_features](size_t i, size_t j) -> size_t {
        return (i * m_features) + j;
    };
    vector<double> features(n_items * m_features);
    for (size_t i = 0; i < n_items; ++i) {
        getline(features_file_input, line);
        boost::split(tokens, line, boost::is_any_of(","));
        for (size_t j = 0; j < m_features; ++j) {
            features[index_features(i, j)] = stod(tokens[j]);
        }
    }

    fstream noise_file_input;
    noise_file_input.open(noise_file_path, ios::in);
    getline(noise_file_input, line);
    boost::split(tokens, line, boost::is_any_of(","));
    vector<double> noise_weights(m_features);
    for (size_t i = 0; i < m_features; ++i) {
        noise_weights[i] = stod(tokens[i]);
    }
    vector<double> noise_utilities(n_items);
    for (size_t i = 0; i < n_items; ++i) {
        noise_utilities[i] = 0;
        for (size_t j = 0; j < m_features; ++j) {
            noise_utilities[i] += features[index_features(i, j)] * noise_weights[j];
        }
    }

    // Calculate constant quantities.
    double log_nu = log(n_noise / n_data);
    double logz_noise = 0;
    for (size_t i = 0; i < n_items; ++i) {
        logz_noise += log1exp(noise_utilities[i]);
    }
    cout << "Log nu: " << log_nu << endl;
    cout << "Log noise: " << logz_noise << endl;

    // Initialize parameters.
    auto random_engine = default_random_engine{};
    random_engine.seed(10000000);
    uniform_real_distribution<double_t> udouble_dist(0, 1e-3);
    vector<double> b_weights(m_features * l_dimensions);
    vector<double> a_weights(m_features);
    double n_logz = 0;

    auto index_b_weights = [l_dimensions](size_t i, size_t j) -> size_t {
        return (i * l_dimensions) + j;
    };

    for (size_t i = 0; i < m_features; ++i) {
        a_weights[i] = noise_weights[i];
    }
    for (size_t i = 0; i < m_features; ++i) {
        for (size_t j = 0; j < l_dimensions; ++j) {
            b_weights[index_b_weights(i, j)] = udouble_dist(random_engine);
        }
    }
    for (size_t i = 0; i < n_items; ++i) {
        n_logz -= log1exp(noise_utilities[i]);
    }

    vector<double> a_gradient(m_features);
    for (size_t iter = 0; iter < n_steps; ++iter) {
        shuffle(begin(permutation), end(permutation), random_engine);
        for (size_t sub_iter = 0; sub_iter < n_samples; ++sub_iter) {
            size_t start_idx = start_indexes[permutation[sub_iter]];
            size_t end_idx;
            if (permutation[sub_iter] == n_samples - 1) {
                end_idx = data_size + n_samples - 1;
            } else {
                end_idx = start_indexes[permutation[sub_iter] + 1] - 1;
            }
            int label = data[start_idx];
            double p_model = n_logz;
            double p_noise = -logz_noise;
            for (size_t j = 0; j < m_features; ++j) {
                a_gradient[j] = 0;
                for (size_t i = start_idx + 1; i < end_idx; ++i) {
                    a_gradient[j] += features[index_features(data[i], j)];
                    p_model += features[index_features(data[i], j)] *
                            a_weights[j];
                    p_noise += features[index_features(data[i], j)] *
                            noise_weights[j];
                }
            }

            vector<double> max_weights(l_dimensions);
            vector<int> max_weight_indexes(l_dimensions);
            for (size_t j = 0; j < l_dimensions; ++j) {
                max_weights[j] = -1;
                max_weight_indexes[j] = -1;
                for (size_t i = start_idx + 1; i < end_idx; ++i) {
                    double weight = 0;
                    for (size_t k = 0; k < m_features; ++k) {
                        weight += features[index_features(data[i], k)] *
                                b_weights[index_b_weights(k, j)];
                    }
                    if (weight > max_weights[j]) {
                        max_weights[j] = weight;
                        max_weight_indexes[j] = data[i];
                    }
                    p_model -= weight;
                }
                p_model += max_weights[j];
            }

            double learning_rate = eta_0 *
                    pow((iter * n_samples) + sub_iter + 1, -iter_power);
            double factor = learning_rate *
                    (label - expit(p_model - p_noise - log_nu));

            for (size_t i = 0; i < m_features; ++i) {
                a_weights[i] += factor * a_gradient[i];
                for (size_t j = 0; j < l_dimensions; ++j) {
                    b_weights[index_b_weights(i, j)] += factor *
                        (features[index_features(max_weight_indexes[j], i)] -
                                a_gradient[i]);
                    if (b_weights[index_b_weights(i, j)] <= 0) {
                        b_weights[index_b_weights(i, j)] =
                                udouble_dist(random_engine);
                    }
                }
            }
            n_logz += factor;
        }
    }

    fstream output_file(output_file_path, ios::out);

    cout << n_logz << endl;
    output_file << n_logz << endl;

    for (size_t i = 0; i < m_features - 1; ++i) {
        cout << a_weights[i] << ",";
        output_file << a_weights[i] << ",";
    }
    cout << a_weights[m_features - 1] << endl;
    output_file << a_weights[m_features - 1] << endl;

    for (size_t i = 0; i < m_features; ++i) {
        for (size_t j = 0; j < l_dimensions - 1; ++j) {
            cout << b_weights[index_b_weights(i, j)] << ",";
            output_file << b_weights[index_b_weights(i, j)] << ",";
        }
        output_file << b_weights[index_b_weights(i, l_dimensions - 1)] << endl;
        cout << endl;
    }

}

int main(int argc, char* argv[]) {
    int fold_number = stoi(argv[1]);
    int dim_number = stoi(argv[2]);
    int feature_set = stoi(argv[3]);
    for (int d = 1; d <= dim_number; ++d) {
        for (int i = 1; i <= fold_number; ++i) {
            train_with_features(
                    (boost::format("/home/diegob/workspace/master-thesis-2015/data/path_set_nce_data_features_%1%_fold_%2%.csv") % feature_set % i).str(),
                    (boost::format("/home/diegob/workspace/master-thesis-2015/data/path_set_nce_features_%1%.csv") % feature_set).str(),
                    (boost::format("/home/diegob/workspace/master-thesis-2015/data/path_set_nce_noise_features_%1%_fold_%2%.csv") % feature_set % i).str(),
                    10, 0.01, 0.1, d,
                    (boost::format("/home/diegob/workspace/master-thesis-2015/data/models/path_set_nce_out_features_%1%_dim_%2%_fold_%3%.csv") % feature_set % d % i).str());
        }
    }

}


/* boost random/lagged_fibonacci.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * Jens Maurer makes no representations about the suitability of this
 * software for any purpose. It is provided "as is" without express or
 * implied warranty.
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: lagged_fibonacci.hpp 13038 2002-03-03 09:13:57Z jmaurer $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_LAGGED_FIBONACCI_HPP
#define BOOST_RANDOM_LAGGED_FIBONACCI_HPP

#include <iostream>
#include <algorithm>     // std::max
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/cstdint.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_01.hpp>

namespace boost {
namespace random {

// lagged Fibonacci generator for the range [0..1)
// contributed by Matthias Troyer
// for p=55, q=24 originally by G. J. Mitchell and D. P. Moore 1958

template<class T, unsigned int p, unsigned int q>
struct fibonacci_validation
{
  BOOST_STATIC_CONSTANT(bool, is_specialized = false);
  static T value() { return 0; }
  static T tolerance() { return 0; }
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class T, unsigned int p, unsigned int q>
const bool fibonacci_validation<T, p, q>::is_specialized;
#endif

#define BOOST_RANDOM_FIBONACCI_VAL(T,P,Q,V,E) \
template<> \
struct fibonacci_validation<T, P, Q>  \
{                                     \
  BOOST_STATIC_CONSTANT(bool, is_specialized = true);     \
  static T value() { return V; }      \
  static T tolerance()                \
    { return std::max(E, static_cast<T>(5*std::numeric_limits<T>::epsilon())); } \
};
// (The extra static_cast<T> in the std::max call above is actually
// unnecessary except for HP aCC 1.30, which claims that
// numeric_limits<double>::epsilon() doesn't actually return a double.)

BOOST_RANDOM_FIBONACCI_VAL(double, 607, 273, 0.4293817707235914, 1e-14)
BOOST_RANDOM_FIBONACCI_VAL(double, 1279, 418, 0.9421630240437659, 1e-14)
BOOST_RANDOM_FIBONACCI_VAL(double, 2281, 1252, 0.1768114046909004, 1e-14)
BOOST_RANDOM_FIBONACCI_VAL(double, 3217, 576, 0.1956232694868209, 1e-14)
BOOST_RANDOM_FIBONACCI_VAL(double, 4423, 2098, 0.9499762202147172, 1e-14)
BOOST_RANDOM_FIBONACCI_VAL(double, 9689, 5502, 0.05737836943695162, 1e-14)
BOOST_RANDOM_FIBONACCI_VAL(double, 19937, 9842, 0.5076528587449834, 1e-14)
BOOST_RANDOM_FIBONACCI_VAL(double, 23209, 13470, 0.5414473810619185, 1e-14)
BOOST_RANDOM_FIBONACCI_VAL(double, 44497,21034, 0.254135073399297, 1e-14)

#undef BOOST_RANDOM_FIBONACCI_VAL

template<class FloatType, unsigned int p, unsigned int q>
class lagged_fibonacci
{
public:
  typedef FloatType result_type;
  BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);
  BOOST_STATIC_CONSTANT(unsigned int, long_lag = p);
  BOOST_STATIC_CONSTANT(unsigned int, short_lag = q);

  result_type min() const { return 0.0; }
  result_type max() const { return 1.0; }

  lagged_fibonacci() { seed(); }
  explicit lagged_fibonacci(uint32_t value) { seed(value); }
  template<class Generator>
  explicit lagged_fibonacci(Generator & gen) { seed(gen); }
  // compiler-generated copy ctor and assignment operator are fine

  void seed(uint32_t value = 331u)
  {
    minstd_rand0 intgen(value);
    seed(intgen);
  }

  // For GCC, moving this function out-of-line prevents inlining, which may
  // reduce overall object code size.  However, MSVC does not grok
  // out-of-line template member functions.
  template<class Generator>
  void seed(Generator & gen)
  {
    uniform_01<Generator, FloatType> gen01(gen);
    // I could have used std::generate_n, but it takes "gen" by value
    for(unsigned int j = 0; j < long_lag; ++j)
      x[j] = gen01();
    i = long_lag;
  }

  result_type operator()()
  {
    if(i >= long_lag)
      fill();
    return x[i++];
  }

  bool validation(result_type x) const
  {
    result_type v = fibonacci_validation<result_type, p, q>::value();
    result_type epsilon = fibonacci_validation<result_type, p, q>::tolerance();
    // std::abs is a source of trouble: sometimes, it's not overloaded
    // for double, plus the usual namespace std noncompliance -> avoid it
    // using std::abs;
    // return abs(x - v) < 5 * epsilon
    return x > v - epsilon && x < v + epsilon;
  }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE
  friend std::ostream& operator<<(std::ostream& os, const lagged_fibonacci& f)
  {
    os << f.i << " ";
    std::streamsize prec =
      os.precision(std::numeric_limits<FloatType>::digits10);
    for(unsigned int i = 0; i < long_lag; ++i)
      os << f.x[i] << " ";
    os.precision(prec);
    return os;
  }
  friend std::istream& operator>>(std::istream& is, lagged_fibonacci& f)
  {
    is >> f.i >> std::ws;
    for(unsigned int i = 0; i < long_lag; ++i)
      is >> f.x[i] >> std::ws;
    return is;
  }
  friend bool operator==(const lagged_fibonacci& x, const lagged_fibonacci& y)
  { return x.i == y.i && std::equal(x.x, x.x+long_lag, y.x); }
#else
  // Use a member function; Streamable concept not supported.
  bool operator==(const lagged_fibonacci& rhs) const
  { return i == rhs.i && std::equal(x, x+long_lag, rhs.x); }
#endif

private:
  void fill();
  unsigned int i;
  FloatType x[long_lag];
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class FloatType, unsigned int p, unsigned int q>
const bool lagged_fibonacci<FloatType, p, q>::has_fixed_range;
template<class FloatType, unsigned int p, unsigned int q>
const unsigned int lagged_fibonacci<FloatType, p, q>::long_lag;
template<class FloatType, unsigned int p, unsigned int q>
const unsigned int lagged_fibonacci<FloatType, p, q>::short_lag;

#endif

template<class FloatType, unsigned int p, unsigned int q>
void lagged_fibonacci<FloatType, p, q>::fill()
{
  // two loops to avoid costly modulo operations
  {  // extra scope for MSVC brokenness w.r.t. for scope
  for(unsigned int j = 0; j < short_lag; ++j) {
    FloatType t = x[j] + x[j+(long_lag-short_lag)];
    if(t >= 1.0)
      t -= 1.0;
    x[j] = t;
  }
  }
  for(unsigned int j = short_lag; j < long_lag; ++j) {
    FloatType t = x[j] + x[j-short_lag];
    if(t >= 1.0)
      t -= 1.0;
    x[j] = t;
  }
  i = 0;
}

} // namespace random

typedef random::lagged_fibonacci<double, 607, 273> lagged_fibonacci607;
typedef random::lagged_fibonacci<double, 1279, 418> lagged_fibonacci1279;
typedef random::lagged_fibonacci<double, 2281, 1252> lagged_fibonacci2281;
typedef random::lagged_fibonacci<double, 3217, 576> lagged_fibonacci3217;
typedef random::lagged_fibonacci<double, 4423, 2098> lagged_fibonacci4423;
typedef random::lagged_fibonacci<double, 9689, 5502> lagged_fibonacci9689;
typedef random::lagged_fibonacci<double, 19937, 9842> lagged_fibonacci19937;
typedef random::lagged_fibonacci<double, 23209, 13470> lagged_fibonacci23209;
typedef random::lagged_fibonacci<double, 44497, 21034> lagged_fibonacci44497;

// It is possible to partially specialize uniform_01<> on lagged_fibonacci<>
// to help the compiler generate efficient code.  For GCC, this seems useless,
// because GCC optimizes (x-0)/(1-0) to (x-0).  This is good enough for now.

} // namespace boost

#endif // BOOST_RANDOM_LAGGED_FIBONACCI_HPP


/* Copyright (c) 2020 C. Pattison
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#pragma once
#include <cassert>
#include <algorithm>
#include <utility>
#include <complex>
#include <random>
#include <cmath>

#include <Eigen/Dense>
#include "syk_types.hpp"

// https://web.eecs.umich.edu/~rajnrao/Acta05rmt.pdf

namespace syk {
using namespace std::complex_literals;

template<typename rng_type>
MatrixType RandomRealIidGauss(rng_type* rand_gen, int n, int m) {
    MatrixType mat = MatrixType::Zero(n, m);
    std::normal_distribution normal;

    for(int j = 0; j < m; ++j) {
        for(int i = 0; i < n; ++i) {
            mat(i, j) = static_cast<MatrixType::Scalar>(normal(*rand_gen));
        }
    }
    return mat;
}

template<typename rng_type>
MatrixType RandomGOE(rng_type* rand_gen, int n) {
    MatrixType A = RandomRealIidGauss(rand_gen, n, n);
    return (A + A.transpose()) / (2 * std::sqrt(n));
}

template<typename rng_type>
MatrixType RandomGUE(rng_type* rand_gen, int n) {
    MatrixType A = RandomRealIidGauss(rand_gen, n, n) + 1.0i * RandomRealIidGauss(rand_gen, n, n);
    return (A + A.adjoint()) / (2 * std::sqrt(n));
}
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2015 Eugene Brevdo <ebrevdo@google.com>
//                    Benoit Steiner <benoit.steiner.goog@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"

#include <Eigen/CXX11/Tensor>

using Eigen::Tensor;
using Eigen::array;
using Eigen::Tuple;

template <int DataLayout>
static void test_simple_index_tuples()
{
  Tensor<float, 4, DataLayout> tensor(2,3,5,7);
  tensor.setRandom();
  tensor = (tensor + tensor.constant(0.5)).log();

  Tensor<Tuple<DenseIndex, float>, 4, DataLayout> index_tuples(2,3,5,7);
  index_tuples = tensor.index_tuples();

  for (DenseIndex n = 0; n < 2*3*5*7; ++n) {
    const Tuple<DenseIndex, float>& v = index_tuples.coeff(n);
    VERIFY_IS_EQUAL(v.first, n);
    VERIFY_IS_EQUAL(v.second, tensor.coeff(n));
  }
}

template <int DataLayout>
static void test_index_tuples_dim()
{
  Tensor<float, 4, DataLayout> tensor(2,3,5,7);
  tensor.setRandom();
  tensor = (tensor + tensor.constant(0.5)).log();

  Tensor<Tuple<DenseIndex, float>, 4, DataLayout> index_tuples(2,3,5,7);

  index_tuples = tensor.index_tuples();

  for (Eigen::DenseIndex n = 0; n < tensor.size(); ++n) {
    const Tuple<DenseIndex, float>& v = index_tuples(n); //(i, j, k, l);
    VERIFY_IS_EQUAL(v.first, n);
    VERIFY_IS_EQUAL(v.second, tensor(n));
  }
}

template <int DataLayout>
static void test_argmax_tuple_reducer()
{
  Tensor<float, 4, DataLayout> tensor(2,3,5,7);
  tensor.setRandom();
  tensor = (tensor + tensor.constant(0.5)).log();

  Tensor<Tuple<DenseIndex, float>, 4, DataLayout> index_tuples(2,3,5,7);
  index_tuples = tensor.index_tuples();

  Tensor<Tuple<DenseIndex, float>, 0, DataLayout> reduced;
  DimensionList<DenseIndex, 4> dims;
  reduced = index_tuples.reduce(
      dims, internal::ArgMaxTupleReducer<Tuple<DenseIndex, float> >());

  Tensor<float, 0, DataLayout> maxi = tensor.maximum();

  VERIFY_IS_EQUAL(maxi(), reduced(0).second);

  array<DenseIndex, 3> reduce_dims;
  for (int d = 0; d < 3; ++d) reduce_dims[d] = d;
  Tensor<Tuple<DenseIndex, float>, 1, DataLayout> reduced_by_dims(7);
  reduced_by_dims = index_tuples.reduce(
      reduce_dims, internal::ArgMaxTupleReducer<Tuple<DenseIndex, float> >());

  Tensor<float, 1, DataLayout> max_by_dims = tensor.maximum(reduce_dims);

  for (int l = 0; l < 7; ++l) {
    VERIFY_IS_EQUAL(max_by_dims(l), reduced_by_dims(l).second);
  }
}

template <int DataLayout>
static void test_argmin_tuple_reducer()
{
  Tensor<float, 4, DataLayout> tensor(2,3,5,7);
  tensor.setRandom();
  tensor = (tensor + tensor.constant(0.5)).log();

  Tensor<Tuple<DenseIndex, float>, 4, DataLayout> index_tuples(2,3,5,7);
  index_tuples = tensor.index_tuples();

  Tensor<Tuple<DenseIndex, float>, 0, DataLayout> reduced;
  DimensionList<DenseIndex, 4> dims;
  reduced = index_tuples.reduce(
      dims, internal::ArgMinTupleReducer<Tuple<DenseIndex, float> >());

  Tensor<float, 0, DataLayout> mini = tensor.minimum();

  VERIFY_IS_EQUAL(mini(), reduced(0).second);

  array<DenseIndex, 3> reduce_dims;
  for (int d = 0; d < 3; ++d) reduce_dims[d] = d;
  Tensor<Tuple<DenseIndex, float>, 1, DataLayout> reduced_by_dims(7);
  reduced_by_dims = index_tuples.reduce(
      reduce_dims, internal::ArgMinTupleReducer<Tuple<DenseIndex, float> >());

  Tensor<float, 1, DataLayout> min_by_dims = tensor.minimum(reduce_dims);

  for (int l = 0; l < 7; ++l) {
    VERIFY_IS_EQUAL(min_by_dims(l), reduced_by_dims(l).second);
  }
}

template <int DataLayout>
static void test_simple_argmax()
{
  Tensor<float, 4, DataLayout> tensor(2,3,5,7);
  tensor.setRandom();
  tensor = (tensor + tensor.constant(0.5)).log();
  tensor(0,0,0,0) = 10.0;

  Tensor<DenseIndex, 0, DataLayout> tensor_argmax;

  tensor_argmax = tensor.argmax();

  VERIFY_IS_EQUAL(tensor_argmax(0), 0);

  tensor(1,2,4,6) = 20.0;

  tensor_argmax = tensor.argmax();

  VERIFY_IS_EQUAL(tensor_argmax(0), 2*3*5*7 - 1);
}

template <int DataLayout>
static void test_simple_argmin()
{
  Tensor<float, 4, DataLayout> tensor(2,3,5,7);
  tensor.setRandom();
  tensor = (tensor + tensor.constant(0.5)).log();
  tensor(0,0,0,0) = -10.0;

  Tensor<DenseIndex, 0, DataLayout> tensor_argmin;

  tensor_argmin = tensor.argmin();

  VERIFY_IS_EQUAL(tensor_argmin(0), 0);

  tensor(1,2,4,6) = -20.0;

  tensor_argmin = tensor.argmin();

  VERIFY_IS_EQUAL(tensor_argmin(0), 2*3*5*7 - 1);
}

template <int DataLayout>
static void test_argmax_dim()
{
  Tensor<float, 4, DataLayout> tensor(2,3,5,7);
  std::vector<int> dims {2, 3, 5, 7};

  for (int dim = 0; dim < 4; ++dim) {
    tensor.setRandom();
    tensor = (tensor + tensor.constant(0.5)).log();

    Tensor<DenseIndex, 3, DataLayout> tensor_argmax;
    array<DenseIndex, 4> ix;
    for (int i = 0; i < 2; ++i) {
      for (int j = 0; j < 3; ++j) {
        for (int k = 0; k < 5; ++k) {
          for (int l = 0; l < 7; ++l) {
            ix[0] = i; ix[1] = j; ix[2] = k; ix[3] = l;
            if (ix[dim] != 0) continue;
            // suppose dim == 1, then for all i, k, l, set tensor(i, 0, k, l) = 10.0
            tensor(ix) = 10.0;
          }
        }
      }
    }

    tensor_argmax = tensor.argmax(dim);

    VERIFY_IS_EQUAL(tensor_argmax.size(),
                    ptrdiff_t(2*3*5*7 / tensor.dimension(dim)));
    for (ptrdiff_t n = 0; n < tensor_argmax.size(); ++n) {
      // Expect max to be in the first index of the reduced dimension
      VERIFY_IS_EQUAL(tensor_argmax.data()[n], 0);
    }

    for (int i = 0; i < 2; ++i) {
      for (int j = 0; j < 3; ++j) {
        for (int k = 0; k < 5; ++k) {
          for (int l = 0; l < 7; ++l) {
            ix[0] = i; ix[1] = j; ix[2] = k; ix[3] = l;
            if (ix[dim] != tensor.dimension(dim) - 1) continue;
            // suppose dim == 1, then for all i, k, l, set tensor(i, 2, k, l) = 20.0
            tensor(ix) = 20.0;
          }
        }
      }
    }

    tensor_argmax = tensor.argmax(dim);

    VERIFY_IS_EQUAL(tensor_argmax.size(),
                    ptrdiff_t(2*3*5*7 / tensor.dimension(dim)));
    for (ptrdiff_t n = 0; n < tensor_argmax.size(); ++n) {
      // Expect max to be in the last index of the reduced dimension
      VERIFY_IS_EQUAL(tensor_argmax.data()[n], tensor.dimension(dim) - 1);
    }
  }
}

template <int DataLayout>
static void test_argmin_dim()
{
  Tensor<float, 4, DataLayout> tensor(2,3,5,7);
  std::vector<int> dims {2, 3, 5, 7};

  for (int dim = 0; dim < 4; ++dim) {
    tensor.setRandom();
    tensor = (tensor + tensor.constant(0.5)).log();

    Tensor<DenseIndex, 3, DataLayout> tensor_argmin;
    array<DenseIndex, 4> ix;
    for (int i = 0; i < 2; ++i) {
      for (int j = 0; j < 3; ++j) {
        for (int k = 0; k < 5; ++k) {
          for (int l = 0; l < 7; ++l) {
            ix[0] = i; ix[1] = j; ix[2] = k; ix[3] = l;
            if (ix[dim] != 0) continue;
            // suppose dim == 1, then for all i, k, l, set tensor(i, 0, k, l) = -10.0
            tensor(ix) = -10.0;
          }
        }
      }
    }

    tensor_argmin = tensor.argmin(dim);

    VERIFY_IS_EQUAL(tensor_argmin.size(),
                    ptrdiff_t(2*3*5*7 / tensor.dimension(dim)));
    for (ptrdiff_t n = 0; n < tensor_argmin.size(); ++n) {
      // Expect min to be in the first index of the reduced dimension
      VERIFY_IS_EQUAL(tensor_argmin.data()[n], 0);
    }

    for (int i = 0; i < 2; ++i) {
      for (int j = 0; j < 3; ++j) {
        for (int k = 0; k < 5; ++k) {
          for (int l = 0; l < 7; ++l) {
            ix[0] = i; ix[1] = j; ix[2] = k; ix[3] = l;
            if (ix[dim] != tensor.dimension(dim) - 1) continue;
            // suppose dim == 1, then for all i, k, l, set tensor(i, 2, k, l) = -20.0
            tensor(ix) = -20.0;
          }
        }
      }
    }

    tensor_argmin = tensor.argmin(dim);

    VERIFY_IS_EQUAL(tensor_argmin.size(),
                    ptrdiff_t(2*3*5*7 / tensor.dimension(dim)));
    for (ptrdiff_t n = 0; n < tensor_argmin.size(); ++n) {
      // Expect min to be in the last index of the reduced dimension
      VERIFY_IS_EQUAL(tensor_argmin.data()[n], tensor.dimension(dim) - 1);
    }
  }
}

void test_cxx11_tensor_argmax()
{
  CALL_SUBTEST(test_simple_index_tuples<RowMajor>());
  CALL_SUBTEST(test_simple_index_tuples<ColMajor>());
  CALL_SUBTEST(test_index_tuples_dim<RowMajor>());
  CALL_SUBTEST(test_index_tuples_dim<ColMajor>());
  CALL_SUBTEST(test_argmax_tuple_reducer<RowMajor>());
  CALL_SUBTEST(test_argmax_tuple_reducer<ColMajor>());
  CALL_SUBTEST(test_argmin_tuple_reducer<RowMajor>());
  CALL_SUBTEST(test_argmin_tuple_reducer<ColMajor>());
  CALL_SUBTEST(test_simple_argmax<RowMajor>());
  CALL_SUBTEST(test_simple_argmax<ColMajor>());
  CALL_SUBTEST(test_simple_argmin<RowMajor>());
  CALL_SUBTEST(test_simple_argmin<ColMajor>());
  CALL_SUBTEST(test_argmax_dim<RowMajor>());
  CALL_SUBTEST(test_argmax_dim<ColMajor>());
  CALL_SUBTEST(test_argmin_dim<RowMajor>());
  CALL_SUBTEST(test_argmin_dim<ColMajor>());
}


/*
 * TypeDefs.hpp
 *
 *  Created on: March 18, 2014
 *      Author: Péter Fankhauser
 *	 Institute: ETH Zurich, ANYbotics
 */

// Eigen
#define EIGEN_DENSEBASE_PLUGIN "grid_map_core/eigen_plugins/DenseBasePlugin.hpp"
#define EIGEN_FUNCTORS_PLUGIN "grid_map_core/eigen_plugins/FunctorsPlugin.hpp"

#include <Eigen/Core>

#pragma once

namespace grid_map {

  typedef Eigen::MatrixXf Matrix;
  typedef Matrix::Scalar DataType;
  typedef Eigen::Vector2d Position;
  typedef Eigen::Vector2d Vector;
  typedef Eigen::Vector3d Position3;
  typedef Eigen::Vector3d Vector3;
  typedef Eigen::Array2i Index;
  typedef Eigen::Array2i Size;
  typedef Eigen::Array2d Length;
  typedef uint64_t Time;

  enum class InterpolationMethods{
      INTER_NEAREST, // nearest neighbor interpolation
      INTER_LINEAR   // bilinear interpolation
      // ToDo: INTER_CUBIC
  };

} /* namespace */


/**  \file trace_ublas.hpp \brief Trace computation for uBlas */
/*
-----------------------------------------------------------------------------
   Copyright (C) 2011 Ruben Martinez-Cantin <rmcantin@unizar.es>

   This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   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
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with this program.  If not, see <http://www.gnu.org/licenses/>.
-----------------------------------------------------------------------------
*/
#ifndef __TRACE_UBLAS_HPP__
#define __TRACE_UBLAS_HPP__

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

namespace bayesopt
{
  namespace utils
  {

    template<class E>
    typename E::value_type trace(const E &A)
    {
      const size_t n = (std::min)(A.size1(),A.size2());
      typename E::value_type sum = 0;
      for (size_t i=0; i<n; ++i)
	sum += A(i,i);

      return sum;
    }

    template<class E>
    typename E::value_type log_trace(const E &A)
    {
      const size_t n = (std::min)(A.size1(),A.size2());
      typename E::value_type sum = 0;
      for (size_t i=0; i<n; ++i)
	sum += std::log(A(i,i));

      return sum;
    }

    template<class E1, class E2>
    typename E1::value_type trace_prod(const E1 & A, const E2 & B )
    {
      namespace ublas = boost::numeric::ublas;

      const size_t n = (std::min)(A.size1(),B.size2());
      typename E1::value_type sum = 0;
      for (size_t i=0; i<n; ++i)
	sum += ublas::inner_prod(ublas::row(A,i),ublas::column(B,i));

      return sum;
    }

  } //  namespace utils
} //namespace bayesopt

#endif


//==================================================================================================
/*!
  @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_ACSCPI_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ACSCPI_HPP_INCLUDED

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

  /*!
    @ingroup group-trigonometric
    This function object returns the inverse secant in \f$\pi\f$ multiples:
    \f$(1/\pi) \arcsin(1/x)\f$.

    @par Header <boost/simd/function/acscpi.hpp>

    @see acsc,  acscd

    @par Example:

      @snippet acscpi.cpp acscpi

    @par Possible output:

      @snippet acscpi.txt acscpi

  **/
  IEEEValue acscpi(IEEEValue const & x);
} }
#endif

#include <boost/simd/function/scalar/acscpi.hpp>
#include <boost/simd/function/simd/acscpi.hpp>

#endif


//==============================================================================
//         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_TOOLBOX_TRIGONOMETRIC_CONSTANTS_PIX_4_HPP_INCLUDED
#define NT2_TOOLBOX_TRIGONOMETRIC_CONSTANTS_PIX_4_HPP_INCLUDED
/*!
 * \file
**/
#include <boost/simd/sdk/constant/constant.hpp>
#include <boost/simd/sdk/constant/register.hpp>

/*!
 * \ingroup trigo_constant
 * \defgroup trigo_constant_pix_4 Pix_4
 *
 * \par Description
 * Constant pix_4 : \f$4\pi\f$.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/pix_4.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::_pix_4_(A0)>::type
 *     pix_4();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Pix_4
 *
 * \return type T value
 *
 **/

namespace nt2
{
  namespace tag
  {
    BOOST_SIMD_CONSTANT_REGISTER( Pix_4, double
                                , 12, 0x41490fdb
                                , 0x402921fb54442d18ll
                                );
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Pix_4, Pix_4);
}

#endif


// Copyright Nick Thompson, 2021
// Use, modification and distribution are subject to 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)

// This implements bilinear interpolation on a uniform grid.
// If dx and dy are both positive, then the (x,y) = (x0, y0) is associated with data index 0 (herein referred to as f[0])
// The point (x0 + dx, y0) is associated with f[1], and (x0 + i*dx, y0) is associated with f[i],
// i.e., we are assuming traditional C row major order.
// The y coordinate increases *downward*, as is traditional in 2D computer graphics.
// This is *not* how people generally think in numerical analysis (although it *is* how they lay out matrices).
// Providing the capability of a grid rotation is too expensive and not ergonomic; you'll need to perform any rotations at the call level.

// For clarity, the value f(x0 + i*dx, y0 + j*dy) must be stored in the f[j*cols + i] position.

#ifndef BOOST_MATH_INTERPOLATORS_BILINEAR_UNIFORM_HPP
#define BOOST_MATH_INTERPOLATORS_BILINEAR_UNIFORM_HPP

#include <utility>
#include <memory>
#include <boost/math/interpolators/detail/bilinear_uniform_detail.hpp>

namespace boost::math::interpolators {

template <class RandomAccessContainer>
class bilinear_uniform
{
public:
    using Real = typename RandomAccessContainer::value_type;
    using Z = typename RandomAccessContainer::size_type;

    bilinear_uniform(RandomAccessContainer && fieldData, Z rows, Z cols, Real dx = 1, Real dy = 1, Real x0 = 0, Real y0 = 0)
    : m_imp(std::make_shared<detail::bilinear_uniform_imp<RandomAccessContainer>>(std::move(fieldData), rows, cols, dx, dy, x0, y0))
    {
    }

    Real operator()(Real x, Real y) const
    {
        return m_imp->operator()(x,y);
    }

    friend std::ostream& operator<<(std::ostream& out, bilinear_uniform<RandomAccessContainer> const & bu) {
        out << *bu.m_imp;
        return out;
    }

private:
    std::shared_ptr<detail::bilinear_uniform_imp<RandomAccessContainer>> m_imp;
};

}
#endif


#include <iostream>
#include <fstream>

#include <vector>
#include <string>

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

#include <iomanip>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace std;

int loadPoses(string file_name, vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> > &poses) {
    FILE *fp = fopen(file_name.c_str(), "r");
    if (!fp)
        return  0;
    while (!feof(fp)) {
        double P[3] [4];
        if (fscanf(fp, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
                   &P[0][0], &P[0][1], &P[0][2], &P[0][3],
                   &P[1][0], &P[1][1], &P[1][2], &P[1][3],
                   &P[2][0], &P[2][1], &P[2][2], &P[2][3] ) == 12) {
            Eigen::Matrix4d T;
            T << P[0][0], P[0][1], P[0][2], P[0][3],
            P[1][0], P[1][1], P[1][2], P[1][3],
            P[2][0], P[2][1], P[2][2], P[2][3],
            0, 0, 0, 1;
            poses.push_back(T);
        }
    }
    fclose(fp);
    return 1;

}

int loadStampPoses(string file_name, vector<double>& stamps,vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> > &poses) {
    FILE *fp = fopen(file_name.c_str(), "r");
    if (!fp)
        return  0;
    while (!feof(fp)) {
        double stamp;
        double P[3] [4];
        if(fscanf(fp,"%lf",&stamp)==1){
            stamp/=1000;
            stamps.push_back(stamp);
        }
        if (fscanf(fp, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
                   &P[0][0], &P[0][1], &P[0][2], &P[0][3],
                   &P[1][0], &P[1][1], &P[1][2], &P[1][3],
                   &P[2][0], &P[2][1], &P[2][2], &P[2][3] ) == 12) {
            Eigen::Matrix4d T;
            T << P[0][0], P[0][1], P[0][2], P[0][3],
            P[1][0], P[1][1], P[1][2], P[1][3],
            P[2][0], P[2][1], P[2][2], P[2][3],
            0, 0, 0, 1;
            poses.push_back(T);
        }
    }
    fclose(fp);
    return 1;

}

int loadNdtPoses(string file_name, vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &pose_xyz,vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &pose_rpy) {
    FILE *fp = fopen(file_name.c_str(), "r");
    if (!fp)
        return  0;
    while (!feof(fp)) {
        double P[6];
        if (fscanf(fp, "%lf %lf %lf %lf %lf %lf",
                   &P[0], &P[1], &P[2], &P[3], &P[4], &P[5] ) == 6) {
            Eigen::Vector3d xyz,rpy;
            xyz<<P[0],P[1],P[2];
            rpy<<P[3],P[4],P[5];
            pose_rpy.push_back(rpy);
            pose_xyz.push_back(xyz);
        }
    }
    fclose(fp);
    return 1;

}

void loadStamp(string file_name, vector<double>& stamp){
    FILE *fp = fopen(file_name.c_str(), "r");
    if (!fp){
        cout<<"open file fail: "<<file_name<<endl;
        return  ;
    }
    while (!feof(fp)) {
        double timestamp;
        if (fscanf(fp, "%lf", &timestamp ) == 1) {
            stamp.push_back(timestamp);
        }
    }
    fclose(fp);
    return ;
}

int main(int argc, char** argv){
//    vector<Eigen::Matrix4d,Eigen::aligned_allocator<Eigen::Matrix4d> > poses;
//    loadPoses("/home/ywl/ourdata/data060502/cam_gt_pose.txt",poses);
//    for(int i=0;i<poses.size();i++){
//        cout<<i<<endl;
//        Eigen::Matrix4d p=poses[i].inverse();
//        cout<<p<<endl;
//    }

#if 1
    string stampfile="/media/ywl/samsungT5/velodyne_points.txt";
    string lvinsposefile="/home/ywl/ourdata/trajectory/mapping_pose.txt";
    string mapposefile="/media/ywl/samsungT5/061403/mapping_pose.txt";
    string gtfile="/home/ywl/ourdata/trajectory/gt_pose.txt";
    vector<double> stamp;
    vector<double> mapstamp;
    vector<Eigen::Matrix4d,Eigen::aligned_allocator<Eigen::Matrix4d> > lvinspose;
    vector<Eigen::Matrix4d,Eigen::aligned_allocator<Eigen::Matrix4d> > mappose;
    loadPoses(lvinsposefile,lvinspose);
    loadStamp(stampfile,stamp);
    mapstamp.push_back(stamp[0]);
    mappose.push_back(lvinspose[0]);
    loadStampPoses(mapposefile,mapstamp,mappose);
//    int firstfram=10;
//    for(int i=1;i<mapstamp.size();i++){
//        mapstamp[i]+=stamp[firstfram];
//    }
//    cout<<fixed<<mapstamp[1]<<" "<<mapstamp[mapstamp.size()-1]<<endl;

    int lastindex=0;
    vector<Eigen::Matrix4d,Eigen::aligned_allocator<Eigen::Matrix4d> > gtpose;
    //gtpose.push_back(mappose[0]);
    for(int i=1;i<mapstamp.size();i++){
        int index=0;
        while(mapstamp[i]>stamp[index]){
            index++;
            if(index==stamp.size())
                break;
        }
        for(int j=lastindex;j<index;j++){
            Eigen::Matrix4d nowpose;
            nowpose=mappose[i-1]*lvinspose[lastindex].inverse()*lvinspose[j];
            gtpose.push_back(nowpose);
        }
        lastindex=index;
    }
    if(gtpose.size()<lvinspose.size()){
        for(int i=gtpose.size();i<lvinspose.size();i++){
            Eigen::Matrix4d nowpose;
            nowpose=mappose[mappose.size()-1]*lvinspose[gtpose.size()].inverse()*lvinspose[i];
            gtpose.push_back(nowpose);
        }
    }
    ofstream ofs;
    ofs.open(gtfile,std::ios::out);
    for(int i=0;i<gtpose.size();i++){
        Eigen::Matrix4d pose=gtpose[i];
        ofs<<fixed<<setprecision(8)<<pose(0,0)<<" "<<pose(0,1)<<" "<<pose(0,2)<<" "<<pose(0,3)<<" "<<
             pose(1,0)<<" "<<pose(1,1)<<" "<<pose(1,2)<<" "<<pose(1,3)<<" "<<
             pose(2,0)<<" "<<pose(2,1)<<" "<<pose(2,2)<<" "<<pose(2,3)<<endl;
    }

#endif

}


#include "NNModel.h"
#include <math.h>
#include <stdio.h>
#include <algorithm>
#include <iostream>
#include <stdlib.h>
#include <string>
#include "IonErr.h"
#include <hdf5.h>

#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

NN::NNModel::NNModel(const string &fname){
	layerTypes_ = NULL;
	layerNames_ = NULL;
	layerDimIn_ = NULL;
	layerDimOut_ = NULL;
	LoadWeights(fname);
}

NN::NNModel::~NNModel(){
	for(unsigned int i = 0; i < m_Layers.size(); ++i) {
		delete m_Layers[i];
	}
	delete [] layerTypes_;
	delete [] layerNames_;
	delete [] layerDimIn_;
	delete [] layerDimOut_;
}

vector<float> NN::NNModel::CalculateOutput(NN::DataChunk* pred){
	//cout << endl << "Calculating output" << endl;
	NN::DataChunk *input = pred;
	NN::DataChunk *output = NULL;
	for(int i = 0; i < (int)m_Layers.size(); ++i){
		//cout << "layer" <<i <<endl;
		output = m_Layers[i]->GetOutput(input);
		if(input != pred)
			delete input;
		input = NULL;
		input = output;
	}
	// last layer
	vector<float> flat_out = output->GetData();
	// Default Activation - softmax
	cout << flat_out[0] <<endl;
	float sum = 0.0;
	for(unsigned int j = 0; j < flat_out.size(); j++) {
		if (flat_out[j] < 10)
			flat_out[j] = exp(flat_out[j]);
		else
			flat_out[j] = exp(10);
		sum += flat_out[j];
	}
	for(unsigned int j = 0; j < flat_out.size(); ++j) {
		flat_out[j] /= sum;
	}
	delete output;
	return flat_out;
}

void NN::NNModel::LoadWeights(const string &fname) {
	Layer *l = NULL;

	if(H5Fis_hdf5(fname.c_str()) > 0)
	{
		cout << endl << "NNModel::Init... load model from " << fname.c_str() << endl;
		hid_t root = H5Fopen(fname.c_str(), H5F_ACC_RDONLY, H5P_DEFAULT);  // open file
		if(root < 0)
		{
			ION_ABORT("ERROR: cannot open HDF5 file " + fname);
		}
		hid_t NNarch = H5Gopen(root, "/arch", H5P_DEFAULT);   //open group
		if (NNarch < 0)
		{
			H5Fclose(root);
			ION_ABORT("ERROR: fail to open HDF5 group arch");
		}
		hid_t dstypes = H5Dopen(NNarch, "types", H5P_DEFAULT);  // open layer types
		if (dstypes < 0)
		{
			H5Gclose(NNarch);
			H5Fclose(root);
			ION_ABORT("ERROR: fail to open HDF5 dataset types");
		}
		hsize_t dSize = H5Dget_storage_size(dstypes);
		dSize /= sizeof(H5T_NATIVE_UINT);
		layerTypes_ = new unsigned int[dSize];
		herr_t ret = H5Dread(dstypes, H5T_NATIVE_UINT, H5S_ALL, H5S_ALL, H5P_DEFAULT, layerTypes_);
		if (ret < 0)
		{
			delete [] layerTypes_;
			layerTypes_ = 0;
			ION_ABORT("ERROR: fail to read HDF5 dataset types");
		}
		H5Dclose(dstypes);
		hid_t dsnames = H5Dopen(NNarch, "names", H5P_DEFAULT);  // open layer names
		if (dsnames < 0)
		{
			H5Gclose(NNarch);
			H5Fclose(root);
			ION_ABORT("ERROR: fail to open HDF5 dataset names");
		}
		layerNames_ = new unsigned int[dSize];
		ret = H5Dread(dsnames, H5T_NATIVE_UINT, H5S_ALL, H5S_ALL, H5P_DEFAULT, layerNames_);
		if (ret < 0)
		{
			delete [] layerNames_;
			layerNames_ = 0;
			ION_ABORT("ERROR: fail to read HDF5 dataset names");
		}
		H5Dclose(dsnames);
		hid_t dsdimin = H5Dopen(NNarch, "dimin", H5P_DEFAULT);  // open layer input dimension
		if (dsdimin < 0)
		{
			H5Gclose(NNarch);
			H5Fclose(root);
			ION_ABORT("ERROR: fail to open HDF5 dataset dimin");
		}
		layerDimIn_ = new unsigned int[dSize];
		ret = H5Dread(dsdimin, H5T_NATIVE_UINT, H5S_ALL, H5S_ALL, H5P_DEFAULT, layerDimIn_);
		if (ret < 0)
		{
			delete [] layerDimIn_;
			layerDimIn_ = 0;
			ION_ABORT("ERROR: fail to read HDF5 dataset dimin");
		}
		H5Dclose(dsdimin);
		hid_t dsdimout = H5Dopen(NNarch, "dimout", H5P_DEFAULT);  // open layer input dimension
		if (dsdimout < 0)
		{
			H5Gclose(NNarch);
			H5Fclose(root);
			ION_ABORT("ERROR: fail to open HDF5 dataset dimout");
		}
		layerDimOut_ = new unsigned int[dSize];
		ret = H5Dread(dsdimout, H5T_NATIVE_UINT, H5S_ALL, H5S_ALL, H5P_DEFAULT, layerDimOut_);
		if (ret < 0)
		{
			delete [] layerDimOut_;
			layerDimOut_ = 0;
			ION_ABORT("ERROR: fail to read HDF5 dataset dimout");
		}
		H5Dclose(dsdimout);
		H5Gclose(NNarch);
		// load weight
		hid_t NNweight = H5Gopen(root, "/weight", H5P_DEFAULT);   //open group weight
		if (NNweight < 0)
		{
			H5Fclose(root);
			ION_ABORT("ERROR: fail to open HDF5 group weight");
		}
		H5Gclose(NNweight);
		H5Fclose(root);

		for(unsigned int i = 0; i < dSize; i++){
			cout<< "Reading layer: "<< i<< endl;
			if((int)layerTypes_[i] == 1){  // "Dense layer"
				vector<int> dims(2); // support only two dimensions for now
				dims.at(0) = layerDimIn_[i];
				dims.at(1) = layerDimOut_[i];
				l = new DenseLayer();
				l->SetName((int)layerNames_[i]);
				l->SetDim(dims);
				l->LoadWeights(fname);
			}else if((int)layerTypes_[i] == 2) { // "Dropout layer"
				continue;
			}
			m_Layers.push_back(l);
		}
	}else{
		ION_ABORT("ERROR: The file is not an HDF5 file " + fname);
	}
}

void NN::DenseLayer::LoadWeights(const string &fname){
	hid_t root = H5Fopen(fname.c_str(), H5F_ACC_RDONLY, H5P_DEFAULT);  // open file
	hid_t NNweight = H5Gopen(root, "/weight", H5P_DEFAULT);   //open group weight
	if (NNweight < 0)
	{
		H5Fclose(root);
		ION_ABORT("ERROR: fail to open HDF5 group weight");
	}
	char buf[100];
	sprintf(buf, "weight%d", mName);
	hid_t dsweight = H5Dopen(NNweight, buf, H5P_DEFAULT);  // open weight dataset
	if (dsweight < 0)
	{
		H5Gclose(NNweight);
		H5Fclose(root);
		ION_ABORT("ERROR: fail to open HDF5 dataset weight");
	}
	hid_t filespace = H5Dget_space(dsweight);
	int dimension = H5Sget_simple_extent_ndims(filespace);
	hsize_t dim[dimension];
	int status = H5Sget_simple_extent_dims ( filespace, dim, NULL );
	if ( status <0 )
	{
		H5Sclose ( filespace );
	    ION_ABORT ( "Internal Error in H5Sget_simple_extent_dims - Read Weight" );
	}
	hsize_t size = H5Dget_storage_size(dsweight);
	size /= sizeof(float);
	cout << "Layer weight: "<<(int)dim[0]<< " * " << (int)dim[1] <<endl;
	float* weight = new float[size];
	herr_t ret = H5Dread(dsweight, H5T_NATIVE_FLOAT, H5S_ALL, H5S_ALL, H5P_DEFAULT, weight);
	H5Dclose(dsweight);
	if(ret < 0)
	{
		H5Gclose(NNweight);
		H5Fclose(root);
		ION_ABORT("ERROR: fail to read HDF5 attribute weight");
	}
	for(unsigned int i = 0; i < dim[0]; i++){
		vector<float> v(size/dim[0]);
		copy(weight + i * size/dim[0], weight + (i + 1) * size/dim[0], v.begin());
		mWeights.push_back(v);
	}
	delete [] weight;
 	// Load bias
	char buf1[100];
	sprintf(buf1, "bias%d", mName);
	hid_t dsbias = H5Dopen(NNweight, buf1, H5P_DEFAULT);  // open weight dataset
	if (dsbias < 0)
	{
		H5Gclose(NNweight);
		H5Fclose(root);
		ION_ABORT("ERROR: fail to open HDF5 dataset bias");
	}
	filespace = H5Dget_space(dsbias);
	dimension = H5Sget_simple_extent_ndims(filespace);
	size = H5Dget_storage_size(dsbias);
	size /= sizeof(float);
	float* bias = new float[size];
	ret = H5Dread(dsbias, H5T_NATIVE_FLOAT, H5S_ALL, H5S_ALL, H5P_DEFAULT, bias);
	H5Dclose(dsbias);
	mBias.resize(dim[1]);
	copy(bias , bias + dim[1], mBias.begin());
	delete [] bias;
}

NN::DataChunk* NN::DenseLayer::GetOutput(NN::DataChunk* pred){
	size_t size = mWeights[0].size();
	size_t size8 = size >> 3;
	NN::DataChunk *output = new NN::DataChunk(size, 0);
	float * x = pred->GetSetData().data();
	float * y = output->GetSetData().data();
	for(unsigned int i = 0; i < mWeights.size(); ++i){
		const float * w = mWeights[i].data();
		size_t k = 0;
		for(unsigned int j = 0; j < size8; ++j){
			y[k] += w[k] * x[i];
			y[k + 1] += w[k + 1] * x[i];
			y[k + 2] += w[k + 2] * x[i];
			y[k + 3] += w[k + 3] * x[i];
			y[k + 4] += w[k + 4] * x[i];
			y[k + 5] += w[k + 5] * x[i];
			y[k + 6] += w[k + 6] * x[i];
			y[k + 7] += w[k + 7] * x[i];
			k += 8;
		}
		while (k < size) { y[k] += w[k] * x[i]; ++k; }  // leftovers
	}

	for (unsigned int i = 0; i < size; ++i) {
	    y[i] += mBias[i];
	}
	return output;
}

void NN::DenseLayer::SetDim(const vector<int> dims){
	mDim1 = dims.at(0);
	mDim2 = dims.at(1);
}

void NN::DenseLayer::SetName(int layerName){
	mName = layerName;
}


///////////////////////////////////////////////////////////////////////////////
//
// ParVoro++: A scalable parallel algorithm for constructing 3D Voronoi tessellations based on KD-tree decomposition
//
// Guoqing Wu
// High Performance Computing Center, Institute of Applied Physics and Computational Mathematics, Beijing
// wu_guoqing@iapcm.ac.cn
//
///////////////////////////////////////////////////////////////////////////////
#ifndef __VORONOI_KD_H
#define __VORONOI_KD_H

#include "StandardIncludes.h"
#include "data/particle.hpp"
#include "data/common.hpp"
#include <voro++/voro++.hh>
#include <itree/avtIntervalTree.h>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/convex_hull_3.h>
#include <CGAL/Extreme_points_traits_adapter_3.h>

#include <boost/iterator/counting_iterator.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel       K;

template<typename RealType, unsigned NDIM>
void
convex_hull(ParticleBlock<RealType, NDIM>* b_,
            const diy::Master::ProxyWithLink& cp,
	    std::vector<std::set<size_t> >& ch_indices)
{
    typedef  diy::ContinuousBounds            Bounds;
    typedef  ParticleBlock<RealType, NDIM>    Block;
    typedef  diy::RegularContinuousLink       RCLink;

    Block*  b  = static_cast<Block*>(b_);
    RCLink* l  = dynamic_cast<RCLink*>(cp.link());
    int index = b->lid;

    std::vector<K::Point_3> points;
    for(size_t i = 0; i <  b->num_orig_particles; i++)
    {
        K::Point_3 p( b->particles[i][0],  b->particles[i][1],  b->particles[i][2]);
        points.push_back(p);
    }

    //This will contain the extreme vertices
    std::vector<size_t> extreme_point_indices;
    //call the function with the traits adapter for vertices
    CGAL::extreme_points_3(CGAL::make_range(boost::counting_iterator<size_t>(0),
                                          boost::counting_iterator<size_t>(points.size())),
                           std::back_inserter(extreme_point_indices),
                           CGAL::make_extreme_points_traits_adapter(CGAL::make_property_map(points)));

    std::copy(extreme_point_indices.begin(), extreme_point_indices.end(), std::inserter(ch_indices[index], ch_indices[index].begin()));
    for(int i=0; i<l->size(); i++)
    {
        for(std::set<size_t>::iterator it=ch_indices[index].begin(); it!=ch_indices[index].end(); ++it)
        {
            // send convex hull vertex to nearest block
            RemoteParticle<RealType, NDIM> rp;
            rp.particle[0] = (RealType)(b->particles[*it][0]);
            rp.particle[1] = (RealType)(b->particles[*it][1]);
            rp.particle[2] = (RealType)(b->particles[*it][2]);
            rp.info.bid = b->bid;
            rp.info.index = *it;
            cp.enqueue(l->target(i), rp);
        }
    }

}

template<typename RealType, unsigned NDIM>
int
incomplete_cells(ParticleBlock<RealType, NDIM>* b,
                 const diy::Master::ProxyWithLink& cp,
	         bool wrap,
                 voro::container* con[],
                 voro::particle_order* po[],
	         avtIntervalTree* itree_spatial,
	         diy::StaticAssigner& assigner,
                 std::vector<std::set<size_t> >& ch_indices)
{
    typedef diy::RegularContinuousLink  RCLink;
    RCLink* l = dynamic_cast<RCLink*>(cp.link());

    std::vector<std::set<int> > to_send( b->num_orig_particles);

    std::set<int> neighbors;
    for(int i = 0; i < l->size(); i++)
        neighbors.insert(l->target(i).gid);

    int index = b->lid;
    int pid;
    voro::voronoicell c;
    double x, y, z, r;
    double center[3];
    RemoteParticle<float, NDIM> rp;
    voro::c_loop_order clo(*con[index],*po[index]);
    if(clo.start()) do if((con[index]->compute_cell(c, clo)))
    {
        clo.pos(pid, x, y, z, r);

        center[0] = x;
        center[1] = y;
        center[2] = z;

        std::vector<double> v;
        c.vertices(x,y,z,v);
        for(int vi = 0; vi < c.p; vi++)
        {
            double verts[3];
            verts[0] = v[3*vi];
            verts[1] = v[3*vi+1];
            verts[2] = v[3*vi+2];
            double rad = distance(center, verts) + EPSILON;

            // check if the circumsphere is too deep inside the block to be able to stick out
	    int j;
            for(j = 0; j < 3; ++j)
            {
                if(verts[j] - b->bounds.min[j] <= rad) break;
                if(b->bounds.max[j] - verts[j] <= rad) break;
            }
            if(j == 3)	// the circumsphere is too deep inside the block
                continue;

	    // computing bounding box of the circumsphere
    	    double min_vec[3], max_vec[3];
            for(int i = 0; i < 3; i++)
            {
	        min_vec[i] = verts[i] - rad;
	        max_vec[i] = verts[i] + rad;
            }

	    // no wrap, NB: wrap case includes no wrap
	    {
                std::vector<int> list;
                itree_spatial->GetElementsListFromRange(min_vec, max_vec, list); //基于区间树查找相交box
                for(int i = 0; i < list.size(); ++i)
                {
		    if(list[i] != b->bid.gid) // do not send to itself
		        to_send[pid].insert(list[i]);
	        }
	    }

    	    if(wrap)
	    {
                std::vector<int> wrap_dir(3,0);
		int flag[3] = {0, 0, 0};
	  	if(min_vec[0] <= b->global_bounds.min[0]) // -X
		{
		    wrap_dir[0] = 1;
		    flag[0]++;
		}
	  	if(min_vec[1] <= b->global_bounds.min[1]) // -Y
		{
		    wrap_dir[1] = 1;
		    flag[1]++;
		}
	  	if(min_vec[2] <= b->global_bounds.min[2]) // -Z
		{
		    wrap_dir[2] = 1;
		    flag[2]++;
		}
	  	if(max_vec[0] >= b->global_bounds.max[0]) // +X
		{
		    wrap_dir[0] = -1;
		    flag[0]++;
		}
	  	if(max_vec[1] >= b->global_bounds.max[1]) // +Y
		{
		    wrap_dir[1] = -1;
		    flag[1]++;
		}
	  	if(max_vec[2] >= b->global_bounds.max[2]) // +Z
		{
		    wrap_dir[2] = -1;
		    flag[2]++;
		}

                // handle the weird case, where the box intersects both sides
	        wrap_dir[0] = (flag[0]>1) ? 0 : wrap_dir[0];
		wrap_dir[1] = (flag[1]>1) ? 0 : wrap_dir[1];
		wrap_dir[2] = (flag[2]>1) ? 0 : wrap_dir[2];

		for(int x = 0; x < 2; x++)
		    for(int y = 0; y < 2; y++)
		        for(int z = 0; z < 2; z++)
			{
			    if(x==0 && y==0 && z==0) // no wrap case
				continue;

			    std::vector<int> dir(3,0);
			    dir[0] = x * wrap_dir[0];
			    dir[1] = y * wrap_dir[1];
			    dir[2] = z * wrap_dir[2];

    	    		    double min_[3], max_[3];
            		    for(int i = 0; i < 3; i++)
            		    {
	                        min_[i] = min_vec[i];
	                        max_[i] = max_vec[i];
                            }
			    for(int i = 0; i < 3; ++i)
                            {
                                min_[i] += dir[i] * (b->global_bounds.max[i] - b->global_bounds.min[i]);
                                max_[i] += dir[i] * (b->global_bounds.max[i] - b->global_bounds.min[i]);
                            }
                	    std::vector<int> list;
                            itree_spatial->GetElementsListFromRange(min_, max_, list); //基于区间树查找相交box
                            for(int i = 0; i < list.size(); ++i)
                            {
		                if(list[i] != b->bid.gid) // do not send to itself
		                to_send[pid].insert(list[i]);
	                    }
			}
	    }//if(wrap)
        }//for vi
    }while(clo.inc());

    // enqueue the particles
    size_t enqueued = 0;
    for(int p = 0; p <  b->num_orig_particles; p++)
    {
        for(std::set<int>::iterator it  = to_send[p].begin(); it != to_send[p].end(); it++)
        {
	    if(ch_indices[b->lid].count(p) && neighbors.count(*it)) // already send as convex hull particle
	        continue;

            rp.particle =  b->particles[p];
            rp.info.bid = b->bid;
            rp.info.index = p;
            diy::BlockID bid(*it, assigner.rank(*it));
            cp.enqueue(bid, rp);
            ++enqueued;
        }
    }

    return enqueued;
}

template<typename RealType, unsigned NDIM>
void
local_cells(ParticleBlock<RealType, NDIM>* b,
            const diy::Master::ProxyWithLink& cp,
            bool wrap,
            voro::container* con[],
            voro::particle_order* po[])
{
    int index = b->lid;
    int nx,ny,nz;
    voro::pre_container pcon(b->global_bounds.min[0], b->global_bounds.max[0], b->global_bounds.min[1], b->global_bounds.max[1], b->global_bounds.min[2], b->global_bounds.max[2], wrap, wrap, wrap);
    for(int j = 0; j < b->num_orig_particles; j++)
        pcon.put(j,  b->particles[j][0],  b->particles[j][1],  b->particles[j][2]);
    pcon.guess_optimal(nx,ny,nz);
    con[index] = new voro::container(b->global_bounds.min[0], b->global_bounds.max[0], b->global_bounds.min[1], b->global_bounds.max[1], b->global_bounds.min[2], b->global_bounds.max[2], 2*nx, 2*ny, 2*nz, wrap, wrap, wrap, 8);
    po[index] = new voro::particle_order();
    pcon.setup(*po[index], *con[index]);

    for(int j = b->num_orig_particles; j < b->num_particles; j++)
        con[index]->put(j, b->particles[j][0], b->particles[j][1], b->particles[j][2]);
}

template<typename RealType, unsigned NDIM>
void
update_cells(ParticleBlock<RealType, NDIM>* b,
             const diy::Master::ProxyWithLink& cp,
             bool wrap,
             voro::container* con[],
             voro::particle_order* po[],
             bool test)
{
    int index = b->lid;
    for(int j = con[index]->total_particles(); j < b->num_particles; j++)
        con[index]->put(j, b->particles[j][0], b->particles[j][1], b->particles[j][2]);

    // Add a wall to the container
    //voro::wall_cylinder wall(15,15,15,0,0,1,10);
    //voro::wall_sphere wall(15, 15, 15, 5);
    //con[index]->add_wall(wall);

    if(test)
        con[index]->compute_all_cells();
}

template<typename RealType, unsigned NDIM>
void
Voronoi_kd(diy::Master& master,
           bool wrap,
           voro::container* con[],
           voro::particle_order* po[],
	   avtIntervalTree* itree_spatial,
	   diy::StaticAssigner& assigner,
           bool test)
{
    std::vector<std::set<size_t> > ch_indices(master.size()); // NB: multi-blocks
    master.foreach([&](ParticleBlock<RealType, NDIM>* b, const diy::Master::ProxyWithLink & cp)
                  {convex_hull<RealType, NDIM>(b, cp, ch_indices);});
    master.exchange();
    master.foreach(&recv_particles<RealType, NDIM>);

    master.foreach([&](ParticleBlock<RealType, NDIM>* b, const diy::Master::ProxyWithLink & cp)
                  {local_cells<RealType, NDIM>(b, cp, wrap, con, po);});

    master.foreach([&](ParticleBlock<RealType, NDIM>* b, const diy::Master::ProxyWithLink & cp)
                  {incomplete_cells<RealType, NDIM>(b, cp, wrap, con, po, itree_spatial, assigner, ch_indices);});
    bool remote = true;  // NB: (remote) exchange some data outside the links
    master.exchange(remote);
    master.foreach(&recv_particles<RealType, NDIM>);

    master.foreach([&](ParticleBlock<RealType, NDIM>* b, const diy::Master::ProxyWithLink & cp)
                  {update_cells<RealType, NDIM>(b, cp, wrap, con, po, test);});
}

#endif


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use of the extract_fhog_features() routine from
    the dlib C++ Library.

    The extract_fhog_features() routine performs the style of HOG feature extraction
    described in the paper:
        Object Detection with Discriminatively Trained Part Based Models by
        P. Felzenszwalb, R. Girshick, D. McAllester, D. Ramanan
        IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol. 32, No. 9, Sep. 2010
    This means that it takes an input image and outputs Felzenszwalb's
    31 dimensional version of HOG features.  We show its use below.
*/

#include <dlib/gui_widgets.h>
#include <dlib/image_io.h>
#include <dlib/image_transforms.h>

using namespace std;
using namespace dlib;

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

#if defined(BUILD_MONOLITHIC)
#define main(cnt, arr)      dlib_fhog_ex_main(cnt, arr)
#endif

int main(int argc, const char** argv)
{
    try
    {
        // Make sure the user entered an argument to this program.  It should be the
        // filename for an image.
        if (argc != 2)
        {
            cout << "error, you have to enter a BMP file as an argument to this program" << endl;
            return 1;
        }

        // Here we declare an image object that can store color rgb_pixels.
        array2d<rgb_pixel> img;

        // Now load the image file into our image.  If something is wrong then
        // load_image() will throw an exception.  Also, if you linked with libpng
        // and libjpeg then load_image() can load PNG and JPEG files in addition
        // to BMP files.
        load_image(img, argv[1]);

        // Now convert the image into a FHOG feature image.  The output, hog, is a 2D array
        // of 31 dimensional vectors.
        array2d<matrix<float,31,1> > hog;
        extract_fhog_features(img, hog);

        cout << "hog image has " << hog.nr() << " rows and " << hog.nc() << " columns." << endl;

        // Let's see what the image and FHOG features look like.
        image_window win(img);
        image_window winhog(draw_fhog(hog));

        // Another thing you might want to do is map between the pixels in img and the
        // cells in the hog image.  dlib provides the image_to_fhog() and fhog_to_image()
        // routines for this.  Their use is demonstrated in the following loop which
        // responds to the user clicking on pixels in the image img.
        point p;  // A 2D point, used to represent pixel locations.
        while (win.get_next_double_click(p))
        {
            point hp = image_to_fhog(p);
            cout << "The point " << p << " in the input image corresponds to " << hp << " in hog space." << endl;
            cout << "FHOG features at this point: " << trans(hog[hp.y()][hp.x()]) << endl;
        }

        // Finally, sometimes you want to get a planar representation of the HOG features
        // rather than the explicit vector (i.e. interlaced) representation used above.
        dlib::array<array2d<float> > planar_hog;
        extract_fhog_features(img, planar_hog);
        // Now we have an array of 31 float valued image planes, each representing one of
        // the dimensions of the HOG feature vector.
    }
    catch (exception& e)
    {
        cout << "exception thrown: " << e.what() << endl;
    }
}

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


//==================================================================================================
/**
  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)
**/
//==================================================================================================
#include <boost/simd/function/ifrexp.hpp>
#include <boost/simd/function/pedantic.hpp>
#include <boost/simd/detail/constant/minexponent.hpp>
#include <boost/simd/constant/halfeps.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/pack.hpp>
#include <utility>

#include <simd_test.hpp>

namespace bs = boost::simd;
namespace bd = boost::dispatch;

template <typename T, std::size_t N, typename Env>
void test(Env& $)
{
  using iT   = bd::as_integer_t<T>;
  using p_iT = bs::pack<iT, N>;
  using p_T  = bs::pack< T, N>;

  T a1[N], m[N];
  iT e[N];

  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(1+i) : -T(1+i);
    std::tie(m[i], e[i])   = bs::pedantic_(bs::ifrexp)(a1[i]);
  }

  p_T  in(&a1[0], &a1[0]+N);
  p_T  mm( &m[0],  &m[0]+N);
  p_iT ee( &e[0],  &e[0]+N);

  auto that = bs::pedantic_(bs::ifrexp)(in);

  STF_EQUAL(that.first, mm);
  STF_EQUAL(that.second, ee);
}

STF_CASE_TPL("Check basic behavior of pedantic(ifrexp) on pack" , STF_IEEE_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;
  test<T, N>($);
  test<T, N/2>($);
  test<T, N*2>($);
}

STF_CASE_TPL("Check behavior of pedantic_(ifrexp) on Zero", STF_IEEE_TYPES)
{
  using iT = bd::as_integer_t<T>;
  auto r = bs::pedantic_(bs::ifrexp)(bs::pack<T>(0));

  STF_EQUAL (r.first , bs::pack< T>(0));
  STF_EQUAL (r.second, bs::pack<iT>(0));
}

STF_CASE_TPL("Check behavior of pedantic_(ifrexp) on Valmax", STF_IEEE_TYPES)
{
  auto r = bs::pedantic_(bs::ifrexp)(bs::Valmax<bs::pack<T>>());

  STF_ULP_EQUAL (r.first , 1-bs::Halfeps<bs::pack<T>>(), 1);
  STF_EQUAL (r.second, bs::Limitexponent<bs::pack<T>>());
}

#ifndef BOOST_SIMD_NO_INVALID
#include <boost/simd/constant/nan.hpp>
STF_CASE_TPL("Check behavior of pedantic_(ifrexp) on NaN", STF_IEEE_TYPES)
{
  using iT = bd::as_integer_t<T>;
  auto r = bs::pedantic_(bs::ifrexp)(bs::Nan<bs::pack<T>>());

  STF_IEEE_EQUAL(r.first , bs::Nan<bs::pack<T>>());
  STF_EQUAL     (r.second, bs::pack<iT>(0));
}
#endif

#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>

STF_CASE_TPL("Check behavior of pedantic_(ifrexp) on infinites", STF_IEEE_TYPES)
{
  using iT = bd::as_integer_t<T>;
  auto r = bs::pedantic_(bs::ifrexp)(bs::Inf<bs::pack<T>>());
  auto q = bs::pedantic_(bs::ifrexp)(bs::Minf<bs::pack<T>>());

  STF_IEEE_EQUAL(r.first , bs::Inf<bs::pack<T>>());
  STF_EQUAL     (r.second, bs::pack<iT>(0));

  STF_IEEE_EQUAL(q.first , bs::Minf<bs::pack<T>>());
  STF_EQUAL     (q.second, bs::pack<iT>(0));
}

#endif

#ifndef BOOST_SIMD_NO_DENORMALS
#include <boost/simd/detail/constant/minexponent.hpp>
#include <boost/simd/constant/mindenormal.hpp>

STF_CASE_TPL("Check behavior of pedantic_(ifrexp) on denormals", STF_IEEE_TYPES)
{
  auto r = bs::pedantic_(bs::ifrexp)(bs::Mindenormal<bs::pack<T>>());

  STF_ULP_EQUAL(r.first , bs::pack<T>(0.5), 1);
  STF_EQUAL     (r.second, bs::Minexponent<bs::pack<T>>()-bs::Nbmantissabits<bs::pack<T>>()+1);
}

#endif


/**
 * \copyright
 * Copyright (c) 2012-2018, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/LICENSE.txt
 */

#include <gtest/gtest.h>

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

#include <Eigen/Eigen>

#include "GeoLib/AnalyticalGeometry.h"
#include "MathLib/LinAlg/Dense/DenseMatrix.h"

#include "MeshLib/CoordinateSystem.h"
#include "MeshLib/Node.h"
#include "MeshLib/Elements/Element.h"
#include "MeshLib/Elements/Line.h"
#include "MeshLib/Elements/Quad.h"
#include "MeshLib/ElementCoordinatesMappingLocal.h"

#include "Tests/TestTools.h"

namespace
{

namespace TestLine2
{
using ElementType = MeshLib::Line;
const unsigned e_nnodes = ElementType::n_all_nodes;

std::unique_ptr<MeshLib::Line> createLine(std::array<double, 3> const& a,
                                          std::array<double, 3> const& b)
{
    auto** nodes = new MeshLib::Node*[e_nnodes];
    nodes[0] = new MeshLib::Node(a);
    nodes[1] = new MeshLib::Node(b);
    return std::make_unique<MeshLib::Line>(nodes);
    }

    std::unique_ptr<MeshLib::Line> createY()
    {
        return createLine({{0.0, -1.0, 0.0}}, {{0.0,  1.0, 0.0}});
    }

    std::unique_ptr<MeshLib::Line> createZ()
    {
        return createLine({{0.0, 0.0, -1.0}}, {{0.0, 0.0,  1.0}});
    }

    std::unique_ptr<MeshLib::Line> createXY()
    {
        // 45degree inclined
        return createLine({{0.0, 0.0, 0.0}}, {{2./sqrt(2), 2./sqrt(2), 0.0}});
    }

    std::unique_ptr<MeshLib::Line> createXYZ()
    {
        return createLine({{0.0, 0.0, 0.0}}, {{2./sqrt(3), 2./sqrt(3), 2./sqrt(3)}});
    }

};

namespace TestQuad4
{
    // Element information
using ElementType = MeshLib::Quad;
const unsigned e_nnodes = ElementType::n_all_nodes;

std::unique_ptr<MeshLib::Quad> createQuad(std::array<double, 3> const& a,
                                          std::array<double, 3> const& b,
                                          std::array<double, 3> const& c,
                                          std::array<double, 3> const& d)
{
    auto** nodes = new MeshLib::Node*[e_nnodes];
    nodes[0] = new MeshLib::Node(a);
    nodes[1] = new MeshLib::Node(b);
    nodes[2] = new MeshLib::Node(c);
    nodes[3] = new MeshLib::Node(d);
    return std::make_unique<MeshLib::Quad>(nodes);
    }

    // 2.5D case: inclined
    std::unique_ptr<MeshLib::Quad> createXYZ()
    {
        // rotate 45 degree around x axis
        return createQuad(
            {{ 1.0,  0.7071067811865475,  0.7071067811865475}},
            {{-1.0,  0.7071067811865475,  0.7071067811865475}},
            {{-1.0, -0.7071067811865475, -0.7071067811865475}},
            {{ 1.0, -0.7071067811865475, -0.7071067811865475}});
    }

    // 2.5D case: inclined
    std::unique_ptr<MeshLib::Quad> createXZ()
    {
        return createQuad(
            {{ 1.0, 0.0,  1.0}},
            {{-1.0, 0.0,  1.0}},
            {{-1.0, 0.0, -1.0}},
            {{ 1.0, 0.0, -1.0}});
    }

    // 2.5D case: inclined
    std::unique_ptr<MeshLib::Quad> createYZ()
    {
        return createQuad(
            {{0.0,  1.0,  1.0}},
            {{0.0, -1.0,  1.0}},
            {{0.0, -1.0, -1.0}},
            {{0.0,  1.0, -1.0}});
    }
};

#if 0
// keep this function for debugging
void debugOutput(MeshLib::Element *ele, MeshLib::ElementCoordinatesMappingLocal &mapping)
{
    std::cout.precision(12);
    std::cout << "original" << std::endl;
    for (unsigned i=0; i<ele->getNumberOfNodes(); i++)
        std::cout << *ele->getNode(i) << std::endl;
    std::cout << "local coords=" << std::endl;
    for (unsigned i=0; i<ele->getNumberOfNodes(); i++)
        std::cout << *mapping.getMappedCoordinates(i) << std::endl;
    std::cout << "R=\n" << mapping.getRotationMatrixToGlobal() << std::endl;
    auto matR(mapping.getRotationMatrixToGlobal());
    std::cout << "global coords=" << std::endl;
    for (unsigned i=0; i<ele->getNumberOfNodes(); i++) {
        double* raw = const_cast<double*>(&(*mapping.getMappedCoordinates(i))[0]);
        Eigen::Map<Eigen::Vector3d> v(raw);
        std::cout << (matR*v).transpose() << std::endl;
    }
}
#endif

// check if using the rotation matrix results in the original coordinates
#define CHECK_COORDS(ele, mapping)\
    for (unsigned ii=0; ii<(ele)->getNumberOfNodes(); ii++) {\
        MathLib::Point3d global(matR*(mapping).getMappedCoordinates(ii));\
        const double eps(std::numeric_limits<double>::epsilon());\
        ASSERT_ARRAY_NEAR(&(*(ele)->getNode(ii))[0], global.getCoords(), 3u, eps);\
    }

} //namespace

TEST(MeshLib, CoordinatesMappingLocalLowerDimLineY)
{
    auto ele = TestLine2::createY();
    MeshLib::ElementCoordinatesMappingLocal mapping(
        *ele, MeshLib::CoordinateSystem(MeshLib::CoordinateSystemType::Y)
                  .getDimension());
    auto matR(mapping.getRotationMatrixToGlobal());
    //debugOutput(ele, mapping);

    double exp_R[3*3] = {0, -1, 0,
                         1,  0, 0,
                         0,  0, 1};
    const double eps(std::numeric_limits<double>::epsilon());
    ASSERT_ARRAY_NEAR(exp_R, matR.data(), matR.size(), eps);
    CHECK_COORDS(ele,mapping);

    for (std::size_t n = 0; n < ele->getNumberOfNodes(); ++n)
        delete ele->getNode(n);
}

TEST(MeshLib, CoordinatesMappingLocalLowerDimLineZ)
{
    auto ele = TestLine2::createZ();
    MeshLib::ElementCoordinatesMappingLocal mapping(
        *ele,
        MeshLib::CoordinateSystem(MeshLib::CoordinateSystemType::Z)
            .getDimension());
    auto matR(mapping.getRotationMatrixToGlobal());
    //debugOutput(ele, mapping);

    double exp_R[3*3] = {0, 0, -1, 0, 1, 0, 1, 0, 0};
    const double eps(std::numeric_limits<double>::epsilon());
    ASSERT_ARRAY_NEAR(exp_R, matR.data(), matR.size(), eps);
    CHECK_COORDS(ele,mapping);

    for (std::size_t n = 0; n < ele->getNumberOfNodes(); ++n)
        delete ele->getNode(n);
}

TEST(MeshLib, CoordinatesMappingLocalLowerDimLineXY)
{
    auto ele = TestLine2::createXY();
    MeshLib::ElementCoordinatesMappingLocal mapping(
        *ele, MeshLib::CoordinateSystem(*ele).getDimension());
    auto matR(mapping.getRotationMatrixToGlobal());
    //debugOutput(ele, mapping);

    double exp_R[3*3] = {0.70710678118654757, -0.70710678118654757, 0,
                         0.70710678118654757,  0.70710678118654757, 0,
                         0,                    0,                   1};
    const double eps(std::numeric_limits<double>::epsilon());
    ASSERT_ARRAY_NEAR(exp_R, matR.data(), matR.size(), eps);
    CHECK_COORDS(ele,mapping);

    for (std::size_t n = 0; n < ele->getNumberOfNodes(); ++n)
        delete ele->getNode(n);
}

TEST(MeshLib, CoordinatesMappingLocalLowerDimLineXYZ)
{
    auto ele = TestLine2::createXYZ();
    MeshLib::ElementCoordinatesMappingLocal mapping(
        *ele, MeshLib::CoordinateSystem(*ele).getDimension());
    auto matR(mapping.getRotationMatrixToGlobal());
    //debugOutput(ele, mapping);

    double exp_R[3*3] = {0.57735026918962584, -0.81649658092772626,  0,
                         0.57735026918962584,  0.40824829046386313, -0.70710678118654757,
                         0.57735026918962584,  0.40824829046386313,  0.70710678118654757};
    const double eps(std::numeric_limits<double>::epsilon());
    ASSERT_ARRAY_NEAR(exp_R, matR.data(), matR.size(), eps);
    CHECK_COORDS(ele,mapping);

    for (std::size_t n = 0; n < ele->getNumberOfNodes(); ++n)
        delete ele->getNode(n);
}

TEST(MeshLib, CoordinatesMappingLocalLowerDimQuadXZ)
{
    auto ele = TestQuad4::createXZ();
    MeshLib::ElementCoordinatesMappingLocal mapping(
        *ele, MeshLib::CoordinateSystem(*ele).getDimension());
    auto matR(mapping.getRotationMatrixToGlobal());
    //debugOutput(ele, mapping);

    // results when using GeoLib::ComputeRotationMatrixToXY()
    double exp_R[3*3] = {  1, 0,  0,
                           0, 0, -1,
                           0, 1,  0};

    const double eps(std::numeric_limits<double>::epsilon());
    ASSERT_ARRAY_NEAR(exp_R, matR.data(), matR.size(), eps);
    CHECK_COORDS(ele,mapping);

    for (std::size_t n = 0; n < ele->getNumberOfNodes(); ++n)
        delete ele->getNode(n);
}

TEST(MeshLib, CoordinatesMappingLocalLowerDimQuadYZ)
{
    auto ele = TestQuad4::createYZ();
    MeshLib::ElementCoordinatesMappingLocal mapping(
        *ele, MeshLib::CoordinateSystem(*ele).getDimension());
    auto matR(mapping.getRotationMatrixToGlobal());
    //debugOutput(ele, mapping);

    // results when using GeoLib::ComputeRotationMatrixToXY()
    double exp_R[3*3] = { 0, 0, 1,
                          0, 1, 0,
                         -1, 0, 0};

    const double eps(std::numeric_limits<double>::epsilon());
    ASSERT_ARRAY_NEAR(exp_R, matR.data(), matR.size(), eps);
    CHECK_COORDS(ele,mapping);

    for (std::size_t n = 0; n < ele->getNumberOfNodes(); ++n)
        delete ele->getNode(n);
}

TEST(MeshLib, CoordinatesMappingLocalLowerDimQuadXYZ)
{
    auto ele = TestQuad4::createXYZ();
    MeshLib::ElementCoordinatesMappingLocal mapping(
        *ele, MeshLib::CoordinateSystem(*ele).getDimension());
    auto matR(mapping.getRotationMatrixToGlobal());
    //debugOutput(ele, mapping);

    // results when using GeoLib::ComputeRotationMatrixToXY()
    double exp_R[3*3] = {  1, 0, 0,
                           0, 0.70710678118654757, -0.70710678118654757,
                           0, 0.70710678118654757,  0.70710678118654757};

    const double eps(std::numeric_limits<double>::epsilon());
    ASSERT_ARRAY_NEAR(exp_R, matR.data(), matR.size(), eps);
    CHECK_COORDS(ele,mapping);

    for (std::size_t n = 0; n < ele->getNumberOfNodes(); ++n)
        delete ele->getNode(n);
}