//============================================================================
//
// This file is part of the Thea toolkit.
//
// This software is distributed under the BSD license, as detailed in the
// accompanying LICENSE.txt file. Portions are derived from other works:
// their respective licenses and copyright information are reproduced in
// LICENSE.txt and/or in the relevant source files.
//
// Author: Siddhartha Chaudhuri
// First version: 2019
//
//============================================================================

#ifndef __Thea_MatVec_hpp__
#define __Thea_MatVec_hpp__

#include "Common.hpp"
#include "MatrixFormat.hpp"

// Resolve a conflict between X.h (which #defines Success) and Eigen (which has an enum called Success). Fortunately both
// headers currently assign the same value to Success (0) so the #undef should be all that's needed.
// https://stackoverflow.com/questions/22400905/eigen-and-cimg-compatibility-issues
#ifdef Success
#  undef Success
#endif
#include <Eigen/Core>
#include <Eigen/Geometry>  // for Quaternion, and because for some weird reason Eigen defines cross() here

#include <complex>

namespace Thea {

#ifdef THEA_ROW_MAJOR
  int const DEFAULT_MATRIX_LAYOUT = MatrixLayout::ROW_MAJOR;
#else  // nothing provided or THEA_COLUMN_MAJOR defined
  int const DEFAULT_MATRIX_LAYOUT = MatrixLayout::COLUMN_MAJOR;
#endif

// Typedef common instantiations. These are fully instantiated and have no further template parameters.
// Alignment is DISABLED for fixed-size vectorizable types (Vector4f, Matrix4f, Vector2d etc). These require special handling
// for every class which directly or indirectly has such a member, and could lead to unexpected bugs and considerably less ease
// of use.
//
// See:
// - http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
// - https://eigen.tuxfamily.org/dox/group__TopicStlContainers.html
// - http://eigen.tuxfamily.org/dox/group__TopicFixedSizeVectorizable.html
// - https://stackoverflow.com/q/41087043
//
#define THEA_DECL_MATRIX_TYPEDEFS(scalar, suffix)                                                                             \
    typedef Eigen::Matrix< scalar, 2, 2,              DEFAULT_MATRIX_LAYOUT      | Eigen::DontAlign >  Matrix2    ## suffix;  \
    typedef Eigen::Matrix< scalar, 3, 3,              DEFAULT_MATRIX_LAYOUT      | Eigen::DontAlign >  Matrix3    ## suffix;  \
    typedef Eigen::Matrix< scalar, 4, 4,              DEFAULT_MATRIX_LAYOUT      | Eigen::DontAlign >  Matrix4    ## suffix;  \
    typedef Eigen::Matrix< scalar, 2, 1,              MatrixLayout::COLUMN_MAJOR | Eigen::DontAlign >  Vector2    ## suffix;  \
    typedef Eigen::Matrix< scalar, 3, 1,              MatrixLayout::COLUMN_MAJOR | Eigen::DontAlign >  Vector3    ## suffix;  \
    typedef Eigen::Matrix< scalar, 4, 1,              MatrixLayout::COLUMN_MAJOR | Eigen::DontAlign >  Vector4    ## suffix;  \
    typedef Eigen::Matrix< scalar, 1, 2,              MatrixLayout::ROW_MAJOR    | Eigen::DontAlign >  RowVector2 ## suffix;  \
    typedef Eigen::Matrix< scalar, 1, 3,              MatrixLayout::ROW_MAJOR    | Eigen::DontAlign >  RowVector3 ## suffix;  \
    typedef Eigen::Matrix< scalar, 1, 4,              MatrixLayout::ROW_MAJOR    | Eigen::DontAlign >  RowVector4 ## suffix;  \
    typedef Eigen::Matrix< scalar, 2, Eigen::Dynamic, DEFAULT_MATRIX_LAYOUT                         >  Matrix2X   ## suffix;  \
    typedef Eigen::Matrix< scalar, 3, Eigen::Dynamic, DEFAULT_MATRIX_LAYOUT                         >  Matrix3X   ## suffix;  \
    typedef Eigen::Matrix< scalar, 4, Eigen::Dynamic, DEFAULT_MATRIX_LAYOUT                         >  Matrix4X   ## suffix;  \
    typedef Eigen::Matrix< scalar, Eigen::Dynamic, 2, DEFAULT_MATRIX_LAYOUT                         >  MatrixX2   ## suffix;  \
    typedef Eigen::Matrix< scalar, Eigen::Dynamic, 3, DEFAULT_MATRIX_LAYOUT                         >  MatrixX3   ## suffix;  \
    typedef Eigen::Matrix< scalar, Eigen::Dynamic, 4, DEFAULT_MATRIX_LAYOUT                         >  MatrixX4   ## suffix;

THEA_DECL_MATRIX_TYPEDEFS(Real, )
THEA_DECL_MATRIX_TYPEDEFS(float, f)
THEA_DECL_MATRIX_TYPEDEFS(double, d)
THEA_DECL_MATRIX_TYPEDEFS(std::complex<float>, cf)
THEA_DECL_MATRIX_TYPEDEFS(std::complex<double>, cd)
THEA_DECL_MATRIX_TYPEDEFS(int, i)

#undef THEA_DECL_MATRIX_TYPEDEFS

// Typedef additional instantiations of fully resizable matrices, NOT including the "plain X" versions which will be declared
// below with (fully optional) template arguments to avoid having to repeatedly type
// Matrix<Eigen::Dynamic, Eigen::Dynamic, T> in templated code.
#define THEA_DECL_RESIZABLE_MATRIX_TYPEDEFS(scalar, suffix)                                                                   \
    typedef Eigen::Matrix< scalar, Eigen::Dynamic, Eigen::Dynamic, DEFAULT_MATRIX_LAYOUT      >  MatrixX    ## suffix;        \
    typedef Eigen::Matrix< scalar, Eigen::Dynamic, 1             , MatrixLayout::COLUMN_MAJOR >  VectorX    ## suffix;        \
    typedef Eigen::Matrix< scalar, 1,              Eigen::Dynamic, MatrixLayout::ROW_MAJOR    >  RowVectorX ## suffix;

THEA_DECL_RESIZABLE_MATRIX_TYPEDEFS(float, f)
THEA_DECL_RESIZABLE_MATRIX_TYPEDEFS(double, d)
THEA_DECL_RESIZABLE_MATRIX_TYPEDEFS(std::complex<float>, cf)
THEA_DECL_RESIZABLE_MATRIX_TYPEDEFS(std::complex<double>, cd)
THEA_DECL_RESIZABLE_MATRIX_TYPEDEFS(int, i)

#undef THEA_DECL_RESIZABLE_MATRIX_TYPEDEFS

/**
 * General 2D dense matrix template, alias for Eigen::Matrix with a custom default layout (row or column major) and
 * alignment preference.
 *
 * This alias currently disables alignment for all fixed-size types: fixed-size vectorizable types (Vector4f, Matrix4f,
 * Vector2d etc) require special handling for every class which directly or indirectly has such a member, and could lead to
 * unexpected bugs and considerably less ease of use.
 *
 * @see
 *   - http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
 *   - https://eigen.tuxfamily.org/dox/group__TopicStlContainers.html
 *   - http://eigen.tuxfamily.org/dox/group__TopicFixedSizeVectorizable.html
 *   - https://stackoverflow.com/q/41087043
 */
template <int Rows, int Cols, typename T = Real,
          int Options = DEFAULT_MATRIX_LAYOUT,
          int MaxRowsAtCompileTime = Rows,
          int MaxColsAtCompileTime = Cols>
using Matrix = Eigen::Matrix<T, Rows, Cols,
                             Options | ((Options & Eigen::DontAlign) == 0 && (Rows == Eigen::Dynamic || Cols == Eigen::Dynamic)
                                      ? Eigen::AutoAlign : Eigen::DontAlign),
                             MaxRowsAtCompileTime, MaxColsAtCompileTime>;

/**
 * General 2D dense matrix template with dynamic resizing, alias for Eigen::Matrix with Eigen::Dynamic and a custom
 * default layout (row or column major).
 */
template <typename T = Real,
          int Options = DEFAULT_MATRIX_LAYOUT,
          int MaxRowsAtCompileTime = Eigen::Dynamic,
          int MaxColsAtCompileTime = Eigen::Dynamic>
using MatrixX = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Options, MaxRowsAtCompileTime, MaxColsAtCompileTime>;

/**
 * General 1D dense column vector template, alias for Eigen::Matrix<T, Size, 1,...>, with a custom alignment
 * preference.
 *
 * This alias currently disables alignment for all fixed-size types: fixed-size vectorizable types (Vector4f, Vector2d
 * etc) require special handling for every class which directly or indirectly has such a member, and could lead to unexpected
 * bugs and considerably less ease of use.
 *
 * @see
 *   - http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
 *   - https://eigen.tuxfamily.org/dox/group__TopicStlContainers.html
 *   - http://eigen.tuxfamily.org/dox/group__TopicFixedSizeVectorizable.html
 *   - https://stackoverflow.com/q/41087043
 */
template <int Size, typename T = Real,
          int Options = MatrixLayout::COLUMN_MAJOR,
          int MaxRowsAtCompileTime = Size>
using Vector = Eigen::Matrix<T, Size, 1,
                             Options | ((Options & Eigen::DontAlign) == 0 && Size == Eigen::Dynamic
                                      ? Eigen::AutoAlign : Eigen::DontAlign),
                             MaxRowsAtCompileTime, 1>;

/**
 * General 1D dense column vector template with dynamic resizing, alias for Eigen::Matrix<T, Eigen::Dynamic, 1,...>
 * with a custom default layout (row or column major).
 */
template <typename T = Real,
          int Options = MatrixLayout::COLUMN_MAJOR,
          int MaxRowsAtCompileTime = Eigen::Dynamic>
using VectorX = Eigen::Matrix<T, Eigen::Dynamic, 1, Options, MaxRowsAtCompileTime, 1>;

/**
 * General 1D dense row vector template, alias for Eigen::Matrix<T, 1, Size,...>, with a custom alignment
 * preference.
 *
 * This alias currently disables alignment for all fixed-size types: fixed-size vectorizable types (RowVector4f,
 * RowVector2d etc) require special handling for every class which directly or indirectly has such a member, and could lead to
 * unexpected bugs and considerably less ease of use.
 *
 * @see
 *   - http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
 *   - https://eigen.tuxfamily.org/dox/group__TopicStlContainers.html
 *   - http://eigen.tuxfamily.org/dox/group__TopicFixedSizeVectorizable.html
 *   - https://stackoverflow.com/q/41087043
 */
template <int Size, typename T = Real,
          int Options = MatrixLayout::ROW_MAJOR,
          int MaxColsAtCompileTime = Size>
using RowVector = Eigen::Matrix<T, 1, Size,
                                Options | ((Options & Eigen::DontAlign) == 0 && Size == Eigen::Dynamic
                                         ? Eigen::AutoAlign : Eigen::DontAlign),
                                1, MaxColsAtCompileTime>;

/**
 * General 1D dense row vector template with dynamic resizing, alias for Eigen::Matrix<T, 1, Eigen::Dynamic,...>
 * with a custom default layout (row or column major).
 */
template <typename T = Real,
          int Options = MatrixLayout::ROW_MAJOR,
          int MaxColsAtCompileTime = Eigen::Dynamic>
using RowVectorX = Eigen::Matrix<T, 1, Eigen::Dynamic, Options, 1, MaxColsAtCompileTime>;

//=============================================================================================================================
// Typedef Eigen::Map wrappers for interpreting raw data as common Eigen types.
//=============================================================================================================================

#define THEA_DECL_MATRIX_MAP_TYPEDEFS(suffix)                                 \
    typedef Eigen::Map< Matrix2    ## suffix >  Matrix2    ## suffix ## Map;  \
    typedef Eigen::Map< Matrix3    ## suffix >  Matrix3    ## suffix ## Map;  \
    typedef Eigen::Map< Matrix4    ## suffix >  Matrix4    ## suffix ## Map;  \
    typedef Eigen::Map< Vector2    ## suffix >  Vector2    ## suffix ## Map;  \
    typedef Eigen::Map< Vector3    ## suffix >  Vector3    ## suffix ## Map;  \
    typedef Eigen::Map< Vector4    ## suffix >  Vector4    ## suffix ## Map;  \
    typedef Eigen::Map< RowVector2 ## suffix >  RowVector2 ## suffix ## Map;  \
    typedef Eigen::Map< RowVector3 ## suffix >  RowVector3 ## suffix ## Map;  \
    typedef Eigen::Map< RowVector4 ## suffix >  RowVector4 ## suffix ## Map;  \
    typedef Eigen::Map< Matrix2X   ## suffix >  Matrix2X   ## suffix ## Map;  \
    typedef Eigen::Map< Matrix3X   ## suffix >  Matrix3X   ## suffix ## Map;  \
    typedef Eigen::Map< Matrix4X   ## suffix >  Matrix4X   ## suffix ## Map;  \
    typedef Eigen::Map< MatrixX2   ## suffix >  MatrixX2   ## suffix ## Map;  \
    typedef Eigen::Map< MatrixX3   ## suffix >  MatrixX3   ## suffix ## Map;  \
    typedef Eigen::Map< MatrixX4   ## suffix >  MatrixX4   ## suffix ## Map;  \
    \
    typedef Eigen::Map< Matrix2    ## suffix const >  Matrix2    ## suffix ## ConstMap;  \
    typedef Eigen::Map< Matrix3    ## suffix const >  Matrix3    ## suffix ## ConstMap;  \
    typedef Eigen::Map< Matrix4    ## suffix const >  Matrix4    ## suffix ## ConstMap;  \
    typedef Eigen::Map< Vector2    ## suffix const >  Vector2    ## suffix ## ConstMap;  \
    typedef Eigen::Map< Vector3    ## suffix const >  Vector3    ## suffix ## ConstMap;  \
    typedef Eigen::Map< Vector4    ## suffix const >  Vector4    ## suffix ## ConstMap;  \
    typedef Eigen::Map< RowVector2 ## suffix const >  RowVector2 ## suffix ## ConstMap;  \
    typedef Eigen::Map< RowVector3 ## suffix const >  RowVector3 ## suffix ## ConstMap;  \
    typedef Eigen::Map< RowVector4 ## suffix const >  RowVector4 ## suffix ## ConstMap;  \
    typedef Eigen::Map< Matrix2X   ## suffix const >  Matrix2X   ## suffix ## ConstMap;  \
    typedef Eigen::Map< Matrix3X   ## suffix const >  Matrix3X   ## suffix ## ConstMap;  \
    typedef Eigen::Map< Matrix4X   ## suffix const >  Matrix4X   ## suffix ## ConstMap;  \
    typedef Eigen::Map< MatrixX2   ## suffix const >  MatrixX2   ## suffix ## ConstMap;  \
    typedef Eigen::Map< MatrixX3   ## suffix const >  MatrixX3   ## suffix ## ConstMap;  \
    typedef Eigen::Map< MatrixX4   ## suffix const >  MatrixX4   ## suffix ## ConstMap;

THEA_DECL_MATRIX_MAP_TYPEDEFS()
THEA_DECL_MATRIX_MAP_TYPEDEFS(f)
THEA_DECL_MATRIX_MAP_TYPEDEFS(d)
THEA_DECL_MATRIX_MAP_TYPEDEFS(cf)
THEA_DECL_MATRIX_MAP_TYPEDEFS(cd)
THEA_DECL_MATRIX_MAP_TYPEDEFS(i)

#undef THEA_DECL_MATRIX_MAP_TYPEDEFS

#define THEA_DECL_RESIZABLE_MATRIX_MAP_TYPEDEFS(suffix)                       \
    typedef Eigen::Map< MatrixX    ## suffix >  MatrixX    ## suffix ## Map;  \
    typedef Eigen::Map< VectorX    ## suffix >  VectorX    ## suffix ## Map;  \
    typedef Eigen::Map< RowVectorX ## suffix >  RowVectorX ## suffix ## Map;  \
    \
    typedef Eigen::Map< MatrixX    ## suffix const >  MatrixX    ## suffix ## ConstMap;  \
    typedef Eigen::Map< VectorX    ## suffix const >  VectorX    ## suffix ## ConstMap;  \
    typedef Eigen::Map< RowVectorX ## suffix const >  RowVectorX ## suffix ## ConstMap;

THEA_DECL_RESIZABLE_MATRIX_MAP_TYPEDEFS(f)
THEA_DECL_RESIZABLE_MATRIX_MAP_TYPEDEFS(d)
THEA_DECL_RESIZABLE_MATRIX_MAP_TYPEDEFS(cf)
THEA_DECL_RESIZABLE_MATRIX_MAP_TYPEDEFS(cd)
THEA_DECL_RESIZABLE_MATRIX_MAP_TYPEDEFS(i)

#undef THEA_DECL_RESIZABLE_MATRIX_MAP_TYPEDEFS

/** Alias for Eigen::Map< Matrix<...> >. */
template < int Rows, int Cols, typename T = Real,
           int Options = DEFAULT_MATRIX_LAYOUT,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int MaxRowsAtCompileTime = Rows,
           int MaxColsAtCompileTime = Cols >
using MatrixMap = Eigen::Map< Matrix<Rows, Cols, T, Options, MaxRowsAtCompileTime, MaxColsAtCompileTime>,
                              MapOptions, StrideType >;

/** Alias for Eigen::Map< Matrix<...> const >. */
template < int Rows, int Cols, typename T = Real,
           int Options = DEFAULT_MATRIX_LAYOUT,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int MaxRowsAtCompileTime = Rows,
           int MaxColsAtCompileTime = Cols >
using MatrixConstMap = Eigen::Map< Matrix<Rows, Cols, T, Options, MaxRowsAtCompileTime, MaxColsAtCompileTime> const,
                                   MapOptions, StrideType >;

/** Alias for Eigen::Map< Vector<...> >. */
template < int Size, typename T = Real,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int Options = MatrixLayout::COLUMN_MAJOR,
           int MaxRowsAtCompileTime = Size >
using VectorMap = Eigen::Map< Vector<Size, T, Options, MaxRowsAtCompileTime>, MapOptions, StrideType >;

/** Alias for Eigen::Map< Vector<...> const >. */
template < int Size, typename T = Real,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int Options = MatrixLayout::COLUMN_MAJOR,
           int MaxRowsAtCompileTime = Size >
using VectorConstMap = Eigen::Map< Vector<Size, T, Options, MaxRowsAtCompileTime> const, MapOptions, StrideType >;

/** Alias for Eigen::Map< RowVector<...> >. */
template < int Size, typename T = Real,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int Options = MatrixLayout::ROW_MAJOR,
           int MaxColsAtCompileTime = Size >
using RowVectorMap = Eigen::Map< RowVector<Size, T, Options, MaxColsAtCompileTime>, MapOptions, StrideType >;

/** Alias for Eigen::Map< RowVector<...> const >. */
template < int Size, typename T = Real,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int Options = MatrixLayout::ROW_MAJOR,
           int MaxColsAtCompileTime = Size >
using RowVectorConstMap = Eigen::Map< RowVector<Size, T, Options, MaxColsAtCompileTime> const, MapOptions, StrideType >;

/** Alias for Eigen::Map< MatrixX<...> >. */
template < typename T = Real,
           int Options = DEFAULT_MATRIX_LAYOUT,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int MaxRowsAtCompileTime = Eigen::Dynamic,
           int MaxColsAtCompileTime = Eigen::Dynamic >
using MatrixXMap = Eigen::Map< MatrixX<T, Options, MaxRowsAtCompileTime, MaxColsAtCompileTime>, MapOptions, StrideType >;

/** Alias for Eigen::Map< MatrixX<...> const >. */
template < typename T = Real,
           int Options = DEFAULT_MATRIX_LAYOUT,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int MaxRowsAtCompileTime = Eigen::Dynamic,
           int MaxColsAtCompileTime = Eigen::Dynamic >
using MatrixXConstMap = Eigen::Map< MatrixX<T, Options, MaxRowsAtCompileTime, MaxColsAtCompileTime> const,
                                    MapOptions, StrideType >;

/** Alias for Eigen::Map< VectorX<...> >. */
template < typename T = Real,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int Options = MatrixLayout::COLUMN_MAJOR,
           int MaxRowsAtCompileTime = Eigen::Dynamic >
using VectorXMap = Eigen::Map< VectorX<T, Options, MaxRowsAtCompileTime>, MapOptions, StrideType >;

/** Alias for Eigen::Map< VectorX<...> const >. */
template < typename T = Real,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int Options = MatrixLayout::COLUMN_MAJOR,
           int MaxRowsAtCompileTime = Eigen::Dynamic >
using VectorXConstMap = Eigen::Map< VectorX<T, Options, MaxRowsAtCompileTime> const, MapOptions, StrideType >;

/** Alias for Eigen::Map< RowVectorX<...> >. */
template < typename T = Real,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int Options = MatrixLayout::ROW_MAJOR,
           int MaxColsAtCompileTime = Eigen::Dynamic >
using RowVectorXMap = Eigen::Map< RowVectorX<T, Options, MaxColsAtCompileTime>, MapOptions, StrideType >;

/** Alias for Eigen::Map< RowVectorX<...> const >. */
template < typename T = Real,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int Options = MatrixLayout::ROW_MAJOR,
           int MaxColsAtCompileTime = Eigen::Dynamic >
using RowVectorXConstMap = Eigen::Map< RowVectorX<T, Options, MaxColsAtCompileTime> const, MapOptions, StrideType >;

//=============================================================================================================================
// Typedef other useful Eigen classes.
//=============================================================================================================================

/** Alias for Eigen::Quaternion<...>. */
template <typename T = Real> using Quaternion = Eigen::Quaternion<T>;

} // namespace Thea

