// Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef UUID_384AFF3AD23A11DFA80B754FE0D72085
#define UUID_384AFF3AD23A11DFA80B754FE0D72085

#include <boost/qvm/assert.hpp>
#include <boost/qvm/detail/vec_assign.hpp>
#include <boost/qvm/scalar_traits.hpp>
#include <boost/qvm/vec_operations2.hpp>
#include <boost/qvm/vec_operations3.hpp>
#include <boost/qvm/vec_operations4.hpp>
#include <string>

namespace boost {
namespace qvm {
namespace qvm_detail {
BOOST_QVM_INLINE_CRITICAL
void const *get_valid_ptr_vec_operations() {
  static int const obj = 0;
  return &obj;
}
} // namespace qvm_detail

////////////////////////////////////////////////

namespace qvm_to_string_detail {
template <class T> std::string to_string(T const &x);
}

namespace qvm_detail {
template <int D> struct to_string_v_defined {
  static bool const value = false;
};

template <int I, int DimMinusOne> struct to_string_vector_elements {
  template <class A> static std::string f(A const &a) {
    using namespace qvm_to_string_detail;
    return to_string(vec_traits<A>::template read_element<I>(a)) + ',' +
           to_string_vector_elements<I + 1, DimMinusOne>::f(a);
  }
};

template <int DimMinusOne>
struct to_string_vector_elements<DimMinusOne, DimMinusOne> {
  template <class A> static std::string f(A const &a) {
    using namespace qvm_to_string_detail;
    return to_string(vec_traits<A>::template read_element<DimMinusOne>(a));
  }
};
} // namespace qvm_detail

template <class A>
inline typename boost::enable_if_c<
    is_vec<A>::value &&
        !qvm_detail::to_string_v_defined<vec_traits<A>::dim>::value,
    std::string>::type
to_string(A const &a) {
  return '(' +
         qvm_detail::to_string_vector_elements<0, vec_traits<A>::dim - 1>::f(
             a) +
         ')';
}

////////////////////////////////////////////////

namespace qvm_detail {
template <int D> struct convert_to_v_defined {
  static bool const value = false;
};
} // namespace qvm_detail

template <class R, class A>
BOOST_QVM_INLINE_TRIVIAL typename enable_if_c<
    is_vec<R>::value && is_vec<A>::value &&
        vec_traits<R>::dim == vec_traits<A>::dim &&
        !qvm_detail::convert_to_v_defined<vec_traits<R>::dim>::value,
    R>::type
convert_to(A const &a) {
  R r;
  assign(r, a);
  return r;
}

////////////////////////////////////////////////

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<is_vec<A>::value && is_vec<B>::value &&
                                  vec_traits<A>::dim == 3 &&
                                  vec_traits<B>::dim == 3,
                              deduce_vec2<A, B, 3>>::type
    cross(A const &a, B const &b) {
  typedef typename deduce_vec2<A, B, 3>::type R;
  R r;
  vec_traits<R>::template write_element<0>(r) =
      vec_traits<A>::template read_element<1>(a) *
          vec_traits<B>::template read_element<2>(b) -
      vec_traits<A>::template read_element<2>(a) *
          vec_traits<B>::template read_element<1>(b);
  vec_traits<R>::template write_element<1>(r) =
      vec_traits<A>::template read_element<2>(a) *
          vec_traits<B>::template read_element<0>(b) -
      vec_traits<A>::template read_element<0>(a) *
          vec_traits<B>::template read_element<2>(b);
  vec_traits<R>::template write_element<2>(r) =
      vec_traits<A>::template read_element<0>(a) *
          vec_traits<B>::template read_element<1>(b) -
      vec_traits<A>::template read_element<1>(a) *
          vec_traits<B>::template read_element<0>(b);
  return r;
}

////////////////////////////////////////////////

template <class A, class B, class Cmp>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<is_vec<A>::value && is_vec<B>::value &&
                             vec_traits<A>::dim == vec_traits<B>::dim,
                         bool>::type
    cmp(A const &a, B const &b, Cmp f) {
  typedef typename deduce_scalar<typename vec_traits<A>::scalar_type,
                                 typename vec_traits<B>::scalar_type>::type T;
  int const dim = vec_traits<A>::dim;
  T v1[dim];
  assign(v1, a);
  T v2[dim];
  assign(v2, b);
  for (int i = 0; i != dim; ++i)
    if (!f(v1[i], v2[i]))
      return false;
  return true;
}

////////////////////////////////////////////////

namespace qvm_detail {
template <class T, int Dim> class zero_vec_ {
  zero_vec_(zero_vec_ const &);
  zero_vec_ &operator=(zero_vec_ const &);
  ~zero_vec_();

public:
  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }
};
} // namespace qvm_detail

template <class V> struct vec_traits;

template <class T, int Dim> struct vec_traits<qvm_detail::zero_vec_<T, Dim>> {
  typedef qvm_detail::zero_vec_<T, Dim> this_vector;
  typedef T scalar_type;
  static int const dim = Dim;

  template <int I>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_vector const &) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < Dim);
    return scalar_traits<scalar_type>::value(0);
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element_idx(int i, this_vector const &) {
    BOOST_QVM_ASSERT(i >= 0);
    BOOST_QVM_ASSERT(i < Dim);
    return scalar_traits<scalar_type>::value(0);
  }
};

template <class T, int Dim, int D>
struct deduce_vec<qvm_detail::zero_vec_<T, Dim>, D> {
  typedef vec<T, D> type;
};

template <class T, int Dim>
BOOST_QVM_INLINE_TRIVIAL qvm_detail::zero_vec_<T, Dim> const &zero_vec() {
  return *(qvm_detail::zero_vec_<T, Dim> const *)
      qvm_detail::get_valid_ptr_vec_operations();
}

template <class A>
BOOST_QVM_INLINE_OPERATIONS typename enable_if_c<is_vec<A>::value, void>::type
set_zero(A &a) {
  assign(a,
         zero_vec<typename vec_traits<A>::scalar_type, vec_traits<A>::dim>());
}

////////////////////////////////////////////////

namespace qvm_detail {
template <class OriginalType, class Scalar> class vector_scalar_cast_ {
  vector_scalar_cast_(vector_scalar_cast_ const &);
  vector_scalar_cast_ &operator=(vector_scalar_cast_ const &);
  ~vector_scalar_cast_();

public:
  template <class T>
  BOOST_QVM_INLINE_TRIVIAL vector_scalar_cast_ &operator=(T const &x) {
    assign(*this, x);
    return *this;
  }

  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }
};

template <bool> struct scalar_cast_vector_filter {};
template <> struct scalar_cast_vector_filter<true> { typedef int type; };
} // namespace qvm_detail

template <class OriginalType, class Scalar>
struct vec_traits<qvm_detail::vector_scalar_cast_<OriginalType, Scalar>> {
  typedef Scalar scalar_type;
  typedef qvm_detail::vector_scalar_cast_<OriginalType, Scalar> this_vector;
  static int const dim = vec_traits<OriginalType>::dim;

  template <int I>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_vector const &x) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < dim);
    return scalar_type(vec_traits<OriginalType>::template read_element<I>(
        reinterpret_cast<OriginalType const &>(x)));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element_idx(int i, this_vector const &x) {
    BOOST_QVM_ASSERT(i >= 0);
    BOOST_QVM_ASSERT(i < dim);
    return scalar_type(vec_traits<OriginalType>::read_element_idx(
        i, reinterpret_cast<OriginalType const &>(x)));
  }
};

template <class OriginalType, class Scalar, int D>
struct deduce_vec<qvm_detail::vector_scalar_cast_<OriginalType, Scalar>, D> {
  typedef vec<Scalar, D> type;
};

template <class Scalar, class T>
BOOST_QVM_INLINE_TRIVIAL qvm_detail::vector_scalar_cast_<T, Scalar> const &
scalar_cast(
    T const &x,
    typename qvm_detail::scalar_cast_vector_filter<is_vec<T>::value>::type =
        0) {
  return reinterpret_cast<qvm_detail::vector_scalar_cast_<T, Scalar> const &>(
      x);
}

////////////////////////////////////////////////