#include "MatrixUtil.hpp"

#endif


/*
 * Copyright (c) 2015, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials provided
 *       with the distribution.
 *
 *    3. Neither the name of the copyright holder nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 *          Erik Nelson            ( eanelson@eecs.berkeley.edu )
 */

///////////////////////////////////////////////////////////////////////////////
//
// This header defines a set of functions for converting a fundamental matrix
// and a pair of camera intrinsics into an essential matrix, and from an
// essential matrix to and a set of camera extrinsics.
//
///////////////////////////////////////////////////////////////////////////////

#include "essential_matrix_solver.h"

#include <Eigen/Core>
#include <Eigen/LU>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <vector>

#include "point_3d.h"
#include "triangulation.h"
#include "../camera/camera.h"

DEFINE_double(min_points_visible_ratio, 0.3,
              "Fraction of keypoint matches whose triangulation must be "
              "visible from both cameras. This value should be lowered if you "
              "expect lots of noisy matches.");

namespace bsfm {

// Compute the essential matrix from a fundamental matrix and camera intrinsics.
Matrix3d EssentialMatrixSolver::ComputeEssentialMatrix(
    const Matrix3d& F, const CameraIntrinsics& intrinsics1,
    const CameraIntrinsics& intrinsics2) {
  // Extract intrinsics matrices.
  Matrix3d K1(intrinsics1.K());
  Matrix3d K2(intrinsics2.K());

  // Calculate the essential matrix.
  return K2.transpose() * F * K1;
}

// Compute the relative transformation between two cameras from an essential
// matrix and a list of keypoint matches. Note that translation can only be
// computed up to a scale factor.
// NOTE: this implementation is based on Hartley & Zisserman's MVG, pg. 258.
bool EssentialMatrixSolver::ComputeExtrinsics(
    const Matrix3d& E, const FeatureMatchList& matches,
    const CameraIntrinsics& intrinsics1, const CameraIntrinsics& intrinsics2,
    Pose& relative_pose) {
  // Initialize the W matrix.
  Matrix3d W;
  W << 0.0, -1.0, 0.0,
       1.0,  0.0, 0.0,
       0.0,  0.0, 1.0;

  // Perform an SVD on the essential matrix.
  Eigen::JacobiSVD<Matrix3d> svd;
  svd.compute(E, Eigen::ComputeFullU | Eigen::ComputeFullV);
  if (!svd.computeU() || !svd.computeV()) {
    VLOG(1) << "Failed to compute a singular value decomposition of "
            << "the essential matrix.";
    return false;
  }

  // The essential matrix must satisfy E = U * diag(1, 1, 0) * V^T. Use U and V
  // to get a new E, run another SVD, and get the normalized U and V.
  Matrix3d sigma(Matrix3d::Identity());
  sigma(2, 2) = 0;
  Matrix3d E_augmented = svd.matrixU() * sigma * svd.matrixV().transpose();

  svd.compute(E_augmented, Eigen::ComputeFullU | Eigen::ComputeFullV);
  if (!svd.computeU() || !svd.computeV()) {
    VLOG(1) << "Failed to compute a singular value decomposition of "
            << "the essential matrix.";
    return false;
  }

  // Compute two possibilities for rotation.
  Matrix3d R1 = svd.matrixU() * W * svd.matrixV().transpose();
  Matrix3d R2 = svd.matrixU() * W.transpose() * svd.matrixV().transpose();

  // Ensure positive determinants.
  if (R1.determinant() < 0)
    R1 = -R1;
  if (R2.determinant() < 0)
    R2 = -R2;

  // Compute two possibilities for translation
  Vector3d t1, t2;
  t1 = svd.matrixU().col(2);
  t2 = -t1;

  // Build four possible Poses.
  std::vector<Pose> poses;
  poses.push_back(Pose(R1, t1));
  poses.push_back(Pose(R1, t2));
  poses.push_back(Pose(R2, t1));
  poses.push_back(Pose(R2, t2));

  // Set the first camera's position to identity in rotation and translation.
  CameraExtrinsics extrinsics1;
  extrinsics1.SetWorldToCamera(Pose());

  Camera camera1;
  camera1.SetExtrinsics(extrinsics1);
  camera1.SetIntrinsics(intrinsics1);

  // Test how many points are in front of each pose and the identity pose.
  Pose best_pose;
  int best_num_points = -1;

  double u = 0.0, v = 0.0;
  for (int ii = 0; ii < poses.size(); ii++) {
    int num_points = 0;

    CameraExtrinsics extrinsics2;
    extrinsics2.SetWorldToCamera(poses[ii]);

    Camera camera2;
    camera2.SetExtrinsics(extrinsics2);
    camera2.SetIntrinsics(intrinsics2);

    for (int jj = 0; jj < matches.size(); jj++) {
      // Triangulate points and test if the 3D estimate is in front of both
      // cameras.
      Point3D point;
      double unused = 0.0;
      if (!Triangulate(matches[jj], camera1, camera2, point, unused)) {
        continue;
      }

      num_points++;
    }

    // Update best_num_points and best_pose.
    if (num_points > best_num_points) {
      best_num_points = num_points;
      best_pose = poses[ii];
    }
  }

  // Return with false if not enough points found in front of the cameras.
  if (best_num_points < FLAGS_min_points_visible_ratio * matches.size()) {
    VLOG(1) << "Did not find enough points in front of both cameras.";
    return false;
  }

  relative_pose = best_pose;

  return true;
}

}  //\namespace bsfm


/*
 *  mesher_tester.cpp
 *  Mesh_3_applications
 *
 *  Created by Jane Tournois on 02/12/09.
 *  Copyright 2009 INRIA. All rights reserved.
 */

//#include <debug.h>
#include <CGAL/AABB_intersections.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Mesh_triangulation_3.h>
#include <CGAL/Mesh_complex_3_in_triangulation_3.h>
#include <CGAL/Mesh_criteria_3.h>
// Implicit domain
#include <CGAL/Implicit_to_labeling_function_wrapper.h>
#include <CGAL/Labeled_mesh_domain_3.h>

#include <CGAL/make_mesh_3.h>
#include "../examples/Mesh_3/implicit_functions.h"
#include <CGAL/refine_mesh_3.h>
#include <CGAL/Mesh_3/Mesh_global_optimizer.h>

/* INPUTS */
// Polyhedral domain
#include <CGAL/Mesh_3/Robust_intersection_traits_3.h>
#include <CGAL/Polyhedral_mesh_domain_3.h>
#include <CGAL/IO/Polyhedron_iostream.h>
// Implicit domain is above
// 3D Image
#include <CGAL/Labeled_image_mesh_domain_3.h>
#include <CGAL/make_mesh_3.h>
#include <CGAL/Image_3.h>

/* OUTPUT */
#include <CGAL/IO/File_medit.h>

/* tools */
#include <sstream>
#include <stdlib.h>
#include <algorithm>

/* OPTIONS and PARAMETERS */
#include <boost/program_options.hpp>
namespace po = boost::program_options;

using namespace CGAL::parameters; //to avoid verbose function and named parameters call

/* FILE SYSTEM */
#include <iostream>
#include <boost/filesystem.hpp>
#include <boost/filesystem/path.hpp>
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/convenience.hpp>
namespace fs = boost::filesystem;

/* DOMAIN */
struct K: public CGAL::Exact_predicates_inexact_constructions_kernel {};
// Polyhedral domain
typedef CGAL::Mesh_3::Robust_intersection_traits_3<K> Geom_traits; // exact constructions here
typedef CGAL::Polyhedron_3<Geom_traits> Polyhedron;
typedef CGAL::Polyhedral_mesh_domain_3<Polyhedron, Geom_traits> Polyhedral_domain;
// Implicit domain
typedef FT_to_point_function_wrapper<K::FT, K::Point_3> I_Function;
typedef CGAL::Implicit_multi_domain_to_labeling_function_wrapper<I_Function> I_Function_wrapper;
typedef I_Function_wrapper::Function_vector I_Function_vector;
typedef CGAL::Labeled_mesh_domain_3<I_Function_wrapper, K> Implicit_domain;
// 3D Image
typedef CGAL::Image_3 Image;
typedef CGAL::Labeled_image_mesh_domain_3<Image,K> Image_domain;

/* TRIANGULATION */
//Polyhedral domain
typedef CGAL::Mesh_triangulation_3<Polyhedral_domain>::type Tr_polyhedron;
typedef CGAL::Mesh_complex_3_in_triangulation_3<Tr_polyhedron> C3t3_polyhedron;
//Implicit domain
typedef CGAL::Mesh_triangulation_3<Implicit_domain>::type Tr_implicit;
typedef CGAL::Mesh_complex_3_in_triangulation_3<Tr_implicit> C3t3_implicit;
//3D Image
typedef CGAL::Mesh_triangulation_3<Image_domain>::type Tr_image;
typedef CGAL::Mesh_complex_3_in_triangulation_3<Tr_image> C3t3_image;

/* MESHING CRITERIA */
//Polyhedral domain
typedef CGAL::Mesh_criteria_3<Tr_polyhedron>       Mesh_criteria_polyhedron;
//typedef Mesh_criteria_polyhedron::Facet_criteria   Facet_criteria_polyhedron;
//typedef Mesh_criteria_polyhedron::Cell_criteria    Cell_criteria_polyhedron;
//Implicit domain
typedef CGAL::Mesh_criteria_3<Tr_implicit>         Mesh_criteria_implicit;
//typedef Mesh_criteria_implicit::Facet_criteria     Facet_criteria_implicit;
//typedef Mesh_criteria_implicit::Cell_criteria      Cell_criteria_implicit;
//3D Image
typedef CGAL::Mesh_criteria_3<Tr_image>            Mesh_criteria_image;
//typedef Mesh_criteria_image::Facet_criteria        Facet_criteria_image;
//typedef Mesh_criteria_image::Cell_criteria         Cell_criteria_image;

template <typename T>
T set_arg(const std::string& param_name,
		  const std::string& param_string,
		  const po::variables_map& vm)
{
	if(vm.count(param_name))
	{
		T param_value = vm[param_name].as<T>();
		//std::cout << param_string << ": " << param_value << "\n";
		return param_value;
	}
	else
	{
		//std::cout << param_string << " ignored.\n";
		return T();
	}
}

void set_implicit_function(I_Function_vector& v,
						   I_Function& f,
						   const std::string& function_name,
						   const po::variables_map& vm)
{
	if(vm.count(function_name))
	{
		v.push_back(f);
		std::cout << function_name << " ";
	}
}

std::vector<std::string> split_line(const std::string& str)
{
	std::vector<std::string> args;

	std::string::size_type lastPos = str.find_first_not_of(" ", 0);
	std::string::size_type pos = str.find_first_of(" ", lastPos);
	while(pos != std::string::npos || lastPos != std::string::npos)
	{
		args.push_back(str.substr(lastPos, pos-lastPos));
		lastPos = str.find_first_not_of(" ", pos);
		pos = str.find_first_of(" ", lastPos);
	}
	return args;
}

template<typename C3t3>
void save_histogram(std::string& filename,
					const C3t3& c3t3)
{
	std::vector<int> histo(181,0);

	for (typename C3t3::Cell_iterator cit = c3t3.cells_begin() ;
		 cit != c3t3.cells_end() ;
		 ++cit)
	{
		if( !c3t3.is_in_complex(cit))
			continue;

		typedef typename K::Point_3 Point_3;
		const Point_3& p0 = cit->vertex(0)->point();
		const Point_3& p1 = cit->vertex(1)->point();
		const Point_3& p2 = cit->vertex(2)->point();
		const Point_3& p3 = cit->vertex(3)->point();

		double a = CGAL::to_double(CGAL::abs(CGAL::Mesh_3::dihedral_angle(p0,p1,p2,p3)));
		histo[std::floor(a)] += 1;
		a = CGAL::to_double(CGAL::abs(CGAL::Mesh_3::dihedral_angle(p0, p2, p1, p3)));
		histo[std::floor(a)] += 1;
		a = CGAL::to_double(CGAL::abs(CGAL::Mesh_3::dihedral_angle(p0, p3, p1, p2)));
		histo[std::floor(a)] += 1;
		a = CGAL::to_double(CGAL::abs(CGAL::Mesh_3::dihedral_angle(p1, p2, p0, p3)));
		histo[std::floor(a)] += 1;
		a = CGAL::to_double(CGAL::abs(CGAL::Mesh_3::dihedral_angle(p1, p3, p0, p2)));
		histo[std::floor(a)] += 1;
		a = CGAL::to_double(CGAL::abs(CGAL::Mesh_3::dihedral_angle(p2, p3, p0, p1)));
		histo[std::floor(a)] += 1;
	}
	std::ofstream file(filename.c_str());
	std::copy(histo.begin(), histo.end(), std::ostream_iterator<int>(file, "\n"));
}

template<typename MeshingCriteria>
MeshingCriteria get_parameters(const std::string& param_line,
							   po::variables_map& vm)
{
	po::options_description mesh("Mesh generation parameters");
	mesh.add_options()
  ("mesh", "Generate mesh")
	("facet_angle", po::value<double>(), "Set facet angle bound")
	("facet_size", po::value<double>(), "Set facet size bound")
	("facet_error", po::value<double>(), "Set approximation error bound")
	("tet_shape", po::value<double>(), "Set tet radius-edge bound")
	("tet_size", po::value<double>(), "Set tet size bound");

	po::options_description implicit_functions("Implicit functions");
	implicit_functions.add_options()
	("torus", "Mesh torus function")
	("sphere", "Mesh sphere function")
	("chair", "Mesh chair function")
	("tanglecube", "Mesh tanglecube function")
	("cube", "Mesh cube function")
	("ellipsoid", "Mesh ellipsoid function")
	("heart", "Mesh heart function")
	("octic", "Mesh octic function");

	po::options_description optim("Optimization parameters");
	optim.add_options()
	("exude", "Exude mesh after refinement")
	("perturb", po::value<double>(), "Perturb mesh after refinement (sliver removal)")
	("lloyd", po::value<int>(), "Lloyd smoothing after refinement. arg is max nb iterations")
	("odt", po::value<int>(), "ODT smoothing after refinement. arg is max nb iterations");

	po::options_description additional_options("Options");
	additional_options.add_options()
	("off_vertices", po::value<int>(), "Use polyhedron vertices as initialization step")
	("no_label_rebind", "Don't rebind cell labels in medit output")
	("show_patches", "Show surface patches in medit output");

	po::options_description cmdline_options("Usage", 1);
	cmdline_options.add(mesh).add(implicit_functions).add(optim).add(additional_options);

	std::vector<std::string> args = split_line(param_line);
	po::store(po::command_line_parser(args).options(cmdline_options).run(), vm);
	po::notify(vm);

	double facet_angle = set_arg<double>("facet_angle", "Facet angle", vm);
	double facet_size  = set_arg<double>("facet_size", "Facet size", vm);
	double facet_error = set_arg<double>("facet_error", "Facet approximation error", vm);
	double tet_shape = set_arg<double>("tet_shape","Tet shape (radius-edge)", vm);
	double tet_size = set_arg<double>("tet_size","Tet size", vm);

	typename MeshingCriteria::Facet_criteria fc(facet_angle, facet_size, facet_error);
	typename MeshingCriteria::Cell_criteria cc(tet_shape, tet_size);
	return MeshingCriteria(fc, cc);
}

void get_implicit_function(const po::variables_map& vm,
						   I_Function_vector& fv)
{
	// Define functions
	I_Function f1( &torus_function);
	I_Function f2( &sphere_function<3>);
	I_Function f3( &chair_function);
	I_Function f4( &tanglecube_function);
	I_Function f5( &cube_function);
	I_Function f6( &ellipsoid_function);
	I_Function f7( &heart_function);
	I_Function f8( &octic_function);

	std::cout << "Function(s): ";
	set_implicit_function(fv, f1, "torus", vm);
	set_implicit_function(fv, f2, "sphere", vm);
	set_implicit_function(fv, f3, "chair", vm);
	set_implicit_function(fv, f4, "tanglecube", vm);
	set_implicit_function(fv, f5, "cube", vm);
	set_implicit_function(fv, f6, "ellipsoid", vm);
	set_implicit_function(fv, f7, "heart", vm);
	set_implicit_function(fv, f8, "octic", vm);
	std::cout << "\n\n";

	if(fv.empty())
		std::cout << "Warning: No implicit function set.\n";
}

template <class Domain>
class Domain_builder
{
public:
  /*void build(const std::string& str);
  Domain domain();*/
};

template <>
class Domain_builder<Polyhedral_domain>
{
  typedef Polyhedral_domain Domain;
public:
  Domain_builder(const std::string& str)
  : domain_(NULL)
  {
    std::ifstream input(str.c_str());
    input >> polyhedron_;
    domain_ = new Domain(polyhedron_);
  }

  ~Domain_builder() { delete domain_; }

  Domain& domain() { return *domain_; }

private:
  Domain* domain_;
  Polyhedron polyhedron_;
};

template <>
class Domain_builder<Image_domain>
{
  typedef Image_domain Domain;
public:
  Domain_builder(const std::string& str)
  : domain_(NULL)
  {
    image_.read(str.c_str());
    domain_ = new Domain(image_, 1e-6);
  }

  ~Domain_builder() { delete domain_; }

  Domain& domain() { return *domain_; }

private:
  Domain* domain_;
  Image image_;
};

//template <>
//class Domain_builder<Implicit_domain>
//{
//  typedef Implicit_domain Domain;
//public:
//  Domain_builder()
//  :	f1_( &torus_function);
//	, f2_( &sphere_function<3>);
//	, f3_( &chair_function);
//	, f4_( &tanglecube_function);
//	, f5_( &cube_function);
//  , f6_( &ellipsoid_function);
//	, f7_( &heart_function);
//	, f8_( &octic_function);
//
//  void build(const std::string& str)
//  {
//    Image image;
//    image.read(str.c_str());
//    delete domain_;
//    domain_ = new Domain(image, 1e-6);
//  }
//
//  Domain& domain() { return *domain_; }
//
//private:
//  Domain* domain_;
//
//  // Define functions
//	I_Function f1_;
//	I_Function f2_;
//	I_Function f3_;
//	I_Function f4_;
//	I_Function f5_;
//	I_Function f6_;
//	I_Function f7_;
//	I_Function f8_;
//};