namespace qvm_detail {
template <int D> struct div_eq_vs_defined { static bool const value = false; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS typename enable_if_c<
    is_vec<A>::value && is_scalar<B>::value &&
        !qvm_detail::div_eq_vs_defined<vec_traits<A>::dim>::value,
    A &>::type
operator/=(A &a, B b) {
  for (int i = 0; i != vec_traits<A>::dim; ++i)
    vec_traits<A>::write_element_idx(i, a) /= b;
  return a;
}

////////////////////////////////////////////////

namespace qvm_detail {
template <int D> struct div_vs_defined { static bool const value = false; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS typename lazy_enable_if_c<
    is_vec<A>::value && is_scalar<B>::value &&
        !qvm_detail::div_vs_defined<vec_traits<A>::dim>::value,
    deduce_vec<A>>::type
operator/(A const &a, B b) {
  typedef typename deduce_vec<A>::type R;
  R r;
  for (int i = 0; i != vec_traits<A>::dim; ++i)
    vec_traits<R>::write_element_idx(i, r) =
        vec_traits<A>::read_element_idx(i, a) / b;
  return r;
}

////////////////////////////////////////////////

namespace qvm_detail {
template <int D> struct dot_vv_defined { static bool const value = false; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS typename lazy_enable_if_c<
    is_vec<A>::value && is_vec<B>::value &&
        vec_traits<A>::dim == vec_traits<B>::dim &&
        !qvm_detail::dot_vv_defined<vec_traits<A>::dim>::value,
    deduce_scalar<typename vec_traits<A>::scalar_type,
                  typename vec_traits<B>::scalar_type>>::type
dot(A const &a, B const &b) {
  typedef typename deduce_scalar<typename vec_traits<A>::scalar_type,
                                 typename vec_traits<B>::scalar_type>::type T;
  T m(scalar_traits<T>::value(0));
  for (int i = 0; i != vec_traits<A>::dim; ++i)
    m += vec_traits<A>::read_element_idx(i, a) *
         vec_traits<B>::read_element_idx(i, b);
  return m;
}

////////////////////////////////////////////////

namespace qvm_detail {
template <int D> struct eq_vv_defined { static bool const value = false; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS typename enable_if_c<
    is_vec<A>::value && is_vec<B>::value &&
        vec_traits<A>::dim == vec_traits<B>::dim &&
        !qvm_detail::eq_vv_defined<vec_traits<A>::dim>::value,
    bool>::type
operator==(A const &a, B const &b) {
  for (int i = 0; i != vec_traits<A>::dim; ++i)
    if (vec_traits<A>::read_element_idx(i, a) !=
        vec_traits<B>::read_element_idx(i, b))
      return false;
  return true;
}

////////////////////////////////////////////////

namespace qvm_detail {
template <int D> struct mag_sqr_v_defined { static bool const value = false; };
} // namespace qvm_detail

template <class A>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<is_vec<A>::value && !qvm_detail::mag_sqr_v_defined<
                                                 vec_traits<A>::dim>::value,
                         typename vec_traits<A>::scalar_type>::type
    mag_sqr(A const &a) {
  typedef typename vec_traits<A>::scalar_type T;
  T m(scalar_traits<T>::value(0));
  for (int i = 0; i != vec_traits<A>::dim; ++i) {
    T x = vec_traits<A>::read_element_idx(i, a);
    m += x * x;
  }
  return m;
}

////////////////////////////////////////////////

namespace qvm_detail {
template <int D> struct mag_v_defined { static bool const value = false; };
} // namespace qvm_detail

template <class A>
BOOST_QVM_INLINE_OPERATIONS typename enable_if_c<
    is_vec<A>::value && !qvm_detail::mag_v_defined<vec_traits<A>::dim>::value,
    typename vec_traits<A>::scalar_type>::type
mag(A const &a) {
  typedef typename vec_traits<A>::scalar_type T;
  T m(scalar_traits<T>::value(0));
  for (int i = 0; i != vec_traits<A>::dim; ++i) {
    T x = vec_traits<A>::read_element_idx(i, a);
    m += x * x;
  }
  return sqrt<T>(m);
}

////////////////////////////////////////////////

namespace qvm_detail {
template <int D> struct minus_eq_vv_defined {
  static bool const value = false;
};
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS typename enable_if_c<
    is_vec<A>::value && is_vec<B>::value &&
        vec_traits<A>::dim == vec_traits<B>::dim &&
        !qvm_detail::minus_eq_vv_defined<vec_traits<A>::dim>::value,
    A &>::type
operator-=(A &a, B const &b) {
  for (int i = 0; i != vec_traits<A>::dim; ++i)
    vec_traits<A>::write_element_idx(i, a) -=
        vec_traits<B>::read_element_idx(i, b);
  return a;
}

////////////////////////////////////////////////

namespace qvm_detail {
template <int D> struct minus_v_defined { static bool const value = false; };
} // namespace qvm_detail

template <class A>
BOOST_QVM_INLINE_OPERATIONS typename lazy_enable_if_c<
    is_vec<A>::value && !qvm_detail::minus_v_defined<vec_traits<A>::dim>::value,
    deduce_vec<A>>::type
operator-(A const &a) {
  typedef typename deduce_vec<A>::type R;
  R r;
  for (int i = 0; i != vec_traits<A>::dim; ++i)
    vec_traits<R>::write_element_idx(i, r) =
        -vec_traits<A>::read_element_idx(i, a);
  return r;
}

////////////////////////////////////////////////

namespace qvm_detail {
template <int D> struct minus_vv_defined { static bool const value = false; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS typename lazy_enable_if_c<
    is_vec<A>::value && is_vec<B>::value &&
        vec_traits<A>::dim == vec_traits<B>::dim &&
        !qvm_detail::minus_vv_defined<vec_traits<A>::dim>::value,
    deduce_vec2<A, B, vec_traits<A>::dim>>::type
operator-(A const &a, B const &b) {
  typedef typename deduce_vec2<A, B, vec_traits<A>::dim>::type R;
  R r;
  for (int i = 0; i != vec_traits<A>::dim; ++i)
    vec_traits<R>::write_element_idx(i, r) =
        vec_traits<A>::read_element_idx(i, a) -
        vec_traits<B>::read_element_idx(i, b);
  return r;
}

////////////////////////////////////////////////

namespace qvm_detail {
template <int D> struct mul_eq_vs_defined { static bool const value = false; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS typename enable_if_c<
    is_vec<A>::value && is_scalar<B>::value &&
        !qvm_detail::mul_eq_vs_defined<vec_traits<A>::dim>::value,
    A &>::type
operator*=(A &a, B b) {
  for (int i = 0; i != vec_traits<A>::dim; ++i)
    vec_traits<A>::write_element_idx(i, a) *= b;
  return a;
}

////////////////////////////////////////////////

namespace qvm_detail {
template <int D> struct mul_vs_defined { static bool const value = false; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS typename lazy_enable_if_c<
    is_vec<A>::value && is_scalar<B>::value &&
        !qvm_detail::mul_vs_defined<vec_traits<A>::dim>::value,
    deduce_vec<A>>::type
operator*(A const &a, B b) {
  typedef typename deduce_vec<A>::type R;
  R r;
  for (int i = 0; i != vec_traits<A>::dim; ++i)
    vec_traits<R>::write_element_idx(i, r) =
        vec_traits<A>::read_element_idx(i, a) * b;
  return r;
}

////////////////////////////////////////////////

namespace qvm_detail {
template <int D> struct mul_sv_defined { static bool const value = false; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS typename lazy_enable_if_c<
    is_scalar<A>::value && is_vec<B>::value &&
        !qvm_detail::mul_sv_defined<vec_traits<B>::dim>::value,
    deduce_vec<B>>::type
operator*(A a, B const &b) {
  typedef typename deduce_vec<B>::type R;
  R r;
  for (int i = 0; i != vec_traits<B>::dim; ++i)
    vec_traits<R>::write_element_idx(i, r) =
        a * vec_traits<B>::read_element_idx(i, b);
  return r;
}

////////////////////////////////////////////////

namespace qvm_detail {
template <int D> struct neq_vv_defined { static bool const value = false; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS typename enable_if_c<
    is_vec<A>::value && is_vec<B>::value &&
        vec_traits<A>::dim == vec_traits<B>::dim &&
        !qvm_detail::neq_vv_defined<vec_traits<A>::dim>::value,
    bool>::type
operator!=(A const &a, B const &b) {
  for (int i = 0; i != vec_traits<A>::dim; ++i)
    if (vec_traits<A>::read_element_idx(i, a) !=
        vec_traits<B>::read_element_idx(i, b))
      return true;
  return false;
}

////////////////////////////////////////////////

namespace qvm_detail {
template <int D> struct normalize_v_defined {
  static bool const value = false;
};
} // namespace qvm_detail

template <class A>
BOOST_QVM_INLINE_OPERATIONS typename lazy_enable_if_c<
    is_vec<A>::value &&
        !qvm_detail::normalize_v_defined<vec_traits<A>::dim>::value,
    deduce_vec<A>>::type
normalized(A const &a) {
  typedef typename vec_traits<A>::scalar_type T;
  T m(scalar_traits<T>::value(0));
  for (int i = 0; i != vec_traits<A>::dim; ++i) {
    T x = vec_traits<A>::read_element_idx(i, a);
    m += x * x;
  }
  if (m == scalar_traits<T>::value(0))
    BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
  T rm = scalar_traits<T>::value(1) / sqrt<T>(m);
  typedef typename deduce_vec<A>::type R;
  R r;
  for (int i = 0; i != vec_traits<A>::dim; ++i)
    vec_traits<R>::write_element_idx(i, r) =
        vec_traits<A>::read_element_idx(i, a) * rm;
  return r;
}

template <class A>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<is_vec<A>::value && !qvm_detail::normalize_v_defined<
                                                 vec_traits<A>::dim>::value,
                         void>::type
    normalize(A &a) {
  typedef typename vec_traits<A>::scalar_type T;
  T m(scalar_traits<T>::value(0));
  for (int i = 0; i != vec_traits<A>::dim; ++i) {
    T x = vec_traits<A>::read_element_idx(i, a);
    m += x * x;
  }
  if (m == scalar_traits<T>::value(0))
    BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
  T rm = scalar_traits<T>::value(1) / sqrt<T>(m);
  for (int i = 0; i != vec_traits<A>::dim; ++i)
    vec_traits<A>::write_element_idx(i, a) *= rm;
}

////////////////////////////////////////////////

namespace qvm_detail {
template <int D> struct plus_eq_vv_defined { static bool const value = false; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS typename enable_if_c<
    is_vec<A>::value && is_vec<B>::value &&
        vec_traits<A>::dim == vec_traits<B>::dim &&
        !qvm_detail::plus_eq_vv_defined<vec_traits<A>::dim>::value,
    A &>::type
operator+=(A &a, B const &b) {
  for (int i = 0; i != vec_traits<A>::dim; ++i)
    vec_traits<A>::write_element_idx(i, a) +=
        vec_traits<B>::read_element_idx(i, b);
  return a;
}

////////////////////////////////////////////////

namespace qvm_detail {
template <int D> struct plus_vv_defined { static bool const value = false; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS typename lazy_enable_if_c<
    is_vec<A>::value && is_vec<B>::value &&
        vec_traits<A>::dim == vec_traits<B>::dim &&
        !qvm_detail::plus_vv_defined<vec_traits<A>::dim>::value,
    deduce_vec2<A, B, vec_traits<A>::dim>>::type
operator+(A const &a, B const &b) {
  typedef typename deduce_vec2<A, B, vec_traits<A>::dim>::type R;
  R r;
  for (int i = 0; i != vec_traits<A>::dim; ++i)
    vec_traits<R>::write_element_idx(i, r) =
        vec_traits<A>::read_element_idx(i, a) +
        vec_traits<B>::read_element_idx(i, b);
  return r;
}

////////////////////////////////////////////////

namespace qvm_detail {
template <class T> class vref_ {
  vref_(vref_ const &);
  vref_ &operator=(vref_ const &);
  ~vref_();

public:
  template <class R> BOOST_QVM_INLINE_TRIVIAL vref_ &operator=(R const &x) {
    assign(*this, x);
    return *this;
  }

  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }
};
} // namespace qvm_detail

template <class V> struct vec_traits<qvm_detail::vref_<V>> {
  typedef typename vec_traits<V>::scalar_type scalar_type;
  typedef qvm_detail::vref_<V> this_vector;
  static int const dim = vec_traits<V>::dim;

  template <int I>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_vector const &x) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < dim);
    return vec_traits<V>::template read_element<I>(
        reinterpret_cast<V const &>(x));
  }

  template <int I>
  static BOOST_QVM_INLINE_CRITICAL scalar_type &write_element(this_vector &x) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < dim);
    return vec_traits<V>::template write_element<I>(reinterpret_cast<V &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element_idx(int i, this_vector const &x) {
    BOOST_QVM_ASSERT(i >= 0);
    BOOST_QVM_ASSERT(i < dim);
    return vec_traits<V>::read_element_idx(i, reinterpret_cast<V const &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type &
  write_element_idx(int i, this_vector &x) {
    BOOST_QVM_ASSERT(i >= 0);
    BOOST_QVM_ASSERT(i < dim);
    return vec_traits<V>::write_element_idx(i, reinterpret_cast<V &>(x));
  }
};

template <class V, int D> struct deduce_vec<qvm_detail::vref_<V>, D> {
  typedef vec<typename vec_traits<V>::scalar_type, D> type;
};

template <class V>
BOOST_QVM_INLINE_TRIVIAL
    typename enable_if_c<is_vec<V>::value, qvm_detail::vref_<V> const &>::type
    vref(V const &a) {
  return reinterpret_cast<qvm_detail::vref_<V> const &>(a);
}

template <class V>
BOOST_QVM_INLINE_TRIVIAL
    typename enable_if_c<is_vec<V>::value, qvm_detail::vref_<V> &>::type
    vref(V &a) {
  return reinterpret_cast<qvm_detail::vref_<V> &>(a);
}

////////////////////////////////////////////////

namespace sfinae {
using ::boost::qvm::assign;
using ::boost::qvm::cmp;
using ::boost::qvm::convert_to;
using ::boost::qvm::cross;
using ::boost::qvm::scalar_cast;
using ::boost::qvm::set_zero;
using ::boost::qvm::to_string;
using ::boost::qvm::operator/=;
using ::boost::qvm::operator/;
using ::boost::qvm::dot;
using ::boost::qvm::operator==;
using ::boost::qvm::mag;
using ::boost::qvm::mag_sqr;
using ::boost::qvm::operator-=;
using ::boost::qvm::operator-;
using ::boost::qvm::operator*=;
using ::boost::qvm::operator*;
using ::boost::qvm::operator!=;
using ::boost::qvm::normalize;
using ::boost::qvm::normalized;
using ::boost::qvm::operator+=;
using ::boost::qvm::operator+;
using ::boost::qvm::vref;
} // namespace sfinae

////////////////////////////////////////////////
} // namespace qvm
} // namespace boost

#endif


/**	\file LatLongConversions.cpp
*	\brief
*/

/****************************************************************************/
/*	LatLongConversions.cpp													*/
/****************************************************************************/
/*                                                                          */
/*  Copyright 2008 - 2010 Paul Kohut                                        */
/*  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.                          */
/*                                                                          */
/****************************************************************************/

//#include "stdafx.h"
#include <boost/regex.hpp>
#include "LatLongConversions.h"
#include "..\GeoFormulas\Conversions.h"

using namespace boost;
using namespace std;

void TrimWhitespace(string & szString)
{
	regex pat("^[ \t]+|[ \t]+$");
	szString = regex_replace(szString, pat, "");
}

double ParseLatitude( string szDeg )
{
	regex_constants::syntax_option_type flags =  regex_constants::icase | regex_constants::perl;
	TrimWhitespace(szDeg);
	string sPattern = "[+-]|[sn]";
	smatch what;
	regex pat(sPattern, flags);
	if(!regex_search(szDeg, what, pat))
	{
		// did not find pattern so assume value is positive and North
		// and stuff 'N' at the beginning of the string
		szDeg = "N" + szDeg;
		regex_search(szDeg, what, pat);
	}

	// Replace all '+' 'n' and 'N' with 'N'
	pat.assign("[+N]", flags);
	szDeg = regex_replace(szDeg, pat, "N");

	// Replace all '-' 's' and 'S' with 'S'
	pat.assign("[-S]", flags);
	szDeg = regex_replace(szDeg, pat, "S");

	// determine if pattern above was found at the beginning or
	// end of the string.  If found at the end the swap it to the
	// beginning.
	size_t pos = what.position();
	if(pos == szDeg.length() - 1)
	{
		szDeg = szDeg[pos] + szDeg.substr(0, pos);
	}

	// String has been normalized continue on with the parsing
	return ParseLatitudeBegin(szDeg);
}

double ParseLatitudeBegin(string & sString)
{
	regex_constants::syntax_option_type flags =  regex_constants::icase | regex_constants::perl;
	string sPattern = "[:. ]";

	regex pat(sPattern, flags);
	smatch matchResults;
	sString = regex_replace(sString, pat, " ");
	pat.assign(" ", flags);

//	if(!regex_match(sString, matchResults, pat))
//		throw CRNavConversionException("regex match not found");

	// Split the string into it's sub string tokens based on sPattern below.
	// Define the sub string match patterns that will be valid for the iterator
	// Note: If sPattern is changed then make sure the sub_matches are changed
	// accordingly too.

	int const sub_matches[] = {1, 2, 3, 4, 5, 6 };
	sPattern = "([NS])(\\d+)[ ](\\d+)[ ](\\d+)[ ](\\d+)";
	pat.assign(sPattern, flags);
	sregex_token_iterator it(sString.begin(), sString.end(), pat, sub_matches);
	if(it == sregex_token_iterator())
	{
		sPattern = "([NS])(\\d+)[ ](\\d+)[ ](\\d+)()";
		pat.assign(sPattern, flags);
		it = sregex_token_iterator(sString.begin(), sString.end(), pat, sub_matches);
		if(it == sregex_token_iterator())
		{
			sPattern = "([NS])(\\d+)[ ](\\d+)()()";
			pat.assign(sPattern, flags);
			it = sregex_token_iterator(sString.begin(), sString.end(), pat, sub_matches);
			if(it == sregex_token_iterator())
			{
				sPattern = "([NS])(\\d+)()()()";
				pat.assign(sPattern, flags);
				it = sregex_token_iterator(sString.begin(), sString.end(), pat, sub_matches);
				if(it == sregex_token_iterator())
					throw CRNavConversionException("Value out of range, are you passing an angle greater than 90 degrees?");
			}
		}
	}

	// Get each piece of the substring to build the final string that
	// can be converted to decimal
	string sneg, sdeg, smin, ssec;
	// Make sure we convert 'S' and '-' to '-'
	// Don't need to do this if 'N' or '+'
	if((*it).str().compare("S") == 0 || (*it).str().compare("s") == 0 || (*it).str().compare("-") == 0)
		sneg += "-";

	it++;

	sdeg = *it++;
	smin = *it++;
	ssec = *it++;
//	ssec += "." + *it++;
	double dDeg = atof(sdeg.c_str());
	double dMin = atof(smin.c_str());
	double dSec = atof(ssec.c_str());
//	if(dDeg > 90.0 || dMin > 59.0 || dSec > 59.0)
//		throw(CRNavConversionException("Degrees > 90 or Minutes > 59 or Seconds > 59"));

	ssec += "." + *it++;
	dSec = atof(ssec.c_str());

	double dVal;
	if(sneg[0] == '-')
		dVal = ConvertDmsToDd(-dDeg, dMin, dSec);
	else
		dVal = ConvertDmsToDd(dDeg, dMin, dSec);
	if(dVal > 90.0 || dVal < -90.0)
		throw(CRNavConversionException("Latitude > 90.0 or < -90.0"));
	return dVal;
}

double ParseLongitude( string szDeg )
{
	regex_constants::syntax_option_type flags =  regex_constants::icase | regex_constants::perl;
	TrimWhitespace(szDeg);
	string sPattern = "[+-]|[ew]";
	smatch what;
	regex pat(sPattern, flags);
	if(!regex_search(szDeg, what, pat))
	{
		// did not find pattern so assume value is positive and North
		// and stuff 'N' at the beginning of the string
		szDeg = "E" + szDeg;
		regex_search(szDeg, what, pat);
	}

	// Replace all '+' 'n' and 'N' with 'N'
	pat.assign("[+E]", flags);
	szDeg = regex_replace(szDeg, pat, "E");

	// Replace all '-' 's' and 'S' with 'S'
	pat.assign("[-W]", flags);
	szDeg = regex_replace(szDeg, pat, "W");

	// determine if pattern above was found at the beginning or
	// end of the string.  If found at the end the swap it to the
	// beginning.
	size_t pos = what.position();
	if(pos == szDeg.length() - 1)
	{
		szDeg = szDeg[pos] + szDeg.substr(0, pos);
	}

	// String has been normalized continue on with the parsing
	return ParseLongitudeBegin(szDeg);
}

double ParseLongitudeBegin(string & sString)
{
	regex_constants::syntax_option_type flags =  regex_constants::icase | regex_constants::perl;
	string sPattern = "[:. ]";

	regex pat(sPattern, flags);
	smatch matchResults;
	sString = regex_replace(sString, pat, " ");
	pat.assign(" ", flags);

	//	if(!regex_match(sString, matchResults, pat))
	//		throw CRNavConversionException("regex match not found");

	// Split the string into it's sub string tokens based on sPattern below.
	// Define the sub string match patterns that will be valid for the iterator
	// Note: If sPattern is changed then make sure the sub_matches are changed
	// accordingly too.

	int const sub_matches[] = {1, 2, 3, 4, 5, 6 };
	sPattern = "([EW])(\\d+)[ ](\\d+)[ ](\\d+)[ ](\\d+)";
	pat.assign(sPattern, flags);
	sregex_token_iterator it(sString.begin(), sString.end(), pat, sub_matches);
	if(it == sregex_token_iterator())
	{
		sPattern = "([EW])(\\d+)[ ](\\d+)[ ](\\d+)()";
		pat.assign(sPattern, flags);
		it = sregex_token_iterator(sString.begin(), sString.end(), pat, sub_matches);
		if(it == sregex_token_iterator())
		{
			sPattern = "([EW])(\\d+)[ ](\\d+)()()";
			pat.assign(sPattern, flags);
			it = sregex_token_iterator(sString.begin(), sString.end(), pat, sub_matches);
			if(it == sregex_token_iterator())
			{
				sPattern = "([EW])(\\d+)()()()";
				pat.assign(sPattern, flags);
				it = sregex_token_iterator(sString.begin(), sString.end(), pat, sub_matches);
				if(it == sregex_token_iterator())
					throw CRNavConversionException("Value out of range, are you passing an angle greater than 180 degrees?");
			}
		}
	}

	// Get each piece of the substring to build the final string that
	// can be converted to decimal
	string sneg, sdeg, smin, ssec;
	// Make sure we convert 'W' and '-' to '-'
	// Don't need to do this if 'E' or '+'
	if((*it).str().compare("W") == 0 || (*it).str().compare("w") == 0 || (*it).str().compare("-") == 0)
		sneg += "-";

	it++;

	sdeg = *it++;
	smin = *it++;
	ssec = *it++;
	//	ssec += "." + *it++;
	double dDeg = atof(sdeg.c_str());
	double dMin = atof(smin.c_str());
	double dSec = atof(ssec.c_str());
//	if(dDeg > 180.0 || dMin > 59.0 || dSec > 59.0)
//		throw(CRNavConversionException("Degrees > 180 or Minutes > 59 or Seconds > 59"));

	ssec += "." + *it++;
	dSec = atof(ssec.c_str());

	double dVal;
	if(sneg[0] == '-')
		dVal = ConvertDmsToDd(-dDeg, dMin, dSec);
	else
		dVal = ConvertDmsToDd(dDeg, dMin, dSec);
	if(dVal > 180.0 || dVal < -180.0)
		throw(CRNavConversionException("Longitude > 180.0 or < -180.0"));
	return dVal;
}

// Pad0 will pad to the beginning of the string as many
// '0' as needed until the length of szOrg is = nMinDigits
string Pad0(string szOrg, size_t nMinDigits)
{
	for(size_t pos = szOrg.length(); pos < nMinDigits; pos++)
	{
		szOrg = "0" + szOrg;
	}
	return szOrg;
}

double ConvertDmsToDd(double const & dDeg, double const & dMin, double const & dSec)
{
//	if(sign < 0)
//		return -dDeg - ((dMin + (dSec / 60.0)) / 60.0);
//	return dDeg + ((dMin + (dSec / 60.0)) / 60.0);
	int bSignBit = false;
	if(dDeg == 0)
	{
		unsigned long * pVal = (unsigned long*) &dDeg;
		bSignBit = *(++pVal) & 1 << 31;
	}

	if(dDeg < 0 || bSignBit)
		return (-(fabs(dDeg)+ (dMin / 60.0) + (dSec / 3600.0)));
	return (dDeg + (dMin / 60.0) + (dSec / 3600.0));
}

double ConvertDmsToDd(string const & sDeg, string const & sMin, string const & sSec)
{
	double dNeg = atof(sDeg.c_str());
	double dDeg = fabs(dNeg);
	double dMin = atof(sMin.c_str());
	double dSec = atof(sSec.c_str());
	if(dNeg < 0.0)
		return ConvertDmsToDd(-dDeg, dMin, dSec);
	return ConvertDmsToDd(dDeg, dMin, dSec);
}

string ConvertDdToDms(double const & dDeg)
{
	char szBuffer[25];
	int nDegrees = (int) dDeg;
	double dMinutes = (dDeg - nDegrees) * 60;
	double nSeconds = (dMinutes - (int) dMinutes) * 60;
	sprintf(szBuffer, "%d:%02d:%08.5f", nDegrees, (int) fabs(dMinutes), fabs(nSeconds));
	return string(szBuffer);
}

string ConvertLatitudeDdToDms(double const & dDeg)
{
	bool bIsNeg = dDeg < 0.0;
	double dVal = fabs(dDeg);
	if(dVal > 90.0)
		dVal = 180.0 - dVal;
	return (ConvertDdToDms(dVal) + (bIsNeg  ? "S" : "N"));
}

string ConvertLongitudeDdToDms(double const & dDeg)
{
	bool bIsNeg = dDeg < 0.0;
	double dVal = fabs(dDeg);
	if(dVal > 180.0)
	{
		dVal = 360.0 - dVal;
		bIsNeg = !bIsNeg;
	}
	return (ConvertDdToDms(dVal) + (bIsNeg  ? "W" : "E"));
}


#include <math.h>
#include <string>
#include <vector>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <sstream>
#include <io.h>
#include <stdio.h>
#include <stdlib.h>

#include <sys/types.h>
//#include <dirent.h>
#include <boost/lexical_cast.hpp>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>

#include "caffe/blob.hpp"
#include "caffe/filler.hpp"
#include "caffe/layers/DenseBlock_layer.hpp"

namespace caffe {

  bool dirExists(string dirStr){
    const char* dirCStr = dirStr.c_str();
	return !_access(dirCStr, 0);
    /*DIR* dir = opendir(dirCStr);
    if (ENOENT == errno){
      return false;
    }
    closedir(dir);
    return true;*/
  }

  void tryCreateDirectory(string fileName){
    vector<string> strVec;
    boost::split(strVec,fileName,boost::is_any_of("/"));
    string newStr="";
    for (int i=0;i<strVec.size()-1;++i){
      newStr += strVec[i] + (i==strVec.size()-2?"":"/");
    }
    boost::filesystem::path dirToCreate(newStr);
    if (!dirExists(newStr)){
      boost::filesystem::create_directories(dirToCreate);
    }
  }

  template <typename Dtype>
  void DenseBlockLayer<Dtype>::LayerSetUp(const vector<Blob<Dtype>*>& bottom, const vector<Blob<Dtype>*>& top){
	this->cpuInited = false;
	this->gpuInited = false;
	DenseBlockParameter dbParam = this->layer_param_.denseblock_param();
        this->numTransition = dbParam.numtransition();
        this->initChannel = dbParam.initchannel();
        this->growthRate = dbParam.growthrate();
        this->trainCycleIdx = 0; //initially, trainCycleIdx = 0
        this->workspace_size_bytes = dbParam.workspace_mb()*1024*1024;
	this->EMA_decay = dbParam.moving_average_fraction();
        this->gpu_idx_ = dbParam.gpuidx();
        this->useDropout = dbParam.use_dropout();
	this->dropoutAmount = dbParam.dropout_amount();
	this->DB_randomSeed = 124816;
        this->useBC = dbParam.use_bc();
        this->BC_ultra_spaceEfficient = dbParam.bc_ultra_space_efficient();
        //Parameter Blobs
	//for transition i,
	//blobs_[i] is its filter blob
	//blobs_[numTransition + i] is its scaler blob
	//blobs_[2*numTransition + i] is its bias blob
	//blobs_[3*numTransition + i] is its globalMean
	//blobs_[4*numTransition + i] is its globalVar
	if (useBC){
          this->blobs_.resize(10*this->numTransition + 1);
	}
	else {
          this->blobs_.resize(5*this->numTransition + 1);
	}
	for (int transitionIdx=0;transitionIdx < this->numTransition;++transitionIdx){
	    //filter
	    //No BC case
	    if (!useBC){
	      int inChannels = initChannel + transitionIdx * growthRate;
	      int filterShape_Arr[] = {growthRate,inChannels,3,3};
	      vector<int> filterShape (filterShape_Arr,filterShape_Arr+4);
	      this->blobs_[transitionIdx].reset(new Blob<Dtype>(filterShape));
	      shared_ptr<Filler<Dtype> > filter_Filler(GetFiller<Dtype>(dbParam.filter_filler()));
	      filter_Filler->Fill(this->blobs_[transitionIdx].get());
	    }
	    else {
	      //3*3 kernel
	      int filter_33_shapeArr[] = {growthRate,4*growthRate,3,3};
	      vector<int> filter33Shape (filter_33_shapeArr,filter_33_shapeArr+4);
	      this->blobs_[transitionIdx].reset(new Blob<Dtype>(filter33Shape));
	      shared_ptr<Filler<Dtype> > filter_Filler3(GetFiller<Dtype>(dbParam.filter_filler()));
	      filter_Filler3->Fill(this->blobs_[transitionIdx].get());

	      //1*1 kernel
	      int inChannels = initChannel + transitionIdx * growthRate;
	      int filter_11_shapeArr[] = {4*growthRate,inChannels,1,1};
	      vector<int> filter11Shape (filter_11_shapeArr,filter_11_shapeArr+4);
              this->blobs_[5*numTransition+transitionIdx].reset(new Blob<Dtype>(filter11Shape));
	      shared_ptr<Filler<Dtype> > filter_Filler1(GetFiller<Dtype>(dbParam.filter_filler()));
	      filter_Filler1->Fill(this->blobs_[5*numTransition+transitionIdx].get());
	    }
	    //scaler & bias
	    int inChannels = initChannel + transitionIdx * growthRate;
	    int BNparamShape_Arr [] = {1,inChannels,1,1};
	    vector<int> BNparamShape (BNparamShape_Arr,BNparamShape_Arr+4);
	    //scaler
	    this->blobs_[numTransition + transitionIdx].reset(new Blob<Dtype>(BNparamShape));
	    shared_ptr<Filler<Dtype> > weight_filler0(GetFiller<Dtype>(dbParam.bn_scaler_filler()));
	    weight_filler0->Fill(this->blobs_[numTransition+transitionIdx].get());

	    int BN_4G_Shape[] = {1,4*growthRate,1,1};
            vector<int> BN_4Gparam_ShapeVec (BN_4G_Shape,BN_4G_Shape+4);
	    //scaler BC
            if (useBC){
              this->blobs_[6*numTransition + transitionIdx].reset(new Blob<Dtype>(BN_4Gparam_ShapeVec));
	      shared_ptr<Filler<Dtype> > weight_filler0_4G(GetFiller<Dtype>(dbParam.bn_scaler_filler()));
              weight_filler0_4G->Fill(this->blobs_[6*numTransition+transitionIdx].get());
	    }
	    //bias
	    this->blobs_[2*numTransition + transitionIdx].reset(new Blob<Dtype>(BNparamShape));
	    shared_ptr<Filler<Dtype> > weight_filler1(GetFiller<Dtype>(dbParam.bn_bias_filler()));
	    weight_filler1->Fill(this->blobs_[2*numTransition+transitionIdx].get());
	    //bias BC
	    if (useBC){
              this->blobs_[7*numTransition + transitionIdx].reset(new Blob<Dtype>(BN_4Gparam_ShapeVec));
	      shared_ptr<Filler<Dtype> > weight_filler1_4G(GetFiller<Dtype>(dbParam.bn_bias_filler()));
	      weight_filler1_4G->Fill(this->blobs_[7*numTransition+transitionIdx].get());
	    }
	    //globalMean
	    this->blobs_[3*numTransition + transitionIdx].reset(new Blob<Dtype>(BNparamShape));
	    for (int blobIdx=0;blobIdx<inChannels;++blobIdx){
	      shared_ptr<Blob<Dtype> > localB = this->blobs_[3*numTransition+transitionIdx];
	      localB->mutable_cpu_data()[localB->offset(0,blobIdx,0,0)] = 0;
	    }
	    //globalMean BC
	    if (useBC){
	      this->blobs_[8*numTransition + transitionIdx].reset(new Blob<Dtype>(BN_4Gparam_ShapeVec));
	      shared_ptr<Blob<Dtype> > localB = this->blobs_[8*numTransition+transitionIdx];
	      for (int blobIdx=0;blobIdx<4*growthRate;++blobIdx){
		localB->mutable_cpu_data()[localB->offset(0,blobIdx,0,0)] = 0;
	      }
	    }
	    //globalVar
	    this->blobs_[4*numTransition + transitionIdx].reset(new Blob<Dtype>(BNparamShape));
	    for (int blobIdx=0;blobIdx<inChannels;++blobIdx){
	      shared_ptr<Blob<Dtype> > localB = this->blobs_[4*numTransition+transitionIdx];
	      localB->mutable_cpu_data()[localB->offset(0,blobIdx,0,0)] = 1;
	    }
	    //globalVar BC
	    if (useBC){
              this->blobs_[9*numTransition + transitionIdx].reset(new Blob<Dtype>(BN_4Gparam_ShapeVec));
	      shared_ptr<Blob<Dtype> > localB = this->blobs_[9*numTransition+transitionIdx];
	      for (int blobIdx=0;blobIdx<4*growthRate;++blobIdx){
		localB->mutable_cpu_data()[localB->offset(0,blobIdx,0,0)] = 1;
	      }
	    }
	}
     //final parameter for the equivalent of blobs_[2] in Caffe-BN
     vector<int> singletonShapeVec;
     singletonShapeVec.push_back(1);
     int singletonIdx = useBC?10*numTransition:5*numTransition;
     this->blobs_[singletonIdx].reset(new Blob<Dtype>(singletonShapeVec));
     this->blobs_[singletonIdx]->mutable_cpu_data()[0] = Dtype(0);
     //parameter specification: globalMean/Var weight decay and lr is 0
     if (!useBC){
       for (int i=0;i<this->blobs_.size();++i){
         if (this->layer_param_.param_size()!=i){
           CHECK_EQ(0, 1)
             << "Nope";
         }
         ParamSpec* fixed_param_spec = this->layer_param_.add_param();
         //global Mean/Var
         if (i>=3*this->numTransition){
           fixed_param_spec->set_lr_mult(0.f);
           fixed_param_spec->set_decay_mult(0.f);
         }
         //BN Scaler and Bias
         else if (i>=this->numTransition){
           fixed_param_spec->set_lr_mult(1.f);
           fixed_param_spec->set_decay_mult(1.f);
         }
         else {
           fixed_param_spec->set_lr_mult(1.f);
	   fixed_param_spec->set_decay_mult(1.f);
         }
       }
    }
    else {
      for (int i=0;i<this->blobs_.size();++i){
         if (this->layer_param_.param_size()!=i){
           CHECK_EQ(0, 1)
             << "Nope";
         }
         ParamSpec* fixed_param_spec = this->layer_param_.add_param();
         if ((i>=3*numTransition)&&(i<5*numTransition)){
           fixed_param_spec->set_lr_mult(0.f);
           fixed_param_spec->set_decay_mult(0.f);
	 }
	 else if (i>=8*numTransition){
	   fixed_param_spec->set_lr_mult(0.f);
           fixed_param_spec->set_decay_mult(0.f);
	 }
	 else {
           fixed_param_spec->set_lr_mult(1.f);
           fixed_param_spec->set_decay_mult(1.f);
	 }
      }
    }
}

template <typename Dtype>
void DenseBlockLayer<Dtype>::Reshape(const vector<Blob<Dtype>*>& bottom, const vector<Blob<Dtype>*>& top){
        this->N = bottom[0]->shape()[0];
        this->H = bottom[0]->shape()[2];
        this->W = bottom[0]->shape()[3];
	int topShapeArr[] = {this->N,this->initChannel+this->numTransition*this->growthRate,this->H,this->W};
	vector<int> topShape(topShapeArr,topShapeArr+4);
	top[0]->Reshape(topShape);
}

template <typename Dtype>
void DenseBlockLayer<Dtype>::syncBlobs(DenseBlockLayer<Dtype>* originLayer){
    vector<shared_ptr<Blob<Dtype> > >& originBlobs = originLayer->blobs();
    for (int blobIdx=0;blobIdx < originBlobs.size();++blobIdx){
      shared_ptr<Blob<Dtype> > localBlob = originBlobs[blobIdx];
      Blob<Dtype> * newBlob = new Blob<Dtype>(localBlob->shape());
      newBlob->CopyFrom(*(localBlob.get()),false);
      shared_ptr<Blob<Dtype> > sharedPtrBlob(newBlob);
      this->blobs_[blobIdx] = sharedPtrBlob;
    }
}

template <typename Dtype>
void DenseBlockLayer<Dtype>::setLogId(int uid){
    this->logId = uid;
}

template <typename Dtype>
void logBlob(Blob<Dtype>* B,string fileName){
    string dataNameStr = fileName + "_data";
    string gradNameStr = fileName + "_grad";
    const char* dataName = (dataNameStr).c_str();
    const char* gradName = (gradNameStr).c_str();

    tryCreateDirectory(dataName);
    tryCreateDirectory(gradName);
    std::ofstream outWriter_data(dataName,std::ofstream::out);
    std::ofstream outWriter_grad(gradName,std::ofstream::out);
    for (int n=0;n<B->shape(0);++n){
      for (int c=0;c<B->shape(1);++c){
        for (int h=0;h<B->shape(2);++h){
	  for (int w=0;w<B->shape(3);++w){
	    outWriter_data<<B->data_at(n,c,h,w)<<",";
	    outWriter_grad<<B->diff_at(n,c,h,w)<<",";
	  }
	}
      }
    }
    outWriter_data<<std::endl;
    outWriter_grad<<std::endl;
}

string itos(int i){
  string output = boost::lexical_cast<string>(i);
  return output;
}

template <typename Dtype>
void DenseBlockLayer<Dtype>::logInternal_cpu(string dir){
    string localDir = dir+"/cpu_"+itos(this->logId)+"/";
    //batch_Mean
    for (int i=0;i<this->batch_Mean.size();++i){
      string blobStr = localDir+"batch_Mean_"+itos(i);
      logBlob(this->batch_Mean[i],blobStr);
    }
    //batch_Var
    for (int i=0;i<this->batch_Var.size();++i){
      string blobStr = localDir+"batch_Var_"+itos(i);
      logBlob(this->batch_Var[i],blobStr);
    }
    if (useBC){
      //batch_Mean
      for (int i=0;i<this->batch_Mean4G.size();++i){
        string blobStr = localDir+"batch_Mean_BC_"+itos(i);
        logBlob(this->batch_Mean4G[i],blobStr);
      }
      //batch_Var
      for (int i=0;i<this->batch_Var4G.size();++i){
        string blobStr = localDir+"batch_Var_BC_"+itos(i);
        logBlob(this->batch_Var4G[i],blobStr);
      }
    }
    //merged_conv
    for (int i=0;i<this->merged_conv.size();++i){
      string blobStr = localDir+"merged_conv_"+itos(i);
      logBlob(this->merged_conv[i],blobStr);
    }
    //BN_XhatVec
    for (int i=0;i<this->BN_XhatVec.size();++i){
      string blobStr = localDir+"BN_XhatVec_"+itos(i);
      logBlob(this->BN_XhatVec[i],blobStr);
    }
    //postBN_blobVec
    for (int i=0;i<this->postBN_blobVec.size();++i){
      string blobStr = localDir+"postBN_blobVec_"+itos(i);
      logBlob(this->postBN_blobVec[i],blobStr);
    }
    //postReLU_blobVec
    for (int i=0;i<this->postReLU_blobVec.size();++i){
      string blobStr = localDir+"postReLU_blobVec_"+itos(i);
      logBlob(this->postReLU_blobVec[i],blobStr);
    }
    //postConv_blobVec
    for (int i=0;i<this->postConv_blobVec.size();++i){
      string blobStr = localDir+"postConv_blobVec_"+itos(i);
      logBlob(this->postConv_blobVec[i],blobStr);
    }
    if (useBC){
      //BC_BN_XhatVec
      for (int i=0;i<this->BC_BN_XhatVec.size();++i){
	string blobStr = localDir+"BC_BN_XhatVec_"+itos(i);
	logBlob(this->BC_BN_XhatVec[i],blobStr);
      }
      //postBN_BCVec
      for (int i=0;i<this->postBN_BCVec.size();++i){
	string blobStr = localDir+"postBN_BCVec_"+itos(i);
	logBlob(this->postBN_BCVec[i],blobStr);
      }
      //postReLU_BCVec
      for (int i=0;i<this->postReLU_BCVec.size();++i){
	string blobStr = localDir+"postReLU_BCVec_"+itos(i);
        logBlob(this->postReLU_BCVec[i],blobStr);
      }
      //postConv_BCVec
      for (int i=0;i<this->postConv_BCVec.size();++i){
	string blobStr = localDir+"postConv_BCVec_"+itos(i);
        logBlob(this->postConv_BCVec[i],blobStr);
      }
    }
    //filter
    for (int i=0;i<this->numTransition;++i){
      string blobStr = localDir+"filter_"+itos(i);
      logBlob(this->blobs_[i].get(),blobStr);
    }
    //scaler
    for (int i=0;i<this->numTransition;++i){
      string blobStr = localDir+"scaler_"+itos(i);
      logBlob(this->blobs_[this->numTransition+i].get(),blobStr);
    }
    //bias
    for (int i=0;i<this->numTransition;++i){
      string blobStr = localDir+"bias_"+itos(i);
      logBlob(this->blobs_[this->numTransition*2+i].get(),blobStr);
    }
    if (useBC){
      //filter
      for (int i=0;i<this->numTransition;++i){
        string blobStr = localDir+"filter_BC_"+itos(i);
        logBlob(this->blobs_[5*numTransition+i].get(),blobStr);
      }
      //scaler
      for (int i=0;i<this->numTransition;++i){
        string blobStr = localDir+"scaler_BC_"+itos(i);
        logBlob(this->blobs_[6*numTransition+i].get(),blobStr);
      }
      //bias
      for (int i=0;i<this->numTransition;++i){
        string blobStr = localDir+"bias_BC_"+itos(i);
        logBlob(this->blobs_[7*numTransition+i].get(),blobStr);
      }
      //Mean
      for (int i=0;i<this->numTransition;++i){
        string blobStr = localDir+"Mean_BC_"+itos(i);
        logBlob(this->blobs_[8*numTransition+i].get(),blobStr);
      }
      //Var
      for (int i=0;i<this->numTransition;++i){
        string blobStr = localDir+"Var_BC_"+itos(i);
        logBlob(this->blobs_[9*numTransition+i].get(),blobStr);
      }
    }
}

template <typename Dtype>
Dtype getZeroPaddedValue(bool isDiff,Blob<Dtype>* inputData,int n,int c,int h,int w){
    int n_blob = inputData->shape(0);
    int c_blob = inputData->shape(1);
    int h_blob = inputData->shape(2);
    int w_blob = inputData->shape(3);
    if ((n<0) || (n>=n_blob)) return 0;
    if ((c<0) || (c>=c_blob)) return 0;
    if ((h<0) || (h>=h_blob)) return 0;
    if ((w<0) || (w>=w_blob)) return 0;
    if (isDiff) return inputData->diff_at(n,c,h,w);
    else return inputData->data_at(n,c,h,w);
}

//Assumption, h_filter and w_filter must be 3 for now
//naivest possible implementation of convolution, CPU forward and backward should not be used in production.
//CPU version of convolution assume img H,W does not change after convolution, which corresponds to denseBlock without BC
//input of size N*c_input*h_img*w_img
template <typename Dtype>
void convolution_Fwd(Blob<Dtype>* input, Blob<Dtype>* output, Blob<Dtype>* filter,int N,int c_output,int c_input,int h_img,int w_img,int h_filter,int w_filter){
    int outputShape[] = {N,c_output,h_img,w_img};
    vector<int> outputShapeVec (outputShape,outputShape + 4);
    output->Reshape(outputShapeVec);
    Dtype * outputPtr = output->mutable_cpu_data();
    for (int n=0;n<N;++n){
      for (int c_outIdx=0;c_outIdx<c_output;++c_outIdx){
        for (int hIdx=0;hIdx<h_img;++hIdx){
	  for (int wIdx=0;wIdx<w_img;++wIdx){
	    outputPtr[output->offset(n,c_outIdx,hIdx,wIdx)]=0;
	    for (int c_inIdx=0;c_inIdx<c_input;++c_inIdx){
	      for (int filter_x=0;filter_x<h_filter;++filter_x){
	        for (int filter_y=0;filter_y<w_filter;++filter_y){
		  int localX = hIdx + (h_filter/2) - filter_x;
	          int localY = wIdx + (w_filter/2) - filter_y;
	          outputPtr[output->offset(n,c_outIdx,hIdx,wIdx)] += (filter->data_at(c_outIdx,c_inIdx,filter_x,filter_y) * getZeroPaddedValue(false,input,n,c_inIdx,localX,localY));
		}
	      }
	    }
	  }
	}
      }
    }
}

//beta = 1 Convolution for bottomDiff
template <typename Dtype>
void convolution_Bwd(Blob<Dtype>* bottom,Blob<Dtype>* top,Blob<Dtype>* filter,int N,int c_output,int c_input,int h_img,int w_img,int h_filter,int w_filter){
    Dtype * filterDiffPtr = filter->mutable_cpu_diff();
    Dtype * bottomDiffPtr = bottom->mutable_cpu_diff();
    //compute FilterGrad
    for (int coutIdx=0;coutIdx<c_output;++coutIdx){
      for (int cinIdx=0;cinIdx<c_input;++cinIdx){
        for (int filter_x=0;filter_x<h_filter;++filter_x){
	  for (int filter_y=0;filter_y<w_filter;++filter_y){
	    Dtype localGradSum=0;
	    for (int n=0;n<N;++n){
	      for (int i_img=0;i_img<h_img;++i_img){
	        for (int j_img=0;j_img<w_img;++j_img){
		  int localX = i_img + (h_filter/2)  - filter_x;
		  int localY = j_img + (w_filter/2) - filter_y;
		  localGradSum += top->diff_at(n,coutIdx,i_img,j_img) * getZeroPaddedValue(false,bottom,n,cinIdx,localX,localY);
		}
	      }
	    }
	    filterDiffPtr[filter->offset(coutIdx,cinIdx,filter_x,filter_y)] = localGradSum;
	  }
	}
      }
    }
    //compute BottomGrad
    for (int n=0;n<N;++n){
      for (int cinIdx=0;cinIdx<c_input;++cinIdx){
        for (int i_img=0;i_img<h_img;++i_img){
	  for (int j_img=0;j_img<w_img;++j_img){
	    Dtype localGradSum=0;
	    for (int coutIdx=0;coutIdx<c_output;++coutIdx){
	      for (int x_img=0;x_img<h_img;++x_img){
	        for (int y_img=0;y_img<w_img;++y_img){
		  int localX = x_img-i_img+(h_filter/2);
		  int localY = y_img-j_img+(w_filter/2);
		  localGradSum += top->diff_at(n,coutIdx,x_img,y_img) * getZeroPaddedValue(false,filter,coutIdx,cinIdx,localX,localY);
		}
	      }
	    }
	    bottomDiffPtr[bottom->offset(n,cinIdx,i_img,j_img)] = localGradSum;
	  }
	}
      }
    }
}

template <typename Dtype>
void ReLU_Fwd(Blob<Dtype>* bottom,Blob<Dtype>* top,int N,int C,int h_img,int w_img){
    //Reshape top
    int topShapeArr[] = {N,C,h_img,w_img};
    vector<int> topShapeVec(topShapeArr,topShapeArr+4);
    top->Reshape(topShapeVec);
    //ReLU Fwd
    Dtype* topPtr = top->mutable_cpu_data();
    for (int n=0;n<N;++n){
      for (int cIdx=0;cIdx<C;++cIdx){
        for (int hIdx=0;hIdx<h_img;++hIdx){
	  for (int wIdx=0;wIdx<w_img;++wIdx){
            Dtype bottomData = bottom->data_at(n,cIdx,hIdx,wIdx);
	    topPtr[top->offset(n,cIdx,hIdx,wIdx)] = bottomData>=0?bottomData:0;
	  }
	}
      }
    }
}

template <typename Dtype>
void ReLU_Bwd(Blob<Dtype>* bottom,Blob<Dtype>* top,int N,int C,int h_img,int w_img){
    Dtype* bottomDiffPtr = bottom->mutable_cpu_diff();
    for (int n=0;n<N;++n){
      for (int cIdx=0;cIdx<C;++cIdx){
        for (int hIdx=0;hIdx<h_img;++hIdx){
	  for (int wIdx=0;wIdx<w_img;++wIdx){
	    bottomDiffPtr[bottom->offset(n,cIdx,hIdx,wIdx)] = bottom->data_at(n,cIdx,hIdx,wIdx)>=0?top->diff_at(n,cIdx,hIdx,wIdx):0;
	  }
	}
      }
    }
}

template <typename Dtype>
Dtype getMean(Blob<Dtype>* A,int channelIdx){
    int N = A->shape(0);
    int H = A->shape(2);
    int W = A->shape(3);
    int totalCount = N*H*W;

    Dtype sum = 0;
    for (int n=0;n<N;++n){
      for (int h=0;h<H;++h){
	for (int w=0;w<W;++w){
          sum += A->data_at(n,channelIdx,h,w);
	}
      }
    }
    return sum/totalCount;
}

template <typename Dtype>
Dtype getVar(Blob<Dtype>* A,int channelIdx){
    int N = A->shape(0);
    int H = A->shape(2);
    int W = A->shape(3);
    int totalCount = N*H*W;
    Dtype mean = getMean(A,channelIdx);

    Dtype sum = 0;
    for (int n=0;n<N;++n){
      for (int h=0;h<H;++h){
        for (int w=0;w<W;++w){
	  sum += (A->data_at(n,channelIdx,h,w)-mean) * (A->data_at(n,channelIdx,h,w)-mean);
	}
      }
    }
    return sum / totalCount;
}

template <typename Dtype>
void BN_inf_Fwd(Blob<Dtype>* input,Blob<Dtype>* output,int N,int C,int h_img,int w_img,Blob<Dtype>* globalMean,Blob<Dtype>* globalVar,Blob<Dtype>* scaler,Blob<Dtype>* bias,Blob<Dtype>* factor_b){
    int channelShape[] = {1,C,1,1};
    vector<int> channelShapeVec(channelShape,channelShape+4);
    Blob<Dtype>* localInf_Mean = new Blob<Dtype>(channelShapeVec);
    Blob<Dtype>* localInf_Var = new Blob<Dtype>(channelShapeVec);
    Dtype scale_factor = factor_b->cpu_data()[0] == 0 ? 0 : (1/factor_b->cpu_data()[0]);
    caffe_cpu_scale(localInf_Mean->count(),scale_factor,globalMean->cpu_data(),localInf_Mean->mutable_cpu_data());
    caffe_cpu_scale(localInf_Var->count(),scale_factor,globalVar->cpu_data(),localInf_Var->mutable_cpu_data());
    //Reshape output
    int outputShape[] = {N,C,h_img,w_img};
    vector<int> outputShapeVec(outputShape,outputShape+4);
    output->Reshape(outputShapeVec);
    //BN Fwd inf
    double epsilon = 1e-5;
    Dtype* outputPtr = output->mutable_cpu_data();

    for (int n=0;n<N;++n){
      for (int cIdx=0;cIdx<C;++cIdx){
        Dtype denom = 1.0 / sqrt(localInf_Var->data_at(0,cIdx,0,0) + epsilon);
	for (int hIdx=0;hIdx<h_img;++hIdx){
	  for (int wIdx=0;wIdx<w_img;++wIdx){
	    outputPtr[output->offset(n,cIdx,hIdx,wIdx)] = scaler->data_at(0,cIdx,0,0) * (denom * (input->data_at(n,cIdx,hIdx,wIdx) - localInf_Mean->data_at(0,cIdx,0,0))) + bias->data_at(0,cIdx,0,0);
	  }
	}
      }
    }
}

template <typename Dtype>
void BN_train_Fwd(Blob<Dtype>* bottom,Blob<Dtype>* top,Blob<Dtype>* output_xhat,Blob<Dtype>* globalMean,Blob<Dtype>* globalVar,Blob<Dtype>* batchMean,Blob<Dtype>* batchVar,Blob<Dtype>* scaler,Blob<Dtype>* bias,int N,int C,int h_img,int w_img,Dtype EMA_decay){
    //reshape output
    int outputShape[] = {N,C,h_img,w_img};
    vector<int> outputShapeVec(outputShape,outputShape+4);
    top->Reshape(outputShapeVec);
    output_xhat->Reshape(outputShapeVec);
    //BN Fwd train
    double epsilon = 1e-5;
    //get batch/global Mean/Var
    for (int channelIdx=0;channelIdx<C;++channelIdx){
      int variance_adjust_m = N*h_img*w_img;
      //batch
      Dtype* batchMean_mutable = batchMean->mutable_cpu_data();
      Dtype* batchVar_mutable = batchVar->mutable_cpu_data();
      batchMean_mutable[channelIdx] = getMean(bottom,channelIdx);
      batchVar_mutable[channelIdx] = (variance_adjust_m / (variance_adjust_m - 1.0)) * getVar(bottom,channelIdx);
      //global
      Dtype* globalMean_mutable = globalMean->mutable_cpu_data();
      Dtype* globalVar_mutable = globalVar->mutable_cpu_data();
      globalMean_mutable[channelIdx] = EMA_decay * globalMean->data_at(0,channelIdx,0,0) + batchMean->data_at(0,channelIdx,0,0);
      globalVar_mutable[channelIdx] = EMA_decay * globalVar->data_at(0,channelIdx,0,0) + batchVar->data_at(0,channelIdx,0,0);
    }
    //process data
    for (int n=0;n<N;++n){
      for (int c=0;c<C;++c){
        for (int h=0;h<h_img;++h){
	  for (int w=0;w<w_img;++w){
	    Dtype* xhat_mutable = output_xhat->mutable_cpu_data();
	    xhat_mutable[output_xhat->offset(n,c,h,w)] = (bottom->data_at(n,c,h,w) - batchMean->data_at(0,c,0,0))/sqrt(batchVar->data_at(0,c,0,0) + epsilon);
	    Dtype* output_mutable = top->mutable_cpu_data();
	    output_mutable[top->offset(n,c,h,w)] = (scaler->data_at(0,c,0,0)) * (output_xhat->data_at(n,c,h,w)) + bias->data_at(0,c,0,0);
	  }
	}
      }
    }
}

template <typename Dtype>
bool decide_channelDiffAllZero(Blob<Dtype>* B,int channelIdx,int N,int C,int H,int W){
  bool output = true;
  for (int n=0;n<N;++n){
    for (int h=0;h<H;++h){
      for (int w=0;w<W;++w){
        output = output && (B->diff_at(n,channelIdx,h,w)<0.001) && (B->diff_at(n,channelIdx,h,w)>-0.001);
      }
    }
  }
  return output;
}

template <typename Dtype>
void BN_train_Bwd(Blob<Dtype>* bottom,Blob<Dtype>* bottom_xhat,Blob<Dtype>* top,Blob<Dtype>* batchMean,Blob<Dtype>* batchVar,Blob<Dtype>* scaler,Blob<Dtype>* bias,int N,int C,int h_img,int w_img,bool betaOneData){
    double epsilon = 1e-5;
    //bias and scaler grad
    Dtype* biasGrad = bias->mutable_cpu_diff();
    Dtype* scalerGrad = scaler->mutable_cpu_diff();
    for (int channelIdx=0;channelIdx<C;++channelIdx){
      biasGrad[channelIdx] = 0;
      scalerGrad[channelIdx] = 0;
      for (int n=0;n<N;++n){
        for (int hIdx=0;hIdx<h_img;++hIdx){
	  for (int wIdx=0;wIdx<w_img;++wIdx){
	    biasGrad[channelIdx] += top->diff_at(n,channelIdx,hIdx,wIdx);
	    scalerGrad[channelIdx] += top->diff_at(n,channelIdx,hIdx,wIdx) * bottom_xhat->data_at(n,channelIdx,hIdx,wIdx);
	  }
	}
      }
    }
    //bottom data grad
    //helper 1:
    Dtype* XhatGrad = bottom_xhat->mutable_cpu_diff();
    for (int n=0;n<N;++n){
      for (int c=0;c<C;++c){
        for (int h=0;h<h_img;++h){
	  for (int w=0;w<w_img;++w){
	    XhatGrad[bottom_xhat->offset(n,c,h,w)] = top->diff_at(n,c,h,w) * scaler->data_at(0,c,0,0);
	  }
	}
      }
    }
    //helper 2:
    Dtype* varGrad = batchVar->mutable_cpu_diff();
    for (int c=0;c<C;++c){
      for (int n=0;n<N;++n){
        for (int h=0;h<h_img;++h){
	  for (int w=0;w<w_img;++w){
	    //varGrad[c] += bottom_xhat->diff_at(n,c,h,w) * (bottom->data_at(n,c,h,w)-batchMean->data_at(0,c,0,0)) * (-0.5) * pow(batchVar->data_at(0,c,0,0) + epsilon,-1.5);
	    varGrad[c] += bottom_xhat->diff_at(n,c,h,w) * (bottom->data_at(n,c,h,w)-batchMean->data_at(0,c,0,0)) * (-0.5) * (1.0 / ((batchVar->data_at(0,c,0,0)+epsilon) * sqrt(batchVar->data_at(0,c,0,0) + epsilon)));
	    //flag
	    //if (decide_channelDiffAllZero<Dtype>(top,c,N,C,h_img,w_img)){
	    //  std::cout<<varGrad[c]<<std::endl;
	    //}

	  }
	}
      }
    }

    //helper 3:
    double m = N * h_img * w_img;
    Dtype* meanGrad = batchMean->mutable_cpu_diff();
    for (int c=0;c<C;++c){
      for (int n=0;n<N;++n){
        for (int h=0;h<h_img;++h){
	  for (int w=0;w<w_img;++w){
	    meanGrad[c] += bottom_xhat->diff_at(n,c,h,w) * (-1.0 / sqrt(batchVar->data_at(0,c,0,0) + epsilon)) + batchVar->diff_at(0,c,0,0) * (-2.0) * (bottom->data_at(n,c,h,w) - batchMean->data_at(0,c,0,0)) / m;
            //if (decide_channelDiffAllZero<Dtype>(top,c,N,C,h_img,w_img)){
	    //  std::cout<<varGrad[c]<<std::endl;
	    //}

	  }
	}
      }
    }

    //combine helpers
    Dtype* bottomDataGrad = bottom->mutable_cpu_diff();
    for (int n=0;n<N;++n){
      for (int c=0;c<C;++c){
        for (int h=0;h<h_img;++h){
	  for (int w=0;w<w_img;++w){
	    //Dtype term1=bottom_xhat->diff_at(n,c,h,w)*pow(batchVar->data_at(0,c,0,0)+epsilon,-0.5);
	    Dtype term1=bottom_xhat->diff_at(n,c,h,w) / (sqrt(batchVar->data_at(0,c,0,0) + epsilon));
	    Dtype term2=batchVar->diff_at(0,c,0,0)*2.0*(bottom->data_at(n,c,h,w) - batchMean->data_at(0,c,0,0)) / m;
	    Dtype term3=batchMean->diff_at(0,c,0,0)/m;
	    if (betaOneData){
	      bottomDataGrad[bottom->offset(n,c,h,w)] += term1 + term2 + term3;
	    }
	    else {
	      bottomDataGrad[bottom->offset(n,c,h,w)] = term1 + term2 + term3;
	    }
	    //std::cout<<term1<<","<<term2<<","<<term3<<std::endl;
	  }
	}
      }
    }

 }

template <typename Dtype>
void DenseBlockLayer<Dtype>::CPU_Initialization(){
    this->batch_Mean.resize(this->numTransition);
    this->batch_Var.resize(this->numTransition);

    this->merged_conv.resize(this->numTransition + 1);
    this->BN_XhatVec.resize(this->numTransition);
    this->postBN_blobVec.resize(this->numTransition);
    this->postReLU_blobVec.resize(this->numTransition);
    this->postConv_blobVec.resize(this->numTransition);
    if (useBC){
        BC_BN_XhatVec.resize(this->numTransition);
        postBN_BCVec.resize(this->numTransition);
        postReLU_BCVec.resize(this->numTransition);
        postConv_BCVec.resize(this->numTransition);
	batch_Mean4G.resize(numTransition);
	batch_Var4G.resize(numTransition);
    }
    for (int transitionIdx=0;transitionIdx<this->numTransition;++transitionIdx){
      int conv_y_Channels = this->growthRate;
      int mergeChannels = this->initChannel + this->growthRate * transitionIdx;
      int channelShapeArr[] = {1,mergeChannels,1,1};
      int conv_y_ShapeArr[] = {this->N,conv_y_Channels,this->H,this->W};
      int mergeShapeArr[] = {this->N,mergeChannels,this->H,this->W};
      vector<int> channelShape(channelShapeArr,channelShapeArr+4);
      vector<int> conv_y_Shape(conv_y_ShapeArr,conv_y_ShapeArr+4);
      vector<int> mergeShape(mergeShapeArr,mergeShapeArr+4);

      this->batch_Mean[transitionIdx] = new Blob<Dtype>(channelShape);
      this->batch_Var[transitionIdx] = new Blob<Dtype>(channelShape);

      this->merged_conv[transitionIdx] = new Blob<Dtype>(mergeShape);
      this->BN_XhatVec[transitionIdx] = new Blob<Dtype>(mergeShape);
      this->postBN_blobVec[transitionIdx] = new Blob<Dtype>(mergeShape);
      this->postReLU_blobVec[transitionIdx] = new Blob<Dtype>(mergeShape);
      this->postConv_blobVec[transitionIdx] = new Blob<Dtype>(conv_y_Shape);
      if (useBC){
        int quadGShapeArr[] = {N,4*growthRate,H,W};
	int quadChannelArr[] = {1,4*growthRate,1,1};
        vector<int> quadGShape(quadGShapeArr,quadGShapeArr+4);
	vector<int> quadChannelShape(quadChannelArr,quadChannelArr+4);
        this->BC_BN_XhatVec[transitionIdx] = new Blob<Dtype>(quadGShape);
        this->postBN_BCVec[transitionIdx] = new Blob<Dtype>(quadGShape);
        this->postReLU_BCVec[transitionIdx] = new Blob<Dtype>(quadGShape);
        this->postConv_BCVec[transitionIdx] = new Blob<Dtype>(quadGShape);
	batch_Mean4G[transitionIdx] = new Blob<Dtype>(quadChannelShape);
	batch_Var4G[transitionIdx] = new Blob<Dtype>(quadChannelShape);
      }
    }
    //the last element of merged_conv serve as output of forward
    int extraMergeOutputShapeArr[] = {this->N,this->initChannel+this->growthRate*this->numTransition,this->H,this->W};
    vector<int> extraMergeOutputShapeVector(extraMergeOutputShapeArr,extraMergeOutputShapeArr+4);
    this->merged_conv[this->numTransition] = new Blob<Dtype>(extraMergeOutputShapeVector);
}

template <typename Dtype>
void mergeChannelData(Blob<Dtype>* outputBlob,Blob<Dtype>* blobA,Blob<Dtype>* blobB){
  int N = blobA->shape(0);
  int frontC = blobA->shape(1); int backC = blobB->shape(1);
  int H = blobA->shape(2);
  int W = blobA->shape(3);

  for (int n=0;n<N;++n){
    for (int c=0;c<frontC+backC;++c){
      for (int h=0;h<H;++h){
        for (int w=0;w<W;++w){
          Dtype inData;
	  if (c<frontC){
	    inData = blobA->cpu_data()[blobA->offset(n,c,h,w)];
	  }
	  else {
	    int readC = c - frontC;
	    inData = blobB->cpu_data()[blobB->offset(n,readC,h,w)];
	  }
	  outputBlob->mutable_cpu_data()[outputBlob->offset(n,c,h,w)] = inData;
	}
      }
    }
  }
}

template <typename Dtype>
void distributeChannelDiff(Blob<Dtype>* inputBlob,Blob<Dtype>* blobA,Blob<Dtype>* blobB){
  int N = blobA->shape(0);
  int frontC = blobA->shape(1); int backC = blobB->shape(1);
  int H = blobA->shape(2);
  int W = blobA->shape(3);

  for (int n=0;n<N;++n){
    for (int c=0;c<frontC+backC;++c){
      for (int h=0;h<H;++h){
        for (int w=0;w<W;++w){
	  Dtype readData = inputBlob->cpu_diff()[inputBlob->offset(n,c,h,w)];
          if (c<frontC){
	    blobA->mutable_cpu_diff()[blobA->offset(n,c,h,w)] = readData;
	  }
	  else {
            int writeC = c-frontC;
	    blobB->mutable_cpu_diff()[blobB->offset(n,writeC,h,w)] = readData;
	  }
        }
      }
    }
  }
}

template <typename Dtype>
void BlobSetZero(Blob<Dtype>* B,int count){
    Dtype* B_mutable_data = B->mutable_cpu_data();
    Dtype* B_mutable_diff = B->mutable_cpu_diff();
    for (int i=0;i<count;++i) {
      B_mutable_data[i] = 0;
      B_mutable_diff[i] = 0;
    }
}

template <typename Dtype>
void DenseBlockLayer<Dtype>::LoopEndCleanup_cpu(){
    for (int transitionIdx=0;transitionIdx<this->numTransition;++transitionIdx){
      int tensorCount = this->N * growthRate * this->H * this->W;
      int tensorMergeCount = this->N * (this->initChannel + this->growthRate * transitionIdx) * this->H * this->W;
      BlobSetZero<Dtype>(this->merged_conv[transitionIdx],tensorMergeCount);
      BlobSetZero<Dtype>(this->BN_XhatVec[transitionIdx],tensorMergeCount);
      BlobSetZero<Dtype>(this->postBN_blobVec[transitionIdx],tensorMergeCount);
      BlobSetZero<Dtype>(this->postReLU_blobVec[transitionIdx],tensorMergeCount);
      BlobSetZero<Dtype>(this->postConv_blobVec[transitionIdx],tensorCount);
    }
}

  template <typename Dtype>
  void DenseBlockLayer<Dtype>::Forward_cpu(const vector<Blob<Dtype>*>& bottom,
                                  const vector<Blob<Dtype>*>& top)
  {
    //init CPU
    if (!this->cpuInited){
	//std::cout<<"fwd cpu init"<<std::endl;
	this->CPU_Initialization();
        this->cpuInited = true;
	//std::cout<<"fwd cpu init done"<<std::endl;
    }
    int bnTimerIdx=useBC?10*numTransition:5*numTransition;
    //deploy init data
    this->merged_conv[0]->CopyFrom(*(bottom[0]));
    //init CPU finish
    for (int transitionIdx=0;transitionIdx<this->numTransition;++transitionIdx){
      //BN
      Blob<Dtype>* BN_bottom = this->merged_conv[transitionIdx];
      Blob<Dtype>* BN_top = this->postBN_blobVec[transitionIdx];
      Blob<Dtype>* Scaler = this->blobs_[numTransition + transitionIdx].get();
      Blob<Dtype>* Bias = this->blobs_[2*numTransition + transitionIdx].get();
      int localChannels = this->initChannel+transitionIdx*this->growthRate;
      if (this->phase_ == TEST){
	//std::cout<<"cpu BN test forward"<<std::endl;
        BN_inf_Fwd<Dtype>(BN_bottom,BN_top,this->N,localChannels,this->H,this->W,this->blobs_[3*this->numTransition+transitionIdx].get(),this->blobs_[4*this->numTransition+transitionIdx].get(),Scaler,Bias,this->blobs_[bnTimerIdx].get());
      }
      else {
	//std::cout<<"cpu BN train forward"<<std::endl;
        BN_train_Fwd<Dtype>(BN_bottom,BN_top,this->BN_XhatVec[transitionIdx],this->blobs_[3*this->numTransition+transitionIdx].get(),this->blobs_[4*this->numTransition+transitionIdx].get(),this->batch_Mean[transitionIdx],this->batch_Var[transitionIdx],Scaler,Bias,this->N,localChannels,this->H,this->W,this->EMA_decay);
      }
      //ReLU
      Blob<Dtype>* ReLU_top = this->postReLU_blobVec[transitionIdx];
      ReLU_Fwd<Dtype>(BN_top,ReLU_top,this->N,localChannels,this->H,this->W);
      //if useBC, Conv1*1-BN(BC)-ReLU(BC)
      if (useBC){
	//BC Conv 1*1
	Blob<Dtype>* BC_filterBlob = this->blobs_[5*numTransition+transitionIdx].get();
        Blob<Dtype>* BC_conv_x = postReLU_blobVec[transitionIdx];
        Blob<Dtype>* BC_conv_y = postConv_BCVec[transitionIdx];
	int BC_conv_inChannel = initChannel+growthRate*transitionIdx;
	int BC_conv_outChannel = 4*growthRate;
        convolution_Fwd<Dtype>(BC_conv_x,BC_conv_y,BC_filterBlob,N,BC_conv_outChannel,BC_conv_inChannel,H,W,1,1);
	//BC BN
	Blob<Dtype>* BC_BN_x = postConv_BCVec[transitionIdx];
	Blob<Dtype>* BC_BN_y = postBN_BCVec[transitionIdx];
        Blob<Dtype>* BC_Scaler = this->blobs_[6*numTransition+transitionIdx].get();
	Blob<Dtype>* BC_Bias = this->blobs_[7*numTransition+transitionIdx].get();
	Blob<Dtype>* BC_Mean = this->blobs_[8*numTransition+transitionIdx].get();
	Blob<Dtype>* BC_Var = this->blobs_[9*numTransition+transitionIdx].get();
	if (this->phase_ == TEST){
	  BN_inf_Fwd<Dtype>(BC_BN_x,BC_BN_y,N,4*growthRate,H,W,BC_Mean,BC_Var,BC_Scaler,BC_Bias,this->blobs_[bnTimerIdx].get());
	}
	else {
	  Blob<Dtype>* BC_xhat = BC_BN_XhatVec[transitionIdx];
	  Blob<Dtype>* BC_batchMean = batch_Mean4G[transitionIdx];
	  Blob<Dtype>* BC_batchVar = batch_Var4G[transitionIdx];
	  BN_train_Fwd<Dtype>(BC_BN_x,BC_BN_y,BC_xhat,BC_Mean,BC_Var,BC_batchMean,BC_batchVar,BC_Scaler,BC_Bias,N,4*growthRate,H,W,EMA_decay);
	}
	//BC ReLU
	Blob<Dtype>* ReLU_x = postBN_BCVec[transitionIdx];
	Blob<Dtype>* ReLU_y = postReLU_BCVec[transitionIdx];
	ReLU_Fwd<Dtype>(ReLU_x,ReLU_y,N,4*growthRate,H,W);
      }
      //Conv
      Blob<Dtype>* filterBlob = this->blobs_[transitionIdx].get();
      Blob<Dtype>* conv_x = useBC?postReLU_BCVec[transitionIdx]:postReLU_blobVec[transitionIdx];
      Blob<Dtype>* conv_y = this->postConv_blobVec[transitionIdx];
      int inConvChannel = useBC?4*growthRate:initChannel+growthRate*transitionIdx;
      convolution_Fwd<Dtype>(conv_x,conv_y,filterBlob,N,growthRate,inConvChannel,H,W,3,3);
      //post Conv merge
      Blob<Dtype>* mergeOutput = merged_conv[transitionIdx+1];
      Blob<Dtype>* mergeInputA = merged_conv[transitionIdx];
      Blob<Dtype>* mergeInputB = postConv_blobVec[transitionIdx];
      mergeChannelData(mergeOutput,mergeInputA,mergeInputB);
    }
    //deploy output data
    top[0]->CopyFrom(*(this->merged_conv[this->numTransition]));
    if (this->phase_ == TRAIN){
      this->blobs_[bnTimerIdx]->mutable_cpu_data()[0] *= this->EMA_decay;
      this->blobs_[bnTimerIdx]->mutable_cpu_data()[0] += 1;
      this->trainCycleIdx+=1;
    }
    //logInternal_cpu("TC_TrueFwdlog");
  }

  template <typename Dtype>
  void DenseBlockLayer<Dtype>::Backward_cpu(const vector<Blob<Dtype>*>& top,
                                    const vector<bool>& propagate_down,
                                    const vector<Blob<Dtype>*>& bottom)
  {
    if (!this->cpuInited){
	this->CPU_Initialization();
        this->cpuInited = true;
    }
    //deploy top diff
    this->merged_conv[this->numTransition]->CopyFrom(*(top[0]),true);
    for (int transitionIdx=this->numTransition-1;transitionIdx>=0;--transitionIdx){
      //distribute diff
      distributeChannelDiff(this->merged_conv[transitionIdx+1],this->merged_conv[transitionIdx],this->postConv_blobVec[transitionIdx]);
      //Conv Bwd
      Blob<Dtype>* conv_top=this->postConv_blobVec[transitionIdx];
      Blob<Dtype>* conv_bottom=useBC?postReLU_BCVec[transitionIdx]:postReLU_blobVec[transitionIdx];
      Blob<Dtype>* filter = this->blobs_[transitionIdx].get();
      int c_input = useBC?4*growthRate:initChannel+growthRate*transitionIdx;
      convolution_Bwd<Dtype>(conv_bottom,conv_top,filter,this->N,this->growthRate,c_input,this->H,this->W,3,3);
      //BC ReLU_BC_Bwd - BN_BC_Bwd - Conv1*1_BC_Bwd
      if (useBC){
	//ReLU BC Bwd
        Blob<Dtype>* BC_ReLU_y = postReLU_BCVec[transitionIdx];
	Blob<Dtype>* BC_ReLU_x = postBN_BCVec[transitionIdx];
	ReLU_Bwd<Dtype>(BC_ReLU_x,BC_ReLU_y,N,4*growthRate,H,W);
	//BN BC Bwd
	Blob<Dtype>* BC_BN_y = postBN_BCVec[transitionIdx];
	Blob<Dtype>* BC_BN_x = postConv_BCVec[transitionIdx];
	Blob<Dtype>* BC_BN_xhat = BC_BN_XhatVec[transitionIdx];
	Blob<Dtype>* BC_Scaler = this->blobs_[6*numTransition+transitionIdx].get();
	Blob<Dtype>* BC_Bias = this->blobs_[7*numTransition+transitionIdx].get();
	Blob<Dtype>* BC_batchMean = batch_Mean4G[transitionIdx];
	Blob<Dtype>* BC_batchVar = batch_Var4G[transitionIdx];
	BN_train_Bwd<Dtype>(BC_BN_x,BC_BN_xhat,BC_BN_y,BC_batchMean,BC_batchVar,BC_Scaler,BC_Bias,N,4*growthRate,H,W,false);
	//Conv1*1 BC Bwd
	Blob<Dtype>* BC_conv_x = postReLU_blobVec[transitionIdx];
	Blob<Dtype>* BC_conv_y = postConv_BCVec[transitionIdx];
	Blob<Dtype>* BC_filter = this->blobs_[5*numTransition+transitionIdx].get();
	int BC_c_input = initChannel+growthRate*transitionIdx;
	int BC_c_output = 4*growthRate;
	convolution_Bwd<Dtype>(BC_conv_x,BC_conv_y,BC_filter,N,BC_c_output,BC_c_input,H,W,1,1);
      }
      //ReLU Bwd
      int localChannel = this->initChannel+this->growthRate*transitionIdx;
      ReLU_Bwd<Dtype>(postBN_blobVec[transitionIdx],postReLU_blobVec[transitionIdx],this->N,localChannel,this->H,this->W);
      //BN Bwd
      Blob<Dtype>* BN_bottom = this->merged_conv[transitionIdx];
      Blob<Dtype>* scaler = this->blobs_[this->numTransition+transitionIdx].get();
      Blob<Dtype>* bias = this->blobs_[2*this->numTransition+transitionIdx].get();
      BN_train_Bwd<Dtype>(BN_bottom,this->BN_XhatVec[transitionIdx],this->postBN_blobVec[transitionIdx],this->batch_Mean[transitionIdx],this->batch_Var[transitionIdx],scaler,bias,this->N,localChannel,this->H,this->W,true);
    }
    bottom[0]->CopyFrom(*(this->merged_conv[0]),true);
    //logInternal_cpu("TC_TrueBwdlog");
    this->LoopEndCleanup_cpu();
}

template <typename Dtype>
void DenseBlockLayer<Dtype>::Forward_cpu_public(const vector<Blob<Dtype>*>& bottom,const vector<Blob<Dtype>*>& top){
  this->Forward_cpu(bottom,top);
}

template <typename Dtype>
void DenseBlockLayer<Dtype>::Backward_cpu_public(const vector<Blob<Dtype>*>& top,const vector<bool>& propagate_down,const vector<Blob<Dtype>*>& bottom){
  this->Backward_cpu(top,propagate_down,bottom);
}

#ifdef CPU_ONLY
STUB_GPU(DenseBlockLayer);
#endif

INSTANTIATE_CLASS(DenseBlockLayer);
REGISTER_LAYER_CLASS(DenseBlock);

}  // namespace caffe


#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/foreach.hpp>
#include <tuple>
#include <algorithm>
#include <set>
#include <queue>
#include <map>
#include <string>
#include <sstream>
#include <vector>
#include <iostream>
#include <fstream>
#include "cml.h"
#include "Valences.h"

using boost::property_tree::ptree;
using std::map;
using std::set;
using std::queue;
using std::string;
using std::pair;
using std::vector;
using std::ostringstream;
using std::logic_error;

class Atom
{
	string id;
	string element;
	int valence; // 0 is it must be deduced from bonds number and standard valences
	int bondsCnt;   // to accumulate number of explicit bonds
	int charge;
public:
	Atom(const string& id, const string& element, int valence = 0, int charge = 0) : id(id), element(element), valence(valence), bondsCnt(0), charge(charge)
	{
	}

	void addBonds(int order)
	{
		bondsCnt += order;

		if (valence != 0 && bondsCnt > valence)
		{
			ostringstream oss;
			oss << "Atom with id '" << id << "' has more bounds than valence.";
			throw logic_error(oss.str());
		}
	}

	// if valence == 0 then deduce it from bonds and standard valences for that element
	void correct_valence()
	{
		if (this->valence == 0)
			this->valence = Valences::getCeilValence(this->element, this->bondsCnt);
	}

	int getImplicitHydrogenCnt() const {
		return valence - bondsCnt + charge;
	}

	string getElement() const { return element; }
};

class Atoms
{
	map<string, Atom> atoms; // atom id => Atom

	// atom id => list of bound atom ids
	map<string, vector<string> > graph;

	void addAtom(const string& id, const string& element, int valence = 0, int charge = 0)
	{
		if (atoms.find(id) != atoms.end())
		{
			ostringstream oss;
			oss << "Atom with id '" << id << "' already exists.";
			throw logic_error(oss.str());
		}

		atoms.insert(make_pair(id, Atom(id, element, valence, charge)));
		graph.insert(make_pair(id, vector<string>()));
	}

	void addBond(const string& id1, const string& id2, int order)
	{
		graph[id1].push_back(id2);
		graph[id2].push_back(id1);
		Atom& a1 = atoms.find(id1)->second;
		Atom& a2 = atoms.find(id2)->second;
		a1.addBonds(order);
		a2.addBonds(order);
	}

	vector<vector<string>> get_connected_components() const
	{
		vector<vector<string> > connected_components;
		set<string> visited;

		for (auto it = graph.cbegin(); it != graph.cend(); ++it)
		{
			const string& currentAtomID = it->first;

			if (visited.find(currentAtomID) == visited.end())
			{
				vector<string> component;
				queue<string> q;
				q.push(currentAtomID);
				visited.insert(currentAtomID);
				component.push_back(currentAtomID);

				while (!q.empty())
				{
					string neighborID = q.front();
					q.pop();
					const vector<string>& adjList = graph.find(neighborID)->second;

					for (auto adjIt = adjList.cbegin(); adjIt != adjList.cend(); ++adjIt)
					{
						if (visited.find(*adjIt) == visited.end())
						{
							visited.insert(*adjIt);
							q.push(*adjIt);
							component.push_back(*adjIt);
						}
					}
				}

				connected_components.push_back(component);
			}
		}

		return connected_components;
	}

	static string getFormula(const map<string, int>& molecule)
	{
		std::ostringstream oss;
		std::map<string, int>::const_iterator tC, tH;
		tH = molecule.end();
		tC = molecule.find("C");
		if(tC != molecule.end())
		{
			const string& element = tC->first;
			int cnt = tC->second;

			oss << element;

			if (cnt > 1)
				oss << cnt;

			tH = molecule.find("H");

			const string& element1 = tH->first;
			cnt = tH->second;

			oss << element1;

			if (cnt > 1)
				oss << cnt;
		}
		for (std::map<string, int>::const_iterator el = molecule.begin(); el != molecule.end(); ++el)
		{
			if(el == tC || el == tH)
				continue;
			const string& element = el->first;
			int cnt = el->second;

			oss << element;

			if (cnt > 1)
				oss << cnt;
		}

		return oss.str();
	}

	// vector of molecules, where molecule = { element => element count }
	vector<map<string, int>> getMolecules() const
	{
		vector<map<string, int>> molecules;

		vector<vector<string>> connected_components = this->get_connected_components();
		for (std::vector<vector<string>>::const_iterator component = connected_components.begin(); component != connected_components.end(); ++component)
		{
			map<string, int> molecule;
			int implicitHydrogenCnt = 0;
			for (std::vector<string>::const_iterator id = (*component).begin(); id != (*component).end(); ++id)
			{
				const Atom& a = atoms.find(*id)->second;
				molecule[a.getElement()]++;
				implicitHydrogenCnt += a.getImplicitHydrogenCnt();
			}

			if (implicitHydrogenCnt)
				molecule["H"] += implicitHydrogenCnt;

			molecules.push_back(molecule);
		}

		return molecules;
	}

public:
	void readFromCML(const string& filename)
	{
		ptree pt;
		try
		{
			read_xml(filename, pt);
		}
		catch(std::exception &e)
		{
			throw string("Error: no file exists");
		}
		std::ifstream chem_atom ("table.txt");
		if(!chem_atom.is_open())
			throw string("Error: source file problem");
		vector<string> atom_tab;
		while(true)
		{
			if(chem_atom.eof())
				break;
			string str;
			getline(chem_atom, str, '\n');
			atom_tab.push_back(str);
		}
		BOOST_FOREACH(ptree::value_type &i, pt.get_child("cml.molecule.atomArray"))
		{
			std::string name = i.first;
			ptree& sub_pt = i.second;
			int valence = 0;
			int charge = 0;

			if (name == "atom")
			{
				string id = sub_pt.get<string>("<xmlattr>.id");
				string elementType = sub_pt.get<string>("<xmlattr>.elementType");

				if(find(atom_tab.begin(), atom_tab.end(), elementType) == atom_tab.end())
				{
					throw string("Error: non chemical atom in file");
				}

				boost::optional<int> mrvValence = sub_pt.get_optional<int>("<xmlattr>.mrvValence");

				if (mrvValence)
				{
					valence = mrvValence.value();
				}

				boost::optional<int> formalcharge = sub_pt.get_optional<int>("<xmlattr>.formalCharge");

				if (formalcharge)
				{
					charge = formalcharge.value();
				}

				boost::optional<int> hydrogenCount = sub_pt.get_optional<int>("<xmlattr>.hydrogenCount");

				if (hydrogenCount)
				{
					valence -= hydrogenCount.value();
				}

				this->addAtom(id, elementType, valence, charge);
			}
		}

		BOOST_FOREACH(ptree::value_type &i, pt.get_child("cml.molecule.bondArray"))
		{
			std::string name = i.first;
			ptree& sub_pt = i.second;

			if (name == "bond")
			{
				string atomRefs2 = sub_pt.get<string>("<xmlattr>.atomRefs2");
				int order = sub_pt.get<int>("<xmlattr>.order");

				vector<string> ids;
				boost::split(ids, atomRefs2, boost::is_any_of(" \t\n"));
				if (ids.size() != 2)
				{
					ostringstream oss;
					oss << "Wrong bond '" << atomRefs2 << "'.";
					throw logic_error(oss.str());
				}
				this->addBond(ids.at(0), ids.at(1), order);
			}
		}

	}

	void correct_valences()
	{
		for (std::map<string, Atom>::iterator id_atom = atoms.begin(); id_atom != atoms.end(); ++id_atom)
		{
			id_atom->second.correct_valence();
		}
	}

	string getFormula()
	{
		vector<map<string, int>> molecules = this->getMolecules();

		vector<string> molecules_str;
		for (std::vector<map<string, int>>::const_iterator molecule = molecules.begin(); molecule != molecules.end(); ++molecule)
		{
			molecules_str.push_back(getFormula(*molecule));
		}

		string formula = "";
		for (unsigned i = 0; i < molecules_str.size(); ++i)
		{
			if (i > 0)
				formula += ".";
			formula += molecules_str[i];
		}
		return formula;
	}
};

string getFormulaFromCML(const string& xmlfile)
{
	Atoms atoms;
	atoms.readFromCML(xmlfile);
	atoms.correct_valences();
	return atoms.getFormula();
}


/// @file tracker.hpp Tracking features in input partitions

#ifndef BIGGLES_TRACKER_HPP___
#define BIGGLES_TRACKER_HPP___
#include <boost/tuple/tuple.hpp>
#include <boost/assert.hpp>

#include "mh_moves/mh_moves.hpp"
#include "model.hpp"
#include "partition.hpp"
#include "samplers.hpp"
#include "partition_sampler.hpp"
#include "sampling/gibbs.hpp"
#include "sampling/metropolis_hastings.hpp"

#include "tracker_detail.hpp"

namespace biggles
{

/** \brief a record for the move statistics
 *
 * this stores how often a proposed move was accepted, rejected or produced an identity move
 * the birth move has index 0, the number of *proper* moves is IDENTITY - BIRTH
 */
struct move_stats_t {
    std::deque<uint64_t> rejected;
    std::deque<uint64_t> accepted;
    std::deque<uint64_t> identity;
    move_stats_t () {
        size_t len = static_cast<size_t>(mh_moves::IDENTITY - mh_moves::BIRTH);
        rejected.assign(len, 0);
        accepted.assign(len, 0);
        identity.assign(len, 0);
    }
};

/// @brief An implementation of the Biggles tracking algorithm.
///
/// The Biggles tracking algorithm is at it's highest level a sampler which maintains an optimum over the samples drawn.
/// More specifically, the Biggles model specifies that there are three main components to the mathematical model used
/// to represent the state of the world:
///
/// - the input observations from the feature detector, \f$ D \f$;
/// - the association between these observations and one or more tracks, \f$ T \f$; and
/// - parameters for the underlying dynamic model of molecule evolution, \f$ \theta \f$.
///
/// The track association also includes a special 'track' known as the clutter which holds all observations we assert
/// are due to spurious detection rather than a tracked molecule. This is the only track which may hold more than one
/// observation per time stamp.
///
/// The value of \f$ T \f$ is represented in this code by biggles::partition which stores a collection of
/// biggles::observation instances for the clutter and a collection of biggles::track instances for each track. The
/// collection of tracks is implemented by biggles::track_collection which implicitly shares tracks between collections.
///
/// The tracking algorithm proceeds by drawing samples of \f$ T \f$ and \f$ \theta \f$ from the joint distribution \f$
/// P(T, \theta | D) \f$. By definition of the mode, we expect more of these samples to be close to the optimum than far
/// from it. We therefore maintain an 'optimal' sample 2-tuple, \f$ ( \hat{T}, \hat{\theta} ) \f$, which serve as our
/// current estimate for the 'true' tracks. After drawing each sample from the joint distribution, we compare the value
/// of the distribution at that point to that of our 'best' sample. If the newly drawn sample has a higher posterior
/// than the current best one, replace the best sample with the one just drawn.
///
/// Details of precisely how these samples are drawn and the mathematical model we use can be found elsewhere in the
/// documentation as indicated by the 'see also' section below.
///
/// @sa biggles::model
/// @sa biggles::partition
/// @sa biggles::sampling
/// @sa biggles::track
/// @sa sampling::gibbs_sampler
///
class tracker : public sampling::new_gibbs_sampler {
public:
    /// @brief Default constructor.
    tracker();

    tracker(const tracker& t);
    /// @brief Initialise the tracker with an initial set of model parameters and partition.
    ///
    /// @param initial_parameters
    /// @param initial_partition
    tracker(const model::parameters& initial_parameters,
            const partition_ptr_t& initial_partition);

    /// @brief Copy constructor.
    ///
    /// @param t

    /// @brief Advance the tracking algorithm one step.
    ///
    /// This draws a single sample for \f$ (T, \theta) \f$ and performs the necessary bookkeeping to maintain the 'best'
    /// sample.
    void advance();

    /// @brief Equivalent to calling advance().
    void operator () () { advance(); }

    /// @name Access to the current tracker state
    /// @{

    /// @brief The number of iterations performed for the algorithm.
    size_t n_iterations() const { return n_iterations_; }

    /// @brief The last value of \f$ T \f$ drawn by the sampler.
    const partition_ptr_t last_partition() const { return last_partition_sample().partition_sample_ptr; }

    /// @brief The last move type accepted by the partition sampler.
    const mh_moves::move_type last_move_type() const {
        return last_proposal_accepted() ? last_partition_sample().executed_move : mh_moves::NONE;
    }

    /// @brief The last value of \f$ \theta \f$ drawn by the sampler.
    const model::parameters& last_parameters() const { return last_sample().get<1>(); }

    /// @brief The last value of \f$ P(T | \theta) \f$ for the last sample drawn.
    float last_log_pdf() const
        { return underlying_partition_sampler().current_sample_log_density(); }

    /// \brief the current state of the Metropolis Hastings sampler
    sampling::mh_state_observer mh_state() const {
        return underlying_partition_sampler().current_state();
    }

    internal_containers_observer mh_internals() const {
        return underlying_partition_sampler().current_internals();
    }

    /// @brief The current best estimate of the true partition.
    const partition_ptr_t best_partition() const { return best_partition_; }

    /// @brief The current best estimate of the true model parameters.
    const model::parameters& best_parameters() const { return best_parameters_; }

    /// @brief The current value of \f$ P(\hat{T} | \hat{\theta}) \f$.
    float best_log_pdf() const { return best_log_pdf_; }

    float acceptance_rate() const
        { return underlying_partition_sampler().acceptance_rate(); }

    void set_partition_is_valid(bool v)
        { underlying_partition_sampler().set_partition_is_valid(v); }

    bool last_proposal_accepted() const
        { return underlying_partition_sampler().last_proposal_accepted(); }

    /// \brief the partition that was proposed in the latest sample (accepted or not)
    const partition_ptr_t last_proposal_partition() const {
        return underlying_partition_sampler().proposed_sample().partition_sample_ptr;
    }
    /// \brief the move that was proposed in the latest sample (accepted or not)
    mh_moves::move_type last_proposal_move() const {
        return underlying_partition_sampler().proposed_sample().proposed_move;
    }
    partition_sampler_sample last_proposed_sample() const {
        return underlying_partition_sampler().proposed_sample();
    }

    /// \brief the proposal density of the last proposal
    float last_proposal_density() const {
        return underlying_partition_sampler().last_log_density();
    }

    /// \brief the PDR of the last MH comparision
    float last_pdr() const {
        return underlying_partition_sampler().last_pdr();
    }

    /// \brief the recent acceptance rate
    float recent_acceptance_rate() const {
        return underlying_partition_sampler().recent_acceptance_rate();
    }

    /// \brief the log alpha of the last MH comparision
    float last_log_alpha() const {
        return underlying_partition_sampler().last_log_alpha();
    }

    /// @brief Return a reference to the histogram of accepted move types.
    const std::deque<uint64_t>& move_histogram() const { return move_hist_; }

    /// @brief Return a reference to the data of move type statistics (a/r/i).
    const move_stats_t& move_statistics() const { return move_stats_; }

    void set_temperature(float temperature) { underlying_partition_sampler().set_temperature(temperature); }

    /// @}

    /// @brief Assignment operator.
    ///
    /// @param t The tracker to copy state from.
    ///
    /// @return A reference to \c *this.
    const tracker& operator = (const tracker& t);

protected:
    /// @brief The number of iterations which have currently been performed.
    size_t n_iterations_;

    /// @brief Our current estimate of \f$ \hat{T} \f$.
    partition_ptr_t best_partition_;

    /// @brief Our current estimate of \f$ \hat{\theta} \f$.
    model::parameters best_parameters_;

    /// @brief The value of \f$ P(T|\theta) \f$ for our current best estimate.
    float best_log_pdf_;

    /// @brief Histogram of accepted move types.
    std::deque<uint64_t> move_hist_;

    /// @brief data for move statistics ([a]ccepted/[r]ejected/[i]dentity)
    move_stats_t move_stats_;

    /// @brief Record an accepted move type in the move type histogram.
    ///
    /// @param mt
    void record_move_type_(mh_moves::move_type mt);

    /// @brief Record the the statistics ([a]ccepted/[r]ejected/[i]dentity) for each move.
    ///
    /// @param sample
    void record_move_stats_(const partition_sampler_sample& sample);
};

}

#endif // BIGGLES_TRACKER_HPP___


#include <memory>
#include <iostream>
#include <fstream>

#include <deal.II/base/conditional_ostream.h>
#include <deal.II/base/parameter_handler.h>
#include <deal.II/base/mpi.h>

#include "framework/builder/framework_builder.h"
#include "utility/reporter/mpi.h"
#include "problem/parameters_dealii_handler.h"

int main(int argc, char* argv[]) {
  try {
    if (argc != 2) {
      std::cerr
          << "Call the program as mpirun -np num_proc bart input_file_name"
          << std::endl;
      return 1;
    }

    std::cout << "BAY AREA RADIATION TRANSPORT\n"
              << "Developed at the University of California, Berkeley"
              << std::endl;

    bart::problem::ParametersDealiiHandler prm;
    dealii::ParameterHandler d2_prm;
    const std::string filename{argv[1]};

    prm.SetUp(d2_prm);
    d2_prm.parse_input(filename, "");
    prm.Parse(d2_prm);

    dealii::Utilities::MPI::MPI_InitFinalize mpi_initialization(argc, argv, 1);

    double k_eff_final;

    const int n_processes = dealii::Utilities::MPI::n_mpi_processes(MPI_COMM_WORLD);
    const int process_id = dealii::Utilities::MPI::this_mpi_process(MPI_COMM_WORLD);

    // Open file for output, if there are multiple processes they will end with
    // a number indicating the process number.
    std::ofstream output_stream, master_output_stream;
    const std::string output_filename_base{prm.OutputFilenameBase()};
    if (n_processes > 1) {
      const std::string full_filename = output_filename_base + dealii::Utilities::int_to_string(process_id, 4);
      output_stream.open((full_filename + ".vtu").c_str());
      master_output_stream.open(output_filename_base + ".pvtu");
    } else {
      output_stream.open((output_filename_base + ".vtu").c_str());
    }
    std::vector<std::string> filenames;
    // Write master pvtu record if multiple files are required
    if ((n_processes > 1) && (process_id == 0)) {
      for (int process = 0; process < n_processes; ++process) {
        const std::string full_filename =
            output_filename_base + dealii::Utilities::int_to_string(process, 4);
        filenames.push_back(full_filename + ".vtu");
      }
    }

    auto reporter_ptr = std::make_shared<bart::utility::reporter::Mpi>(
        std::make_unique<dealii::ConditionalOStream>(std::cout, process_id == 0));
    switch(prm.SpatialDimension()) {
      case 1: {
        bart::framework::builder::FrameworkBuilder<1> builder(reporter_ptr);
        auto framework_ptr = builder.BuildFramework("main", prm);
        framework_ptr->SolveSystem();
        framework_ptr->OutputResults(output_stream);
        if (n_processes > 1)
          framework_ptr->OutputMasterFile(master_output_stream, filenames, process_id);
        k_eff_final = framework_ptr->system()->k_effective.value_or(0);
        break;
      }
      case 2: {
        bart::framework::builder::FrameworkBuilder<2> builder(reporter_ptr);
        auto framework_ptr = builder.BuildFramework("main", prm);
        framework_ptr->SolveSystem();
        framework_ptr->OutputResults(output_stream);
        if (n_processes > 1)
          framework_ptr->OutputMasterFile(master_output_stream, filenames, process_id);
        k_eff_final = framework_ptr->system()->k_effective.value_or(0);
        break;
      }
      case 3: {
        bart::framework::builder::FrameworkBuilder<3> builder(reporter_ptr);
        auto framework_ptr = builder.BuildFramework("main", prm);
        framework_ptr->SolveSystem();
        framework_ptr->OutputResults(output_stream);
        if (n_processes > 1)
          framework_ptr->OutputMasterFile(master_output_stream, filenames, process_id);
        k_eff_final = framework_ptr->system()->k_effective.value_or(0);
        break;
      }
    }

    std::cout << "Final k_effective: " << k_eff_final << std::endl;

  } catch (std::exception &exc) {
    std::cerr << std::endl << std::endl
              << "----------------------------------------------------"
              << std::endl;
    std::cerr << "Exception on processing: " << std::endl
              << exc.what() << std::endl
              << "Aborting!" << std::endl
              << "----------------------------------------------------"
              << std::endl;
    return 1;
  } catch (...) {
    std::cerr << std::endl << std::endl
              << "----------------------------------------------------"
              << std::endl;
    std::cerr << "Unknown exception!" << std::endl
              << "Aborting!" << std::endl
              << "----------------------------------------------------"
              << std::endl;
    return 1;
  }
  return 0;
}