template <class C3T3, class Mesh_criteria, class Domain>
void mesh(const std::string& data, const po::variables_map& vm)
{
  if(!fs::is_directory(data))
    std::cout << "!! Problem while reading " << data << "\n";

  std::string output_dir;
  if(vm.count("outdir"))
    output_dir = vm["outdir"].as<std::string>();
  else output_dir = data + "output";
  if(!fs::is_directory(output_dir) && !fs::create_directory(output_dir))
    std::cout << "!! Problem while creating " << output_dir << "\n";

  fs::path path(data);
  for(fs::directory_iterator it(path); it != fs::directory_iterator(); ++it)
  {
    if(fs::is_directory(*it)
       || (fs::extension(*it) != ".off" && (fs::extension(*it) != ".inr" && ( fs::extension(fs::basename(*it)) != ".inr" || fs::extension(*it) != ".gz"))) )
      continue;

    std::string line_param;
    std::string filename(fs::basename(*it));
    if ( fs::extension(*it) == ".gz" )
      filename = fs::basename(fs::basename(*it));
    std::string filename_param(data + filename + ".txt");

    std::ifstream file_param(filename_param.data()); //parameters
    if(!file_param)
    {
      std::cout << "Could not read parameters in : '" << filename_param << "'. Next file." << std::endl;
      continue;
    }
    unsigned int i = 1;

    // Timer
    CGAL::Timer timer;
    timer.start();

    // we keep c3t3 between lines
    C3T3 c3t3_save;

    //Load the domain
    std::cout << "****** [" << filename << "] Create domain...";
    std::flush(std::cout);
    Domain_builder<Domain> domain_builder(it->path().string());
    std::cout <<"done (" << timer.time() << "s) ******\n\n";

    while(std::getline(file_param,line_param))
    {
      std::cout << "*** Meshing " << filename << "[" << i << "] with : " << line_param << std::endl;

      po::variables_map vm_p;
      Mesh_criteria mcp = get_parameters<Mesh_criteria>(line_param, vm_p);

      //Mesh generation (reload domain and rebuild c3t3)
      if ( vm_p.count("mesh") )
      {
        timer.stop();
        timer.reset();
        timer.start();
        std::cout << "  Generate mesh...";
        std::flush(std::cout);
        c3t3_save = CGAL::make_mesh_3<C3T3>(domain_builder.domain(), mcp, no_exude(), no_perturb());
        std::cout << "done (" << timer.time() << "s - "
                  << c3t3_save.triangulation().number_of_vertices() << " vertices, "
                  << c3t3_save.number_of_cells() << " cells)\n";
      }

      C3T3 c3t3 = c3t3_save;

      //Optimization
      timer.stop();
      timer.reset();
      timer.start();
      if(vm_p.count("lloyd"))
      {
        std::cout << "  Lloyd optimization...";
        std::flush(std::cout);
        CGAL::lloyd_optimize_mesh_3(c3t3, domain_builder.domain(), max_iteration_number=vm_p["lloyd"].as<int>());
        std::cout << "done (" << timer.time() << "s)\n";
      }
      timer.stop();
      timer.reset();
      timer.start();
      if(vm_p.count("odt"))
      {
        std::cout << "  Odt optimization...";
        std::flush(std::cout);
        CGAL::odt_optimize_mesh_3(c3t3, domain_builder.domain(), max_iteration_number=vm_p["odt"].as<int>());
        std::cout << "done (" << timer.time() << "s)\n";
      }
      timer.stop();
      timer.reset();
      timer.start();
      if(vm_p.count("perturb"))
      {
        std::cout << "  Perturbation...";
        std::flush(std::cout);
        CGAL::perturb_mesh_3(c3t3, domain_builder.domain(), time_limit=vm_p["perturb"].as<double>());
        std::cout << "done (" << timer.time() << "s)\n";
      }
      timer.stop();
      timer.reset();
      timer.start();
      if(vm_p.count("exude"))
      {
        std::cout << "  Exudation...";
        std::flush(std::cout);
        CGAL::exude_mesh_3(c3t3);
        std::cout << "done (" << timer.time() << "s)\n";
      }
      timer.stop();
      timer.reset();
      timer.start();

      //save mesh
      std::cout << "  Save mesh...";
      std::stringstream ssout;
      ssout << i;
      std::string output_filename = output_dir +"/" + filename + "-out-" + ssout.str().c_str() + ".mesh";
      std::ofstream medit_file(output_filename.c_str());
      c3t3.output_to_medit(medit_file, !vm_p.count("no_label_rebind"), vm_p.count("show_patches"));

      //save histogram
      std::cout << "done. \n  Save histogram...";
      std::string histo_filename = output_dir +"/" + filename + "-histo-" + ssout.str().c_str() + ".txt";
      save_histogram<C3T3>(histo_filename, c3t3);
      i++;
      std::cout << "done.\n\n\n";
    }
  }
}

int main(int argc, char** argv)
{
	// options
	po::options_description generic("Options");
	generic.add_options()("help", "Produce help message")
	("polyhedron", "Test the polyhedral domain mesher")
	("implicit",   "Test the implicit domain mesher")
	("image",      "Test the 3D image domain mesher")
	("outdir", po::value<std::string>(), "Output directory. arg is location");

	po::options_description cmdline_options("Usage", 1);
	cmdline_options.add(generic);//.add(others);

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, cmdline_options), vm);
	po::notify(vm);

	if(vm.count("help") || argc < 2)
	{
		std::cout << cmdline_options << std::endl;
		std::cout << "* Polyhedra: .off files should be in applications/data/Polyhedra\n";
		std::cout << "* Images:    .inr.gz files should be in applications/data/3D_images\n";
		std::cout << "  Note: for each file toto.domain, add a toto.txt file with meshing parameters\n\n";
		return 1;
	}

	//what are we testing
	bool mesh_polyhedra = vm.count("polyhedron");
	bool mesh_implicit = vm.count("implicit");
	bool mesh_images = vm.count("image");

	// iterate on data files
	std::string data_dir("data"); // or a user defined path...
	fs::path data_path(data_dir);

	if(mesh_polyhedra)
	{
		std::string data_poly = data_dir + "/Polyhedra/";
    mesh<C3t3_polyhedron,Mesh_criteria_polyhedron,Polyhedral_domain>(data_poly,vm);
	}

	if(mesh_images)
	{
		std::string data_img = data_dir + "/3D_images/";
    mesh<C3t3_image,Mesh_criteria_image,Image_domain>(data_img,vm);
  }

	if(mesh_implicit)
	{
		std::string data_imp = data_dir + "/Implicit/";
		//mesh<C3t3_implicit,Mesh_criteria_implicit>(data_imp,vm);
	}
	return 0;
}


// -*-coding: mule-utf-8-unix; fill-column: 58; -*-

/**
 * @file
 * A retarget algo abstraction
 *
 * @author Sergei Lodyagin <serg@kogorta.dp.ua>
 */

#include <algorithm>
#include <iterator>
#include <numeric>
#include <boost/thread.hpp>
#include "types/fixed.h"
#include "types/abstract_iterator.h"
#include "algos/retarget.h"
#include "log.h"
#include "util.h"
#include "pars.h"
#include "btc_time.h"
#include "checkpoints.h"
#include "main.h"
#include "types.h"

using namespace types;

extern std::map<uint256, CBlockIndex*> mapBlockIndex;
extern CBlockIndex* pindexGenesisBlock;

namespace retarget {

template<class Algo>
class difficulty_impl;

//! Allows change difficulty only by 2 or 1/2
template<>
class difficulty_impl<pars::twice_and_half>
  : public difficulty
{
public:
  compact_bignum_t next_block_difficulty(
    const duration desired_timespan,
    const iterator& rbegin,
    const iterator& rend
  ) override
  {
    if (rbegin == rend)
      return min_difficulty_by_design;

    assert(height_diff(rbegin, rend) > 0);

    auto prev = rbegin;
    ++prev;
    const auto actual_timespan =
      rbegin->time - prev->time;
    return (
      (actual_timespan < desired_timespan)
        ? rbegin->difficulty >> 1
        : rbegin->difficulty << 1
    ).GetCompact();
  }

  compact_bignum_t dos_min_difficulty(
    compact_bignum_t last_reliable_block_difficulty,
    coin::times::block::clock::duration past
  ) const override
  {
    return min_difficulty_by_design;
  }
};

template<>
class difficulty_impl<pars::digishield>
  : public difficulty
{
public:
  //! The limit parameter for dos_min_difficulty()
  const percent_t adjustment_by_design = 110.0_pct;

  compact_bignum_t next_block_difficulty(
    const duration desired_timespan,
    const iterator& rbegin,
    const iterator& rend
  ) override
  {
    if (rbegin == rend)
      return min_difficulty_by_design;

    auto prev = rbegin;
    ++prev;
    auto nActualTimespan = rbegin->time - prev->time;

    LOG() << "  nActualTimespan = "
          << nActualTimespan
          << "  before bounds\n";

    // thanks to RealSolid & WDC for this code

    if ( pars::digishield::limit_steps() ) //switch off for development purposes
    {
      //Amplitude Filter by daft27
      nActualTimespan = desired_timespan
        + (nActualTimespan - desired_timespan)/8;

      //Guts of DigiShield Retarget
      if (nActualTimespan < desired_timespan * 3/4)
        nActualTimespan = desired_timespan * 3/4;

      if (nActualTimespan > desired_timespan * 3/2)
        nActualTimespan = desired_timespan * 3/2;
    }

    if ( coin::times::block::zero_duration == nActualTimespan )
    {
        nActualTimespan = coin::times::block::seconds(1);
    }

    // Retarget
    const auto last_block_difficulty =
      rbegin->difficulty;
    CBigNum bnNew = last_block_difficulty;
    bnNew *= to_fixed(nActualTimespan);
    bnNew /= to_fixed(desired_timespan);

    if (bnNew > min_difficulty_by_design)
      bnNew = min_difficulty_by_design;

    LOG()
      << "GetNextWorkRequired [DigiShield] RETARGET \n"
      << "retargetTimespan = " << desired_timespan
      << " nActualTimespan = " << nActualTimespan
      << "\nBefore: " << last_block_difficulty << ' '
      << CBigNum(last_block_difficulty).getuint256()
      << "\nAfter: " << bnNew.GetCompact() << ' '
      << bnNew.getuint256() << '\n';

    return bnNew.GetCompact();
  }

  compact_bignum_t dos_min_difficulty(
    compact_bignum_t last_reliable_block_difficulty,
    coin::times::block::clock::duration past
  ) const override
  {
    if (past < coin::times::block::zero_duration)
    throw types::exception<coin::except::invalid_timestamp>(
        "block with timestamp before last checkpoint"
        );

    return std::min(
      CBigNum(last_reliable_block_difficulty)
      * past / block_period_by_design
      * adjustment_by_design,
      CBigNum(min_difficulty_by_design)
      ).GetCompact();
  }
};

template<>
class difficulty_impl<pars::kgw> : public difficulty
{
public:
  const int past_blocks_min =
    pars::kgw::past_min() / block_period_by_design;
  const int past_blocks_max =
    pars::kgw::past_max() / block_period_by_design;

  compact_bignum_t next_block_difficulty(
    const duration desired_timespan,
    const iterator& rbegin,
    const iterator& rend
  ) override
  {
    using namespace coin::times::block;

    // TODO don't use floating point
    using float_duration = std::chrono::duration<double>;

    // early blocks rule
    if (height_diff(rbegin, rend) < past_blocks_min)
      return min_difficulty_by_design;

    iterator start = rbegin;
    std::advance(start, past_blocks_min - 1);

    int64_t mass = past_blocks_min - 1;
    const auto breaking_el = std::find_if(
      start,
      rend,
      [
        this,
        desired_timespan,
        &rbegin,
        &mass
      ]
        (const block::info_type& bi)
      {
        // != 0: fixes the same-time blocks retargetting
        // exploit
        constexpr auto min_duration = seconds(1);

        if (++mass > past_blocks_max)
          return true;

        const clock::duration desired_passed =
          desired_timespan * mass;
        const clock::duration actual_passed =
          // the timewrap fix (part 1)
          (rbegin->time <= bi.time)
            ? min_duration
            : rbegin->time - bi.time;

        assert(actual_passed != zero_duration);
        const double PastRateAdjustmentRatio =
          (desired_passed != zero_duration)
        ?
          std::chrono::duration_cast<float_duration>(
            desired_passed
          )
          / std::chrono::duration_cast<float_duration>(
              actual_passed
          )
        :
          1.0;

        // TODO tabulate it as fixed point values
        const double EventHorizonDeviation =
          1 + (0.7084
            * pow((double(mass)/double(past_blocks_min)),
                  -1.228));

        const double EventHorizonDeviationFast =
          EventHorizonDeviation;
        const double EventHorizonDeviationSlow =
          1.0 / EventHorizonDeviation;

        return PastRateAdjustmentRatio <=
          EventHorizonDeviationSlow
          || PastRateAdjustmentRatio >=
          EventHorizonDeviationFast;
      }
    );

    const auto distance = height_diff(rbegin, breaking_el);
    assert(distance >= 0);

    const CBigNum PastDifficultyAverage =
      std::accumulate(
        rbegin,
        breaking_el,
        CBigNum(0),
        [](const CBigNum& acc, const block::info_type& bi)
        {
          return acc + bi.difficulty;
        }
      ) / (distance + 1);

    // != 0: fixes the same-time blocks retargetting
    // exploit
    constexpr auto min_duration = seconds(1);

    const auto range_desired_timespan =
      desired_timespan * (distance + 1);
    const auto range_actual_timespan =
      // the timewrap fix (part 2)
      (rbegin->time <= breaking_el->time)
      ? min_duration
      : rbegin->time - breaking_el->time;

    assert(range_actual_timespan != zero_duration);
    CBigNum bnNew(PastDifficultyAverage);
    if (range_desired_timespan != zero_duration)
    {
      // actual time passed
      bnNew *= to_fixed(range_actual_timespan);
      // desired
      bnNew /= to_fixed(range_desired_timespan);
    }
    if (bnNew > min_difficulty_by_design)
      bnNew = min_difficulty_by_design;

    LOG()
      << "GetNextWorkRequired [KGW] RETARGET ["
      << height_diff(rbegin, breaking_el)
      << " last blocks analysed]"
      << "\navg difficulty: "
      << PastDifficultyAverage.GetCompact()
      << "\ndesired timespan for the range = "
      << range_desired_timespan
      << "\nactual timespan for the range = "
      << range_actual_timespan
      << "\nBefore: "
      << rbegin->difficulty << ' '
      << CBigNum(rbegin->difficulty)
           . getuint256()
      << "\nAfter: " << bnNew.GetCompact() << ' '
      << bnNew.getuint256() << std::endl;

    return bnNew.GetCompact();
  }

  compact_bignum_t dos_min_difficulty(
    compact_bignum_t last_reliable_block_difficulty,
    coin::times::block::clock::duration past
  ) const override
  {
    return min_difficulty_by_design;
  }
};

difficulty& difficulty::instance()
{
  static boost::once_flag of = BOOST_ONCE_INIT;
  static difficulty* instance = nullptr;
  boost::call_once([]()
  {
    instance = new difficulty_impl
      <pars::retarget_algorithm>();
  }, of);

  assert(instance);
  return *instance;
}

compact_bignum_t difficulty
::next_block_difficulty(const CBlockIndex* pindexLast)
{
  using namespace types;

  // Genesis block
  if (!pindexLast)
    return min_difficulty_by_design;

  // Limit adjustment step
  const auto pindexFirst = pindexLast->pprev;
  if (!pindexFirst)
    return pindexLast->nBits; // slod

  return next_block_difficulty(
    block_period_by_design,
    block::const_iterator<CBlockIndex>(pindexLast),
    block::const_iterator<CBlockIndex>(pindexGenesisBlock)
  );
}

difficulty::difficulty()
  : min_difficulty_by_design(
      pars::testnet_switch(pars::min_difficulty_by_design)
    ),
    block_period_by_design(
      pars::testnet_switch(pars::block_period_by_design)
    )
{
}

const CBlockIndex* difficulty
::dos_last_reliable_block()
{
  static const CBlockIndex* last_checkpointed_block =
    Checkpoints::GetLastCheckpoint(mapBlockIndex);

  if (last_checkpointed_block)
    return last_checkpointed_block;
  else
  {
    assert(pindexGenesisBlock);
    return pindexGenesisBlock;
  }
}

} // retarget


// BLAS level 3 -- complex numbers

//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <iostream>
#include <complex>
#include <boost/numeric/bindings/blas/level3.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/conj.hpp>
#ifdef F_USE_STD_VECTOR
#include <vector>
#include <boost/numeric/bindings/std/vector.hpp>
#endif
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace blas = boost::numeric::bindings::blas;
namespace bindings = boost::numeric::bindings;

using std::cout;
using std::endl;

typedef double real_t;
typedef std::complex<real_t> cmplx_t;

#ifndef F_USE_STD_VECTOR
typedef ublas::matrix<cmplx_t, ublas::row_major> m_t;
#else
typedef ublas::matrix<cmplx_t, ublas::column_major, std::vector<cmplx_t> > m_t;
#endif

int main() {

  cout << endl;

  m_t a (2, 2);
  a (0, 0) = cmplx_t (1., 0.);
  a (0, 1) = cmplx_t (2., 0.);
  a (1, 0) = cmplx_t (3., 0.);
  a (1, 1) = cmplx_t (4., 0.);
  print_m (a, "A");
  cout << endl;

  m_t b (2, 3);
  b (0, 0) = cmplx_t (1., 0.);
  b (0, 1) = cmplx_t (2., 0.);
  b (0, 2) = cmplx_t (3., 0.);
  b (1, 0) = cmplx_t (1., 0.);
  b (1, 1) = cmplx_t (2., 0.);
  b (1, 2) = cmplx_t (3., 0.);
  print_m (b, "B");
  cout << endl;

  m_t c (2, 3);

  // c = a b
  blas::gemm ( 1.0, a, b, 0.0, c);
  print_m (c, "A B");
  cout << endl;

  a (0, 0) = cmplx_t (0., 1.);
  a (0, 1) = cmplx_t (0., 2.);
  a (1, 0) = cmplx_t (0., 3.);
  a (1, 1) = cmplx_t (0., 4.);
  print_m (a, "A");
  cout << endl;

  // c = a b
  blas::gemm (1.0, a, b, 0.0, c);
  print_m (c, "A B");
  cout << endl;

  // c = a^T b
  blas::gemm ( 1.0, bindings::trans(a), b, 0.0, c);
  print_m (c, "A^T B");
  cout << endl;

  // c = a^H b
  blas::gemm (1.0, bindings::conj(a), b, 0.0, c);
  print_m (c, "A^H B");

  cout << endl;

}


/*
 * lstmlayer.cpp
 *
 * Feed-forward:
 *    x(t) -- inputs
 *    h(t-1) -- hidden state, aka outputs on prev timestep
 *    c(t) -- cell state
 *
 *    Phase 1: I/O gates
 *      zz_a(t) = ww_xa*x(t) + ww_ha * h(t-1) + bb_a  -- input
 *      zz_i(t) = ww_xi*x(t) + ww_hi * h(t-1) + bb_i  -- input gate
 *      zz_f(t) = ww_xf*x(t) + ww_hf * h(t-1) + bb_g  -- forget gate
 *      zz_o(t) = ww_xo*x(t) + ww_ho * h(t-1) + bb_o  -- output gate
 *
 *      a(t) = activ1 (zz_a(t))
 *      i(t) = activ2 (zz_i(t))
 *      f(t) = activ2 (zz_f(t))
 *      o(t) = activ2 (zz_o(t))
 *
 *    Phase 2: State
 *      c(t) = elem_prod(i(t), a(t)) + elem_prod(f(t), c(t-1))
 *
 *    Phase 3: Output
 *      h(t) = elem_prod(o(t), activ1(c(t))
 *
 * Back propagation:
 *
 */
#include <boost/assert.hpp>

#include "core/utils.h"
#include "core/random.h"
#include "core/functions.h"
#include "lstmlayer.h"

using namespace std;
using namespace boost;
using namespace yann;

namespace yann {

// prefixes for read/write
static const char g_ltsm_names_suffix[LstmLayer::Gate_Max + 1] = "aifo";

////////////////////////////////////////////////////////////////////////////////////////////////
//
// LstmLayer_Context implementation
//
class LstmLayer_Context :
    public Layer::Context
{
  typedef Layer::Context Base;

  friend class LstmLayer;

public:
  LstmLayer_Context(
      const MatrixSize & output_size,
      const MatrixSize & max_batch_size) :
    Base(output_size, max_batch_size),
    _pos(0)
  {
    init();
  }

  LstmLayer_Context(
      const RefVectorBatch & output) :
    Base(output),
    _pos(0)
  {
    init();
  }

  // Layer::Context overwrites
  virtual void reset_state()
  {
    Base::reset_state();

    _pos = 0;
  }

private:
  void init()
  {
    for(auto & zz_gate: _zz_gate) {
      zz_gate.resize(get_batch_size(), get_output_size());
    }
    for(auto & gate: _gate) {
      gate.resize(get_batch_size(), get_output_size());
    }

    _cc.resize(get_batch_size(), get_output_size());
    _activ_cc.resize(get_batch_size(), get_output_size());

    _hh.resize(get_batch_size(), get_output_size());
  }

protected:
  MatrixSize  _pos;
  VectorBatch _zz_gate[LstmLayer::Gate_Max]; // pre-activation function results for each gate
  VectorBatch _gate[LstmLayer::Gate_Max];    // post-activation function results for each gate
  VectorBatch _cc;                      // cell state (save it for each step)
  VectorBatch _activ_cc;                // activ1(c(t))
  VectorBatch _hh;                      // hidden state aka output (save it for each step)
}; // class LstmLayer_Context

////////////////////////////////////////////////////////////////////////////////////////////////
//
// LstmLayer_TrainingContext implementation
//
class LstmLayer_TrainingContext :
    public LstmLayer_Context
{
  typedef LstmLayer_Context Base;
  friend class LstmLayer;

public:
  LstmLayer_TrainingContext(
      const MatrixSize & input_size,
      const MatrixSize & output_size,
      const MatrixSize & batch_size,
      const unique_ptr<Layer::Updater> & updater) :
    Base(output_size, batch_size)
  {
    init(updater, input_size);
  }

  LstmLayer_TrainingContext(
      const MatrixSize & input_size,
      const RefVectorBatch & output,
      const unique_ptr<Layer::Updater> & updater) :
    Base(output)
  {
    init(updater, input_size);
  }

  // Layer::Context overwrites
  virtual void start_epoch()
  {
    Base::start_epoch();

    for(auto & ww_x_updater: _ww_x_updater) {
      YANN_SLOW_CHECK(ww_x_updater);
      ww_x_updater->start_epoch();
    }
    for(auto & ww_h_updater: _ww_h_updater) {
      YANN_SLOW_CHECK(ww_h_updater);
      ww_h_updater->start_epoch();
    }
    for(auto & bb_updater: _bb_updater) {
      YANN_SLOW_CHECK(bb_updater);
      bb_updater->start_epoch();
    }
  }

  virtual void reset_state()
  {
    Base::reset_state();

    for(auto & delta_ww_x: _delta_ww_x) {
      delta_ww_x.setZero();
    }
    for(auto & delta_ww_h: _delta_ww_h) {
      delta_ww_h.setZero();
    }
    for(auto & delta_bb: _delta_bb) {
      delta_bb.setZero();
    }

    for(auto & ww_x_updater: _ww_x_updater) {
      YANN_SLOW_CHECK(ww_x_updater);
      ww_x_updater->reset();
    }
    for(auto & ww_h_updater: _ww_h_updater) {
      YANN_SLOW_CHECK(ww_h_updater);
      ww_h_updater->reset();
    }
    for(auto & bb_updater: _bb_updater) {
      YANN_SLOW_CHECK(bb_updater);
      bb_updater->reset();
    }

    _gradient_hh.setZero();
    _gradient_cc.setZero();
  }

private:
  void init(
      const unique_ptr<Layer::Updater> & updater,
      const MatrixSize & input_size)
  {
    YANN_CHECK_GT(updater, 0);
    YANN_CHECK_GT(input_size, 0);

    // deltas
    for(auto & delta_ww_x: _delta_ww_x) {
      delta_ww_x.resize(input_size, get_output_size());
    }
    for(auto & delta_ww_h: _delta_ww_h) {
      delta_ww_h.resize(get_output_size(), get_output_size());
    }
    for(auto & delta_bb: _delta_bb) {
      delta_bb.resize(get_output_size());
    }

    // updaters
    for(auto & ww_x_updater: _ww_x_updater) {
      ww_x_updater = updater->copy();
      YANN_SLOW_CHECK(ww_x_updater);
      ww_x_updater->init(input_size, get_output_size());
    }
    for(auto & ww_h_updater: _ww_h_updater) {
      ww_h_updater = updater->copy();
      YANN_SLOW_CHECK(ww_h_updater);
      ww_h_updater->init(get_output_size(), get_output_size());
    }
    for(auto & bb_updater: _bb_updater) {
      bb_updater = updater->copy();
      YANN_SLOW_CHECK(bb_updater);
      bb_updater->init(1, get_output_size()); // RowMajor
    }

    // gate radients
    _gradient_hh.resize(get_output_size());
    _gradient_cc.resize(get_output_size());
    _gradient_gate.resize(get_output_size());
    _gradient_zz_gate.resize(get_output_size());
    _tmp_derivative.resize(get_output_size());
  }

private:
  Matrix _delta_ww_x[LstmLayer::Gate_Max];
  Matrix _delta_ww_h[LstmLayer::Gate_Max];
  Vector _delta_bb[LstmLayer::Gate_Max];

  unique_ptr<Layer::Updater> _ww_x_updater[LstmLayer::Gate_Max];
  unique_ptr<Layer::Updater> _ww_h_updater[LstmLayer::Gate_Max];
  unique_ptr<Layer::Updater> _bb_updater[LstmLayer::Gate_Max];

  Vector _gradient_hh;
  Vector _gradient_cc;
  Vector _gradient_gate;
  Vector _gradient_zz_gate;
  Vector _tmp_derivative;
}; // class LstmLayer_TrainingContext

}; // namespace yann

////////////////////////////////////////////////////////////////////////////////////////////////
//
// yann::LstmLayer implementation
//
yann::LstmLayer::LstmLayer(
    const MatrixSize & input_size,
    const MatrixSize & output_size) :
    _gate_activation_function(new SigmoidFunction()),
    _io_activation_function(new TanhFunction())
{
  YANN_CHECK_GT(input_size, 0);

  for(auto & ww_x: _ww_x) {
    ww_x.resize(input_size, output_size);
  }
  for(auto & ww_h: _ww_h) {
    ww_h.resize(output_size, output_size);
  }
  for(auto & bb: _bb) {
    bb.resize(output_size);
  }
}

yann::LstmLayer::~LstmLayer()
{
}

void yann::LstmLayer::set_values(
    const Matrix (& ww_x)[Gate_Max],
    const Matrix (& ww_h)[Gate_Max],
    const Vector (& bb)[Gate_Max])
{
  for(auto ii = 0; ii < Gate_Max; ++ii) {
    YANN_CHECK(is_same_size(ww_x[ii], _ww_x[ii]));
    YANN_CHECK(is_same_size(ww_h[ii], _ww_h[ii]));
    YANN_CHECK(is_same_size(bb[ii], _bb[ii]));

    _ww_x[ii] = ww_x[ii];
    _ww_h[ii] = ww_h[ii];
    _bb[ii] = bb[ii];
  }
}

void yann::LstmLayer::set_activation_functions(
    const std::unique_ptr<ActivationFunction> & gate_activation_function,
    const std::unique_ptr<ActivationFunction> & io_activation_function)
{
  YANN_CHECK(gate_activation_function);
  YANN_CHECK(io_activation_function);
  _gate_activation_function = gate_activation_function->copy();
  _io_activation_function = io_activation_function->copy();
}

// Layer overwrites
bool yann::LstmLayer::is_valid() const
{
  if(!Base::is_valid()) {
    return false;
  }
  if(!_io_activation_function || !_gate_activation_function) {
    return false;
  }
  for(auto ii = 0; ii < Gate_Max; ++ii) {
    if(!is_same_size(_ww_x[0], _ww_x[ii])) {
      return false;
    }
    if(!is_same_size(_ww_h[0], _ww_h[ii])) {
      return false;
    }
    if(!is_same_size(_bb[0], _bb[ii])) {
      return false;
    }
  }

  return true;
}

std::string yann::LstmLayer::get_name() const
{
  return "LstmLayer";
}

string yann::LstmLayer::get_info() const
{
  YANN_CHECK(is_valid());

  ostringstream oss;
  oss << Base::get_info()
      << ", state activation: " << _io_activation_function->get_info()
      << ", output activation: " << _gate_activation_function->get_info()
      ;
  return oss.str();
}

bool yann::LstmLayer::is_equal(const Layer & other, double tolerance) const
{
  if(!Base::is_equal(other, tolerance)) {
    return false;
  }
  auto the_other = dynamic_cast<const LstmLayer*>(&other);
  if(the_other == nullptr) {
    return false;
  }
  // TOOD: add deep compare
  if(_io_activation_function->get_info() != the_other->_io_activation_function->get_info()) {
    return false;
  }
  if(_gate_activation_function->get_info() != the_other->_gate_activation_function->get_info()) {
    return false;
  }
  for(auto ii = 0; ii < Gate_Max; ++ii) {
    if(!_ww_x[ii].isApprox(the_other->_ww_x[ii], tolerance)) {
      return false;
    }
    if(!_ww_h[ii].isApprox(the_other->_ww_h[ii], tolerance)) {
      return false;
    }
    if(!_bb[ii].isApprox(the_other->_bb[ii], tolerance)) {
      return false;
    }
  }
  return true;
}

MatrixSize yann::LstmLayer::get_input_size() const
{
  YANN_SLOW_CHECK(is_valid());
  return _ww_x[Gate_A].rows();
}

MatrixSize yann::LstmLayer::get_output_size() const
{
  YANN_SLOW_CHECK(is_valid());
  return _ww_x[Gate_A].cols();
}

unique_ptr<Layer::Context> yann::LstmLayer::create_context(const MatrixSize & batch_size) const
{
  YANN_CHECK(is_valid());
  return make_unique<LstmLayer_Context>(get_output_size(), batch_size);
}
unique_ptr<Layer::Context> yann::LstmLayer::create_context(const RefVectorBatch & output) const
{
  YANN_CHECK(is_valid());
  return make_unique<LstmLayer_Context>(output);
}
unique_ptr<Layer::Context> yann::LstmLayer::create_training_context(
    const MatrixSize & batch_size, const std::unique_ptr<Layer::Updater> & updater) const
{
  YANN_CHECK(is_valid());
  YANN_CHECK(updater);
  return make_unique<LstmLayer_TrainingContext>(
      get_input_size(), get_output_size(), batch_size, updater);
}
unique_ptr<Layer::Context> yann::LstmLayer::create_training_context(
    const RefVectorBatch & output, const std::unique_ptr<Layer::Updater> & updater) const
{
  YANN_CHECK(is_valid());
  YANN_CHECK(updater);
  return make_unique<LstmLayer_TrainingContext>(
      get_input_size(), output, updater);
}

template<typename InputType>
void yann::LstmLayer::feedforward_internal(
    const InputType & input,
    Context * context,
    enum OperationMode mode) const
{
  auto ctx = dynamic_cast<LstmLayer_Context *>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(is_valid());
  YANN_CHECK_LE(get_batch_size(input) + ctx->_pos, ctx->get_batch_size());
  YANN_CHECK_LE(get_batch_size(input), get_batch_size(ctx->get_output()));
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());

  for(MatrixSize ii = 0; ii < get_batch_size(input); ++ii, ++ctx->_pos) {
    // Phase 1: I/O gates
    //  zz_a(t) = ww_xa*x(t) + ww_ha * h(t-1) + bb_a  -- input
    //  zz_i(t) = ww_xi*x(t) + ww_hi * h(t-1) + bb_i  -- input gate
    //  zz_f(t) = ww_xf*x(t) + ww_hf * h(t-1) + bb_g  -- forget gate
    //  zz_o(t) = ww_xo*x(t) + ww_ho * h(t-1) + bb_o  -- output gate
    auto in  = get_batch(input, ii);
    auto out = get_batch(ctx->get_output(), ctx->_pos);
    auto hh  = get_batch(ctx->_hh, ctx->_pos);
    if(ctx->_pos > 0) {
      const auto & hh_prev = get_batch(ctx->_hh, ctx->_pos - 1);
      for(auto jj = 0; jj < Gate_Max; ++jj) {
        auto zz_gate = get_batch(ctx->_zz_gate[jj], ctx->_pos);
        zz_gate.noalias() = MatrixFunctions<InputType>::product(in, _ww_x[jj]);
        zz_gate.noalias() += MatrixFunctions<RefConstMatrix>::product(hh_prev, _ww_h[jj]) + _bb[jj];
      }
    } else {
      for(auto jj = 0; jj < Gate_Max; ++jj) {
        auto zz_gate = get_batch(ctx->_zz_gate[jj], ctx->_pos);
        zz_gate.noalias() = MatrixFunctions<InputType>::product(in, _ww_x[jj]);
        zz_gate.noalias() += _bb[jj];
      }
    }

    //  a(t) = activ1 (zz_a(t))
    //  i(t) = activ2 (zz_i(t))
    //  f(t) = activ2 (zz_f(t))
    //  o(t) = activ2 (zz_o(t))
    for(auto jj = 0; jj < Gate_Max; ++jj) {
      const auto zz_gate = get_batch(ctx->_zz_gate[jj], ctx->_pos);
      auto gate = get_batch(ctx->_gate[jj], ctx->_pos);
      if(jj > 0) {
        _gate_activation_function->f(zz_gate, gate, Operation_Assign);
      } else {
        _io_activation_function->f(zz_gate, gate, Operation_Assign);
      }
    }

    // Phase 2: State
    //  c(t) = elem_prod(i(t), a(t)) + elem_prod(f(t), c(t-1))
    const auto gate_aa = get_batch(ctx->_gate[Gate_A], ctx->_pos);
    const auto gate_ii = get_batch(ctx->_gate[Gate_I], ctx->_pos);
    const auto gate_ff = get_batch(ctx->_gate[Gate_F], ctx->_pos);
    auto cc = get_batch(ctx->_cc, ctx->_pos);
    if(ctx->_pos > 0) {
      const auto & cc_prev = get_batch(ctx->_cc, ctx->_pos - 1);
      cc.array() = gate_ii.array() * gate_aa.array() + gate_ff.array() * cc_prev.array();
    } else {
      cc.array() = gate_ii.array() * gate_aa.array();
    }

    // Phase 3: Output
    //  h(t) = elem_prod(o(t), activ1(c(t))
    const auto gate_oo = get_batch(ctx->_gate[Gate_O], ctx->_pos);
    auto activ_cc = get_batch(ctx->_activ_cc, ctx->_pos);
    _io_activation_function->f(cc, activ_cc, Operation_Assign);
    hh.array() = gate_oo.array() * activ_cc.array();

    // done!
    switch(mode) {
    case Operation_Assign:
      out.noalias() = hh;
      break;
    case Operation_PlusEqual:
      out.noalias() += hh;
      break;
    }
  }
}

void yann::LstmLayer::feedforward(
    const RefConstVectorBatch & input,
    Context * context,
    enum OperationMode mode) const
{
  feedforward_internal(input, context, mode);
}

void yann::LstmLayer::feedforward(
    const RefConstSparseVectorBatch & input,
    Context * context,
    enum OperationMode mode) const
{
  feedforward_internal<RefConstSparseMatrix>(input, context, mode);
}

template<
  typename MainInputType,
  typename InputType,
  typename GradientInputType,
  typename ContextType
>
void yann::LstmLayer::backprop_gate(
    enum IOGates gate,
    const std::unique_ptr<ActivationFunction> & activation_function,
    const RefConstVector & gradient_gate,
    const InputType & input,
    boost::optional<GradientInputType> gradient_in,
    ContextType * ctx) const
{
  YANN_SLOW_CHECK(activation_function);
  YANN_SLOW_CHECK(ctx);

  const auto zz_gate = get_batch(ctx->_zz_gate[gate], ctx->_pos);
  const auto & ww_x = _ww_x[gate];
  const auto & ww_h = _ww_h[gate];
  auto & gradient_zz_gate = ctx->_gradient_zz_gate;
  auto & tmp_derivative = ctx->_tmp_derivative;

  // gate(t) = activ(zz_gate(t))
  //
  // grad(zz_gate) = grad(gate) * activ'(zz_gate)
  activation_function->derivative(zz_gate, tmp_derivative);
  gradient_zz_gate.array() = gradient_gate.array() * tmp_derivative.array();

  // zz_gate(t) = ww_x_gate*x(t) + ww_h_gate * h(t-1) + bb_gate
  //
  // grad(ww_x_gate) = grad(zz_gate) * d zz_gate/dww_x_gate = grad(zz_gate) * x(t)
  // grad(ww_h_gate) = grad(zz_gate) * d zz_gate/dww_h_gate = grad(zz_gate) * h(t-1)
  // grad(b) = grad(zz_gate) * d zz_gate/db = grad(zz_gate)
  YANN_SLOW_CHECK(is_same_size(ctx->_delta_ww_x[gate], _ww_x[gate]));
  YANN_SLOW_CHECK(is_same_size(ctx->_delta_ww_h[gate], _ww_h[gate]));
  YANN_SLOW_CHECK(is_same_size(ctx->_delta_bb[gate], _bb[gate]));
  ctx->_delta_ww_x[gate] += MatrixFunctions<MainInputType>::product(input.transpose(), gradient_zz_gate);
  if(ctx->_pos > 0) {
    auto prev_hh = get_batch(ctx->_hh, ctx->_pos - 1);
    ctx->_delta_ww_h[gate] += MatrixFunctions<RefConstMatrix>::product(prev_hh.transpose(), gradient_zz_gate);
  }
  ctx->_delta_bb[gate].noalias() += gradient_zz_gate;

  // zz_gate(t) = ww_x_gate*x(t) + ww_h_gate * h(t-1) + bb_gate
  //
  // grad(h) = grad(zz_gate) * d zz_gate/dh = grad(zz_gate) * transp(ww_h_gate)
  if(ctx->_pos > 0) {
    auto & gradient_hh = ctx->_gradient_hh;
    gradient_hh.noalias() += MatrixFunctions<RefConstMatrix>::product(gradient_zz_gate, ww_h.transpose());
  }

  // zz_gate(t) = ww_x_gate*x(t) + ww_h_gate * h(t-1) + bb_gate
  //
  //  grad(x) = grad(zz_gate) * d zz_gate/dx = grad(zz_gate) * transp(ww_x_gate)
  if(gradient_in) {
    (*gradient_in).noalias() += MatrixFunctions<RefConstMatrix>::product(gradient_zz_gate, ww_x.transpose());
  }
}

template<typename InputType>
void yann::LstmLayer::backprop_internal(
    const RefConstVectorBatch & gradient_output,
    const InputType & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  auto ctx = dynamic_cast<LstmLayer_TrainingContext *>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(is_valid());
  YANN_CHECK_GT(get_batch_size(gradient_output), 0);
  YANN_CHECK_LE(get_batch_size(gradient_output), ctx->_pos);
  YANN_CHECK_EQ(get_batch_item_size(gradient_output), get_output_size());
  YANN_CHECK_EQ(get_batch_size(input), get_batch_size(gradient_output));
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());
  YANN_CHECK(!gradient_input || is_same_size(input, *gradient_input));

  // Zero out since we will be adding to it
  if(gradient_input) {
    gradient_input->setZero();
  }

  for(MatrixSize ii = get_batch_size(gradient_output) - 1; ii >= 0 && (--ctx->_pos) >= 0; --ii) {
    const auto in = get_batch(input, ii);
    auto gradient_in = gradient_input ? make_optional(get_batch(*gradient_input, ii)) : boost::none;
    const auto gate_aa  = get_batch(ctx->_gate[Gate_A], ctx->_pos);
    const auto gate_ii  = get_batch(ctx->_gate[Gate_I], ctx->_pos);
    const auto gate_ff  = get_batch(ctx->_gate[Gate_F], ctx->_pos);
    const auto gate_oo  = get_batch(ctx->_gate[Gate_O], ctx->_pos);
    auto & gradient_gate = ctx->_gradient_gate;

    // note that output gradient also has the internal feedback loop
    auto & gradient_hh = ctx->_gradient_hh;
    gradient_hh += get_batch(gradient_output, ii);

    // State cell
    //
    //  h(t) = elem_prod(o(t), activ1(c(t))
    //  grad(c) = grad(h) * d h/dactiv1(c) * activ1'(c) = elem_prod(grad(h), o, activ1(c)
    const auto cc = get_batch(ctx->_cc, ctx->_pos);
    auto & gradient_cc = ctx->_gradient_cc;
    auto & tmp_derivative = ctx->_tmp_derivative;
    _io_activation_function->derivative(cc, tmp_derivative);
    // add to the gradient from previous step
    gradient_cc.array() += gradient_hh.array() * gate_oo.array() * tmp_derivative.array();

    // Gate O: Output gate:
    //
    //  h(t) = elem_prod(o(t), activ1(c(t))
    //  grad(o) = grad(h) * dh/do = elem_prod(grad(h), activ1(c))
    const auto activ_cc = get_batch(ctx->_activ_cc, ctx->_pos);
    gradient_gate.array() = gradient_hh.array() * activ_cc.array();
    gradient_hh.setZero(); // we will update it in backprop_gate for next step
    backprop_gate<InputType>(Gate_O, _gate_activation_function, gradient_gate, in, gradient_in, ctx);

    // Gate A
    //
    //  c(t) = elem_prod(i(t), a(t)) + elem_prod(f(t), c(t-1))
    //  grad(a) = grad(c) * dc/da = elem_prod(grad(c), i(t))
    gradient_gate.array() = gradient_cc.array() * gate_ii.array();
    backprop_gate<InputType>(Gate_A, _io_activation_function, gradient_gate, in, gradient_in, ctx);

    // Gate I: Input gate
    //
    //  c(t) = elem_prod(i(t), a(t)) + elem_prod(f(t), c(t-1))
    //  grad(i) = grad(c) * dc/di = elem_prod(grad(c), a(t))
    gradient_gate.array() = gradient_cc.array() * gate_aa.array();
    backprop_gate<InputType>(Gate_I, _gate_activation_function, gradient_gate, in, gradient_in, ctx);

    // Gate F: Forget gate
    //
    //  c(t) = elem_prod(i(t), a(t)) + elem_prod(f(t), c(t-1))
    //  grad(f) = grad(c) * dc/df = elem_prod(grad(c), c(t-1))
    if(ctx->_pos > 0) {
      const auto prev_cc = get_batch(ctx->_activ_cc, ctx->_pos - 1);
      gradient_gate.array() = gradient_cc.array() * prev_cc.array();
    } else {
      gradient_gate.setZero();
    }
    backprop_gate<InputType>(Gate_F, _gate_activation_function, gradient_gate, in, gradient_in, ctx);

    // State gradient for prev step
    //
    //  c(t) = elem_prod(i(t), a(t)) + elem_prod(f(t), c(t-1))
    //  grad(c(t-1)) = grad(c) * dc(t)/dc(t-1) = elem_prod(grad(c(t)), f(t))
    if(ctx->_pos > 0) {
      gradient_cc.array() = gradient_cc.array() * gate_ff.array();
    }
  }
}

void yann::LstmLayer::backprop(
    const RefConstVectorBatch & gradient_output,
    const RefConstVectorBatch & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  backprop_internal(gradient_output, input, gradient_input, context);
}

void yann::LstmLayer::backprop(
    const RefConstVectorBatch & gradient_output,
    const RefConstSparseVectorBatch & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  // <RefConstSparseMatrix> is required for MatrixFunctions<>::product
  backprop_internal<RefConstSparseMatrix>(gradient_output, input, gradient_input, context);
}

void yann::LstmLayer::init(enum InitMode mode, boost::optional<InitContext> init_context)
{
  switch (mode) {
  case InitMode_Zeros:
    for(auto & ww_x: _ww_x) {
      ww_x.setZero();
    }
    for(auto & ww_h: _ww_h) {
      ww_h.setZero();
    }
    for(auto & bb: _bb) {
      bb.setZero();
    }
    break;
  case InitMode_Random:
    {
      unique_ptr<RandomGenerator> gen01 = RandomGenerator::normal_distribution(0, 1,
          init_context ? optional<Value>(init_context->seed()) : boost::none);
      for(auto & ww_x: _ww_x) {
        gen01->generate(ww_x);
      }
      for(auto & ww_h: _ww_h) {
        gen01->generate(ww_h);
      }
      for(auto & bb: _bb) {
        gen01->generate(bb);
      }
    }
    break;
  }
}

void yann::LstmLayer::update(Context * context, const size_t & tests_num)
{
  auto ctx = dynamic_cast<LstmLayer_TrainingContext *>(context);
  YANN_CHECK(ctx);

  for(auto ii = 0; ii < Gate_Max; ++ii) {
    YANN_SLOW_CHECK(ctx->_ww_x_updater[ii]);
    YANN_SLOW_CHECK(ctx->_ww_h_updater[ii]);
    YANN_SLOW_CHECK(ctx->_bb_updater[ii]);

    ctx->_ww_x_updater[ii]->update(ctx->_delta_ww_x[ii], tests_num, _ww_x[ii]);
    ctx->_ww_h_updater[ii]->update(ctx->_delta_ww_h[ii], tests_num, _ww_h[ii]);
    ctx->_bb_updater[ii]->update(ctx->_delta_bb[ii], tests_num, _bb[ii]);
  }
}

// the format is (no new lines):
// (wxa:<_ww_x[Gate_A]>,wxh:<_ww_h[Gate_A]>,ba:<_bb[Gate_A]>,
//  wxi:<_ww_x[Gate_I]>,wxi:<_ww_h[Gate_I]>,bi:<_bb[Gate_I]>,
//  wxf:<_ww_x[Gate_F]>,wxf:<_ww_h[Gate_F]>,bf:<_bb[Gate_F]>,
//  wxo:<_ww_x[Gate_O]>,wxo:<_ww_h[Gate_O]>,bo:<_bb[Gate_O]>)
void yann::LstmLayer::read(std::istream & is)
{
  Base::read(is);

  read_char(is, '(');

  for(auto ii = 0; ii < Gate_Max; ++ii) {
    const char & suffix =  g_ltsm_names_suffix[ii];
    if(ii > 0) {
      read_char(is, ',');
    }
    read_object(is, string("wx") + suffix, _ww_x[ii]);
    read_char(is, ',');
    read_object(is, string("wh") + suffix, _ww_h[ii]);
    read_char(is, ',');
    read_object(is, string("b") + suffix, _bb[ii]);
  }
  read_char(is, ')');
}

// the format is (no new lines):
// (wxa:<_ww_x[Gate_A]>,wxh:<_ww_h[Gate_A]>,ba:<_bb[Gate_A]>,
//  wxi:<_ww_x[Gate_I]>,wxi:<_ww_h[Gate_I]>,bi:<_bb[Gate_I]>,
//  wxf:<_ww_x[Gate_F]>,wxf:<_ww_h[Gate_F]>,bf:<_bb[Gate_F]>,
//  wxo:<_ww_x[Gate_O]>,wxo:<_ww_h[Gate_O]>,bo:<_bb[Gate_O]>)
void yann::LstmLayer::write(std::ostream & os) const
{
  Base::write(os);

  os << "(";
  for(auto ii = 0; ii < Gate_Max; ++ii) {
    const char & suffix =  g_ltsm_names_suffix[ii];
    if(ii > 0) {
      os << ",";
    }
    write_object(os, string("wx") + suffix, _ww_x[ii]);
    os << ",";
    write_object(os, string("wh") + suffix, _ww_h[ii]);
    os << ",";
    write_object(os, string("b") + suffix, _bb[ii]);
  }
  os << ")";
}


#ifndef TRIUMF_BNMR_SLR_STR_EXP_HPP
#define TRIUMF_BNMR_SLR_STR_EXP_HPP

#include <boost/math/quadrature/tanh_sinh.hpp>
#include <cmath>
#include <triumf/bnmr/slr/common.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

// β-detected nuclear magnetic resonance (β-NMR)
namespace bnmr {

// spin-lattice relaxation (SLR)
namespace slr {

/// pulsed stretched exponential integral (from 0 to time_p)
template <typename T = double>
T pulsed_str_exp_integral(T time, T time_p, T nuclear_lifetime, T slr_rate,
                          T beta) {
  // integrand for the numeric integral
  auto integrand = [=](T t_p) {
    return std::exp(-(time - t_p) / nuclear_lifetime) *
           std::exp(-std::pow(slr_rate * (time - t_p), beta));
  };
  // create the integrator for tanh-sinh quadrature
  static boost::math::quadrature::tanh_sinh<T> integrator;
  // evaluate the integral from 0 to time_p
  T Q = integrator.integrate(integrand, 0.0, time_p);
  return Q;
}

/// pulsed stretched exponential
template <typename T = double>
T pulsed_str_exp(T time, T nuclear_lifetime, T pulse_length, T asymmetry,
                 T slr_rate, T beta) {
  if (time == 0.0) {
    return asymmetry;
  } else if (time > 0.0 and time <= pulse_length) {
    return asymmetry *
           pulsed_str_exp_integral<T>(time, time, nuclear_lifetime, slr_rate,
                                      beta) /
           normalization<T>(time, nuclear_lifetime);
  } else if (time > pulse_length) {
    return (asymmetry *
            pulsed_str_exp_integral<T>(time, pulse_length, nuclear_lifetime,
                                       slr_rate, beta) /
            normalization<T>(pulse_length, nuclear_lifetime)) /
           std::exp(-(time - pulse_length) / nuclear_lifetime);
  } else {
    return 0.0;
  }
}

/// pulsed stretched exponential (ROOT)
template <typename T = double> T pulsed_str_exp(const T *x, const T *par) {
  return pulsed_str_exp<T>(*x, par[0], par[1], par[2], par[3], par[4]);
}

} // namespace slr

} // namespace bnmr

} // namespace triumf

#endif // TRIUMF_BNMR_SLR_STR_EXP_HPP


#include "matrix_utils.h"
#include <Eigen/SparseCholesky>
#include <Eigen/LU>

namespace rsurfaces
{
    namespace MatrixUtils
    {
        void TripleTriplets(const std::vector<Triplet> &orig, std::vector<Triplet> &output)
        {
            for (const Triplet &t : orig)
            {
                output.push_back(Triplet(3 * t.row(), 3 * t.col(), t.value()));
                output.push_back(Triplet(3 * t.row() + 1, 3 * t.col() + 1, t.value()));
                output.push_back(Triplet(3 * t.row() + 2, 3 * t.col() + 2, t.value()));
            }
        }

        void TripleMatrix(const Eigen::MatrixXd &M, Eigen::MatrixXd &out)
        {
            for (int i = 0; i < M.rows(); i++)
            {
                for (int j = 0; j < M.cols(); j++)
                {
                    out(3 * i, 3 * j) = M(i, j);
                    out(3 * i + 1, 3 * j + 1) = M(i, j);
                    out(3 * i + 2, 3 * j + 2) = M(i, j);
                }
            }
        }

        void MatrixIntoColumn(const Eigen::MatrixXd &M, Eigen::VectorXd &out)
        {
            int rows = M.rows();
            int cols = M.cols();

            for (int i = 0; i < rows; i++)
            {
                for (int j = 0; j < cols; j++)
                {
                    int ind = cols * i + j;
                    out(ind) = M(i, j);
                }
            }
        }

        void ColumnIntoMatrix(const Eigen::VectorXd &v, Eigen::MatrixXd &out)
        {
            int rows = out.rows();
            int cols = out.cols();

            for (int i = 0; i < rows; i++)
            {
                for (int j = 0; j < cols; j++)
                {
                    int ind = cols * i + j;
                    out(i, j) = v(ind);
                }
            }
        }

        void SolveSparseSystem(const Eigen::SparseMatrix<double> &M, Eigen::VectorXd &rhs, Eigen::VectorXd &output)
        {
            Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver;
            solver.compute(M);
            if (solver.info() != Eigen::Success)
            {
                // decomposition failed
                std::cout << "Sparse decomposition failed. Exiting." << std::endl;
                std::exit(1);
            }
            Eigen::VectorXd x = solver.solve(rhs);
            if (solver.info() != Eigen::Success)
            {
                // solving failed
                std::cout << "Sparse solve failed. Exiting." << std::endl;
                std::exit(1);
            }
            output = x;
        }

        void SolveDenseSystem(const Eigen::MatrixXd &M, Eigen::VectorXd &rhs, Eigen::VectorXd &output)
        {
            Eigen::PartialPivLU<Eigen::MatrixXd> solver = M.partialPivLu();
            output = solver.solve(rhs);
        }

        void SolveDenseSystem(Eigen::PartialPivLU<Eigen::MatrixXd> &solver, const Eigen::MatrixXd &M, Eigen::VectorXd &rhs, Eigen::VectorXd &output)
        {
            solver.compute(M);
            output = solver.solve(rhs);
        }

    } // namespace MatrixUtils
} // namespace rsurfaces


// #include <iostream>
// #include <boost/random/mersenne_twister.hpp>
// #include <boost/random/uniform_int.hpp>
// #include <boost/random/uniform_real.hpp>
//
// #include "ParticleFilter.h"
//
// using std::cout;
// using std::endl;
//
// /// Randomly initialize all particles
// int ParticleFilter::init(int num_particles, double x_min, double x_max,
// 	      double y_min, double y_max,
// 	      double heading_min, double heading_max,
// 	      double velocity_min, double velocity_max)
// {
//      num_particles_ = num_particles;
//
//      x_min_ = x_min;
//      x_max_ = x_max;
//      y_min_ = y_min;
//      y_max_ = y_max;
//      heading_min_ = heading_min;
//      heading_max_ = heading_max;
//      velocity_min_ = velocity_min;
//      velocity_max_ = velocity_max;
//
//      boost::uniform_int<> x_dist(x_min_, x_max_);
//      boost::uniform_int<> y_dist(y_min_, y_max_);
//      boost::uniform_int<> heading_dist(heading_min_, heading_max_);
//      boost::uniform_int<> x_velocity_dist(velocity_min_, velocity_max_);
//      boost::uniform_int<> y_velocity_dist(velocity_min_, velocity_max_);
//
//      for (int i = 0 ; i < num_particles_ ; i++) {
// 	  double x=0, y=0, heading=0, x_velocity=0, y_velocity;
//
// 	  x = x_dist(gen_);
// 	  y = y_dist(gen_);
// 	  heading = heading_dist(gen_);
// 	  x_velocity = x_velocity_dist(gen_);
// 	  y_velocity = y_velocity_dist(gen_);
//
// 	  cv::Point center(x,y);
// 	  cv::Point2d velocity(x_velocity,y_velocity);
//
// 	  Particle particle(center,heading,velocity,1.0/num_particles_);
// 	  particles_.push_back(particle);
//      }
//
//      return 0;
// }
//
// /// Use motion model to update particles
// int ParticleFilter::kinematic_step()
// {
//      std::vector<Particle>::iterator it;
//      for (it = particles_.begin(); it != particles_.end(); it++) {
// 	  it->kinematic_step();
//      }
//      return 0;
// }
//
// int ParticleFilter::importance_weight_update(cv::Point target)
// {
//      double champ = -9999;
//      double wNorm = 0;
//      std::vector<Particle>::iterator it;
//      for (it = particles_.begin(); it != particles_.end(); it++) {
// 	  double w = it->importance_weight_update(target);
// 	  wNorm += w;
//
// 	  if (w > champ) {
// 	       champ = w;
// 	  }
//      }
//
//      wMax_ = champ;
//
//      for (it = particles_.begin(); it != particles_.end(); it++) {
// 	  it->normalize_importance_weight(wNorm);
//      }
//
//      return 0;
// }
//
// int ParticleFilter::importance_weight_update(cv::Point target, cv::Point2d velocity)
// {
//      double champ = -9999;
//      double wNorm = 0;
//      std::vector<Particle>::iterator it;
//      for (it = particles_.begin(); it != particles_.end(); it++) {
// 	  double w = it->importance_weight_update(target, velocity);
// 	  wNorm += w;
//
// 	  if (w > champ) {
// 	       champ = w;
// 	  }
//      }
//
//      wMax_ = champ;
//
//      for (it = particles_.begin(); it != particles_.end(); it++) {
// 	  it->normalize_importance_weight(wNorm);
//      }
//
//      return 0;
// }
//
//
//
// int ParticleFilter::resample_wheel()
// {
//      boost::uniform_int<> index_select(0, particles_.size()-1);
//      boost::uniform_real<> beta_select(0, 2*wMax_);
//
//      std::vector<Particle> newParticles;
//
//      int index = index_select(gen_);
//      double beta = 0;
//
//      //std::vector<Particle>::iterator it;
//      //for (it = particles_.begin(); it != particles_.end(); it++) {
//      for (int i = 0 ; i < num_particles_; i++) {
// 	  beta += beta_select(gen_);
// 	  while (beta > particles_[index].not_normalized_weight()) {
// 	       beta -= particles_[index].not_normalized_weight();
// 	       index = (index + 1) % particles_.size();
// 	  }
//
// 	  Particle part1 = particles_[index];
// 	  Particle part2 = part1;
//
// 	  boost::uniform_int<> x1_velocity_dist(velocity_min_, velocity_max_);
// 	  boost::uniform_int<> y1_velocity_dist(velocity_min_, velocity_max_);
//
// 	  boost::uniform_int<> x2_velocity_dist(velocity_min_, velocity_max_);
// 	  boost::uniform_int<> y2_velocity_dist(velocity_min_, velocity_max_);
//
// 	  cv::Point2d velocity1, velocity2;
// 	  velocity1.x = x1_velocity_dist(gen_);
// 	  velocity1.y = y1_velocity_dist(gen_);
//
// 	  velocity2.x = x2_velocity_dist(gen_);
// 	  velocity2.y = y2_velocity_dist(gen_);
//
// 	  part1.set_velocity(velocity1);
// 	  part2.set_velocity(velocity2);
//
// 	  newParticles.push_back(part1);
// 	  newParticles.push_back(part2);
//      }
//      particles_ = newParticles;
//      return 0;
// }
//
//
// int ParticleFilter::getParticles(std::vector<Particle> &particles)
// {
//      particles = particles_;
//      return 0;
// }


// Modified from NetKet source for YANNQ Project
// <Chae-Yeun Park>(chae.yeun.park@gmail.com)
// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// 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.

#ifndef YANNQ_ACTIVATIONLAYER_HH
#define YANNQ_ACTIVATIONLAYER_HH

#include <complex>
#include <fstream>
#include <random>
#include <vector>
#include <tuple>
#include <type_traits>

#include <Eigen/Dense>

#include "AbstractLayer.hpp"
#include "activations.hpp"

/**
 * Misc: Because of bugs in Intel C compiler (ICC), constexpr static member
 * variable of template class does not work. If there's any update in ICC
 * regarding this bug, one may update name function to return such a member
 * variable.
 * */

namespace yannq {

template <typename T, class Activation>
class ActivationLayer
	: public AbstractLayer<T>
{
public:
	using Scalar = T;
	using Vector = typename AbstractLayer<T>::Vector;
	using Matrix = typename AbstractLayer<T>::Matrix;
	using VectorRef = typename AbstractLayer<T>::VectorRef;
	using VectorConstRef = typename AbstractLayer<T>::VectorConstRef;

private:

	Activation f_;

public:
	ActivationLayer()
	{
	}

	template<typename ...Ts,
		typename disable_if<
            std::is_same<
                typename std::remove_reference<typename get_nth_type<0, Ts...>::type>::type,
                ActivationLayer<T, Activation>
			>::value, int>::type = 0>
	ActivationLayer(Ts&&... args)
		: f_(std::forward<Ts>(args)...)
	{
	}

	ActivationLayer(const ActivationLayer&) = default;
	ActivationLayer(ActivationLayer&&) = default;

	ActivationLayer& operator=(const ActivationLayer&) = default;
	ActivationLayer& operator=(ActivationLayer&&) = default;

	std::string name() const override
	{
		return std::string("Activation Layer");
	}

	bool operator==(const ActivationLayer& rhs) const
	{
		return f_.name() == rhs.f_.name();
	}

	uint32_t paramDim() const override { return 0; }

	Vector getParams() const override
	{
		return Vector{};
	}

	void setParams(VectorConstRef pars) override
	{
		(void)pars;
	}

	uint32_t outputDim(uint32_t inputDim) const override
	{
		return inputDim;
	}

	// Feedforward
	void forward(const VectorConstRef& input, VectorRef output) override
	{
		assert(input.size() == output.size());
		f_.operator()(input, output);
	}

	// Computes derivative.
	void backprop(const VectorConstRef& prev_layer_output,
			const VectorConstRef& this_layer_output,
			const VectorConstRef& dout,
			VectorRef din, VectorRef /*der*/) override
	{
		din.resize(prev_layer_output.size());
		f_.ApplyJacobian(prev_layer_output, this_layer_output, dout, din);
	}

	nlohmann::json desc() const override
	{
		nlohmann::json layerpar;
		layerpar["name"] = name();
		layerpar["activation"] = f_.name();

		return layerpar;
	}

	template<class Archive>
	void serialize(Archive& ar)
	{
		ar(f_);
	}
};

template<typename T>
using Identity = ActivationLayer<T, activation::Identity<T> >;

template<typename T>
using LnCosh = ActivationLayer<T, activation::LnCosh<T> >;

template<typename T>
using Tanh = ActivationLayer<T, activation::Tanh<T> >;

template<typename T>
using WeakTanh = ActivationLayer<T, activation::WeakTanh<T> >;

template<typename T>
using Sigmoid = ActivationLayer<T, activation::Sigmoid<T> >;

template<typename T>
using ReLU = ActivationLayer<T, activation::ReLU<T> >;

template<typename T>
using LeakyReLU = ActivationLayer<T, activation::LeakyReLU<T> >;

template<typename T>
using HardTanh = ActivationLayer<T, activation::HardTanh<T> >;

template<typename T>
using SoftShrink = ActivationLayer<T, activation::SoftShrink<T> >;

template<typename T>
using LeakyHardTanh = ActivationLayer<T, activation::LeakyHardTanh<T> >;

template<typename T>
using SoftSign = ActivationLayer<T, activation::SoftSign<T> >;

template<typename T>
using Cos = ActivationLayer<T, activation::Cos<T> >;

}  // namespace yannq

#endif


#ifndef STAN_MATH_PRIM_MAT_FUN_MDIVIDE_LEFT_TRI_HPP
#define STAN_MATH_PRIM_MAT_FUN_MDIVIDE_LEFT_TRI_HPP

#include <boost/math/tools/promotion.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/promote_common.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>
#ifdef STAN_OPENCL
#include <stan/math/opencl/opencl_context.hpp>
#include <stan/math/opencl/multiply.hpp>
#include <stan/math/opencl/tri_inverse.hpp>
#include <stan/math/opencl/transpose.hpp>
#include <stan/math/opencl/copy.hpp>
#endif
namespace stan {
namespace math {

/**
 * Returns the solution of the system Ax=b when A is triangular.
 * @tparam TriView Specifies whether A is upper (Eigen::Upper)
 * or lower triangular (Eigen::Lower).
 * @tparam T1 type of elements in A
 * @tparam T2 type of elements in b
 * @tparam R1 number of rows in A
 * @tparam C1 number of columns in A
 * @tparam R2 number of rows in b
 * @tparam C2 number of columns in b
 * @param A Triangular matrix.
 * @param b Right hand side matrix or vector.
 * @return x = A^-1 b, solution of the linear system.
 * @throws std::domain_error if A is not square or the rows of b don't
 * match the size of A.
 */
template <int TriView, typename T1, typename T2, int R1, int C1, int R2, int C2>
inline Eigen::Matrix<typename boost::math::tools::promote_args<T1, T2>::type,
                     R1, C2>
mdivide_left_tri(const Eigen::Matrix<T1, R1, C1> &A,
                 const Eigen::Matrix<T2, R2, C2> &b) {
  check_square("mdivide_left_tri", "A", A);
  check_multiplicable("mdivide_left_tri", "A", A, "b", b);
  return promote_common<Eigen::Matrix<T1, R1, C1>, Eigen::Matrix<T2, R1, C1> >(
             A)
      .template triangularView<TriView>()
      .solve(
          promote_common<Eigen::Matrix<T1, R2, C2>, Eigen::Matrix<T2, R2, C2> >(
              b));
}

/**
 * Returns the solution of the system Ax=b when A is triangular and b=I.
 * @tparam T type of elements in A
 * @tparam R1 number of rows in A
 * @tparam C1 number of columns in A
 * @param A Triangular matrix.
 * @return x = A^-1 .
 * @throws std::domain_error if A is not square
 */
template <int TriView, typename T, int R1, int C1>
inline Eigen::Matrix<T, R1, C1> mdivide_left_tri(
    const Eigen::Matrix<T, R1, C1> &A) {
  check_square("mdivide_left_tri", "A", A);
  int n = A.rows();
  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> b;
  b.setIdentity(n, n);
  A.template triangularView<TriView>().solveInPlace(b);
  return b;
}

/**
 * Returns the solution of the system Ax=b when A is triangular
 * and A and b are matrices of doubles.
 * @tparam TriView Specifies whether A is upper (Eigen::Upper)
 * or lower triangular (Eigen::Lower).
 * @tparam R1 number of rows in A
 * @tparam C1 number of columns in A
 * @tparam R2 number of rows in b
 * @tparam C2 number of columns in b
 * @param A Triangular matrix.
 * @param b Right hand side matrix or vector.
 * @return x = A^-1 b, solution of the linear system.
 * @throws std::domain_error if A is not square or the rows of b don't
 * match the size of A.
 */
template <int TriView, int R1, int C1, int R2, int C2>
inline Eigen::Matrix<double, R1, C2> mdivide_left_tri(
    const Eigen::Matrix<double, R1, C1> &A,
    const Eigen::Matrix<double, R2, C2> &b) {
  check_square("mdivide_left_tri", "A", A);
  check_multiplicable("mdivide_left_tri", "A", A, "b", b);
#ifdef STAN_OPENCL
  if (A.rows()
      >= opencl_context.tuning_opts().tri_inverse_size_worth_transfer) {
    matrix_cl A_cl(A);
    matrix_cl b_cl(b);
    matrix_cl A_inv_cl(A.rows(), A.cols());
    if (TriView == Eigen::Lower) {
      A_inv_cl = tri_inverse<TriangularViewCL::Lower>(A_cl);
    } else {
      A_inv_cl = tri_inverse<TriangularViewCL::Upper>(A_cl);
    }
    matrix_cl C_cl = A_inv_cl * b_cl;
    return from_matrix_cl(C_cl);
  } else {
#endif
    return A.template triangularView<TriView>().solve(b);
#ifdef STAN_OPENCL
  }
#endif
}

/**
 * Returns the solution of the system Ax=b when A is triangular, b=I and
 * both are matrices of doubles.
 * @tparam TriView Specifies whether A is upper (Eigen::Upper)
 * or lower triangular (Eigen::Lower).
 * @tparam R1 number of rows in A
 * @tparam C1 number of columns in A
 * @param A Triangular matrix.
 * @return x = A^-1 .
 * @throws std::domain_error if A is not square
 */
template <int TriView, int R1, int C1>
inline Eigen::Matrix<double, R1, C1> mdivide_left_tri(
    const Eigen::Matrix<double, R1, C1> &A) {
  check_square("mdivide_left_tri", "A", A);
  const int n = A.rows();
#ifdef STAN_OPENCL
  if (A.rows()
      >= opencl_context.tuning_opts().tri_inverse_size_worth_transfer) {
    matrix_cl A_cl(A);
    if (TriView == Eigen::Lower) {
      A_cl = tri_inverse<TriangularViewCL::Lower>(A_cl);
    } else {
      A_cl = tri_inverse<TriangularViewCL::Upper>(A_cl);
    }
    return from_matrix_cl(A_cl);
  } else {
#endif
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> b;
    b.setIdentity(n, n);
    A.template triangularView<TriView>().solveInPlace(b);
    return b;
#ifdef STAN_OPENCL
  }
#endif
}

}  // namespace math
}  // namespace stan
#endif