/*
 * This file belongs to the Galois project, a C++ library for exploiting parallelism.
 * The code is being released under the terms of the 3-Clause BSD License (a
 * copy is located in LICENSE.txt at the top-level directory).
 *
 * Copyright (C) 2018, The University of Texas at Austin. All rights reserved.
 * UNIVERSITY EXPRESSLY DISCLAIMS ANY AND ALL WARRANTIES CONCERNING THIS
 * SOFTWARE AND DOCUMENTATION, INCLUDING ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR ANY PARTICULAR PURPOSE, NON-INFRINGEMENT AND WARRANTIES OF
 * PERFORMANCE, AND ANY WARRANTY THAT MIGHT OTHERWISE ARISE FROM COURSE OF
 * DEALING OR USAGE OF TRADE.  NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH
 * RESPECT TO THE USE OF THE SOFTWARE OR DOCUMENTATION. Under no circumstances
 * shall University be liable for incidental, special, indirect, direct or
 * consequential damages or loss of profits, interruption of business, or
 * related expenses which may arise from use of Software or Documentation,
 * including but not limited to those resulting from defects in Software and/or
 * Documentation, or loss or inaccuracy of data of any kind.
 */
#ifndef _MR_BC_BIT_SET_
#define _MR_BC_BIT_SET_

#include "galois/DynamicBitset.h"
#include <boost/random/detail/integer_log2.hpp>

/** OPTIONS **********/
/** 1. Optimized mode: enable ONLY ONE of them at most **/
// #define REVERSE_MODE //! Not applicable for v6
// #define FLIP_MODE  //! Not applicable for v6
/** 2. Do you need an indicator? **/
#define USE_INDICATOR
/*********************/

/**
 * Derivate from DynamicBitSet
 **/
class MRBCBitSet : public galois::DynamicBitSet {
  // using Base = galois::DynamicBitSet;
  // @DynamicBitSet (protected)
  // using Base::bitvec;
  // using Base::num_bits;
  // using Base::bits_uint64;

  #ifdef USE_INDICATOR
    //! indicate the index of bit to process
    size_t indicator;
  #endif

  // Member functions
  inline size_t get_word(size_t pos) const { return pos < bits_uint64? 0 : pos / bits_uint64; }
  inline size_t get_offset(size_t pos) const { return pos < bits_uint64? pos : pos % bits_uint64; }
  inline uint64_t get_mask(size_t pos) const { return uint64_t(1) << get_offset(pos); }

  #if defined(REVERSE_MODE) || defined(FLIP_MODE)
    size_t reverse(size_t pos) {
      return pos == npos? npos : num_bits - pos - 1;
    }
  #endif

  #ifdef FLIP_MODE
    void flip_recursive(size_t pos) {
      size_t next = find_next(pos);
      if (next != npos)
        flip_recursive(next);
      // do the flip for pos
      uint64_t block = get_word(pos), mask = get_mask(pos);
      uint64_t rBlock = get_word(reverse(pos)), rMask = get_mask(reverse(pos));
      // flip if asymmetrical
      if (!(bitvec[rBlock] & rMask)) {
        bitvec[block].fetch_and(~mask);
        size_t r_old = bitvec[rBlock];
        while (!bitvec[rBlock].compare_exchange_weak(
          r_old, r_old | rMask, std::memory_order_relaxed));
      }
    }
  #endif

  size_t right_most_bit(uint64_t w) const {
    // assert(w >= 1);
    return boost::integer_log2<uint64_t>(w & -w);
  }

  size_t left_most_bit(uint64_t w) const {
      return boost::integer_log2<uint64_t>(w);
  }

  size_t find_from_block(size_t first, bool fore=true) const {
    size_t i;
    if (fore) {
      for (i = first; i < bitvec.size() && bitvec[i] == 0; i++);
      if (i >= bitvec.size())
          return npos;
      return i * bits_uint64 + right_most_bit(bitvec[i]);
    }
    else {
      for (i = first; i > 0 && bitvec[i] == 0; i--);
      if (i <= 0 && bitvec[i] == 0)
        return npos;
      return i * bits_uint64 + left_most_bit(bitvec[i]);
    }
  }

public:
  typedef size_t iterator;
  typedef size_t reverse_iterator;

  //! sign for N/A
  static const size_t npos = std::numeric_limits<size_t>::max();

  #ifdef FLIP_MODE
    void flip() {
      size_t first = find_first();
      if (first != npos)
        flip_recursive(first);
    }
  #endif

  #ifdef USE_INDICATOR
  // Accessors
  size_t getIndicator() const { return indicator; }
  void setIndicator(size_t index) { indicator = index; }
  #endif

  // @DynamicBitSet
  #ifdef _GALOIS_DYNAMIC_BIT_SET_
  // using Base::size;
  #else
  size_t size() const { return num_bits; }
  #endif

  //! Constructor which initializes to an empty bitset.
  MRBCBitSet() {
    resize(numSourcesPerRound);
    #ifdef USE_INDICATOR
      indicator = npos;
    #endif
  }

  #ifdef _GALOIS_DYNAMIC_BIT_SET_
  // using Base::test;
  #else
  // @DynamicBitSet
  bool test(size_t index) const {
    size_t bit_index = get_word(index);
    uint64_t bit_mask = get_mask(index);
    return ((bitvec[bit_index] & bit_mask) != 0);
  }
  #endif

  /**
   * Test and then set
   *
   * @returns test result before set
   */
  bool test_set(size_t pos, bool val=true) {
    bool const ret = test(pos);
    if (ret != val) {
      uint64_t old_val = bitvec[get_word(pos)];
      if (val) {
        while (!bitvec[get_word(pos)].compare_exchange_weak(
          old_val, (old_val | get_mask(pos)),
          std::memory_order_relaxed));
      }
      else {
        while (!bitvec[get_word(pos)].compare_exchange_weak(
          old_val, (old_val & ~get_mask(pos)),
          std::memory_order_relaxed));
      }
    }
    return ret;
  }

  #ifdef _GALOIS_DYNAMIC_BIT_SET_
  // using Base::set;
  #else
  // @DynamicBitSet
  void set(size_t index) {
    size_t bit_index = get_word(index);
    uint64_t bit_mask = get_mask(index);
    if ((bitvec[bit_index] & bit_mask) == 0) { // test and set
      size_t old_val = bitvec[bit_index];
      while (!bitvec[bit_index].compare_exchange_weak(
          old_val, old_val | bit_mask,
          std::memory_order_relaxed));
    }
  }
  #endif

  // DISABLED @DynamicBitSet
  void reset(size_t index) {
    size_t bit_index = get_word(index);
    uint64_t bit_mask = get_mask(index);
    bitvec[bit_index].fetch_and(~bit_mask);
    // @ Better implementation:
    // while (!bitvec[bit_index].compare_exchange_weak(
    //   old_val, old_val & ~bit_mask,
    //   std::memory_order_relaxed));
  }

  #ifdef _GALOIS_DYNAMIC_BIT_SET_
  // using Base::reset;
  // using Base::resize;
  #else
  // @DynamicBitSet
  void reset() { std::fill(bitvec.begin(), bitvec.end(), uint64_t(0)); }

  // @DynamicBitSet
  void resize(uint64_t n) {
    assert(bits_uint64 == 64); // compatibility with other devices
    num_bits = n;
    bitvec.resize((n + bits_uint64 - 1) / bits_uint64);
    reset();
  }
  #endif

  bool none() {
    for (size_t i = 0; i < bitvec.size(); ++i)
      if (bitvec[i])
        return false;
    return true;
  }

  #ifdef USE_INDICATOR
    /**
     * Set a bit with the side-effect updating indicator to the first.
     */
    void set_indicator(size_t pos) {
      #ifdef REVERSE_MODE
        set(reverse(pos));
      #else
        set(pos);
      #endif
      if (pos < indicator) {
        indicator = pos;
      }
    }

    bool test_set_indicator(size_t pos, bool val=true) {
      #ifdef REVERSE_MODE
        if (test_set(reverse(pos), val)) {
          if (pos == indicator) {
            forward_indicator();
          }
          return true;
        }
        else
          return false;
      #else
        if (test_set(pos, val)) {
          if (pos == indicator) {
            forward_indicator();
          }
          return true;
        }
        else
          return false;
      #endif
    }

    /**
     * Return true if indicator is npos
     */
    bool nposInd() {
      return indicator == npos;
    }
  #endif
  /**
   * Returns: the lowest index i such as bit i is set, or npos if *this has no on bits.
   */
  size_t find_first() const {
    return find_from_block(0);
  }

  size_t find_last() const {
    return find_from_block(bitvec.size() - 1, false);
  }

  #ifdef REVERSE_MODE
    inline size_t begin() { return reverse(find_last()); }
  #else
    inline size_t begin() { return find_first(); }
  #endif
  inline size_t end() { return npos; }

  #if defined(REVERSE_MODE) || defined(FLIP_MODE)
    inline size_t rbegin() { return reverse(find_first()); }
  #else
    inline size_t rbegin() { return find_last(); }
  #endif
  inline size_t rend() { return npos; }

  /**
   * Returns: the lowest index i greater than pos such as bit i is set, or npos if no such index exists.
   */
  size_t find_next(size_t pos) const {
    if (pos == npos) {
      return find_first();
    }
    if (++pos >= size() || size() == 0) {
      return npos;
    }
    size_t curBlock = get_word(pos);
    auto curOffset = get_offset(pos);
    auto seg = bitvec[curBlock];
    while (seg != bitvec[curBlock])
      seg = bitvec[curBlock];
    uint64_t res = seg >> curOffset;
    return res?
      pos + right_most_bit(res) : find_from_block(++curBlock);
  }

  size_t find_prev(size_t pos) const{
    if (pos >= size()) {
      return find_last();
    }
    // Return npos if no bit set
    if (pos-- == 0 || size() == 0) {
      return npos;
    }
    size_t curBlock = get_word(pos);
    uint64_t res = bitvec[curBlock] & ((uint64_t(2) << get_offset(pos)) - 1);
    return res?
      curBlock * bits_uint64 + left_most_bit(res) :
      (curBlock?
        find_from_block(--curBlock, false) : npos);
  }

  /**
   * To move iterator to the previous set bit, and return the old value.
   */
  inline size_t forward_iterate(size_t& i) {
    size_t old = i;
    #ifdef REVERSE_MODE
      i = reverse(find_prev(reverse(i)));
    #else
      i = find_next(i);
    #endif
    return old;
  }

  /**
   * To move iterator to the next set bit.
   */
  inline size_t backward_iterate(size_t& i) {
    size_t old = i;
    #ifdef FLIP_MODE
      i = nposInd()? find_first() : find_next(i);
      return reverse(old);
    #else
      #ifdef REVERSE_MODE
      i = reverse(find_next(reverse(i)));
      #else
      i = find_prev(i);
      #endif
      return old;
    #endif
  }

  #ifdef USE_INDICATOR
    /**
     * To move indicator to the previous set bit, and return the old value.
     */
    size_t forward_indicator() {
      return forward_iterate(indicator);
    }

    /**
     * To move indicator to the next set bit.
     */
    size_t backward_indicator() {
      return backward_iterate(indicator);
    }
  #endif
};
#endif


/* Copyright (c) 2017, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The Astrobee platform is licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with the
 * License. You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations
 * under the License.
 */

#include <camera/camera_model.h>
#include <camera/camera_params.h>

#include <Eigen/Geometry>
#include <ceres/rotation.h>
#include <gtest/gtest.h>

#include <vector>
#include <string>

#define EXPECT_VECTOR3D_NEAR(p1, p2, t) EXPECT_NEAR(p1[0], p2[0], t); EXPECT_NEAR(p1[1], p2[1], t); \
  EXPECT_NEAR(p1[2], p2[2], t);

#define EXPECT_VECTOR2D_NEAR(p1, p2, t) EXPECT_NEAR(p1[0], p2[0], t); EXPECT_NEAR(p1[1], p2[1], t);

TEST(camera_model, angleaxis_transforms) {
  // Make an arbitrary rotation
  Eigen::Matrix3d R1 =
    Eigen::AngleAxisd(-.156 * M_PI,
                      Eigen::Vector3d(.14, -.56, .4).normalized()).matrix();
  Eigen::Matrix3d R2;
  Eigen::Vector3d rodrigues;

  camera::RotationToRodrigues(R1, &rodrigues);
  camera::RodriguesToRotation(rodrigues, &R2);

  for (int j = 0; j < 3; j++) {
    for (int i = 0; i < 3; i++) {
      EXPECT_NEAR(R1(j, i), R2(j, i), 1e-6);
    }
  }

  Eigen::Vector3d input(4, 5, 3);
  Eigen::Vector3d output2 = R1 * input;
  Eigen::Vector3d output1;
  ceres::AngleAxisRotatePoint(&rodrigues[0],
                              &input[0], &output1[0]);
  for (int i = 0; i < 3; i++)
    EXPECT_NEAR(output1[i], output2[i], 1e-6);
}

TEST(camera_model, camera) {
  std::string data_dir = std::string(TEST_DIR) + "/../../sparse_mapping/test/data/";
  camera::CameraParameters params(data_dir + "iss_tango_camera.xml");  // Ideally we would use a
                                                   // different file which
                                                   // represents an undistorted
                                                   // image.
                                                   // params.undistorted =
                                                   // true;
  params.SetDistortedSize(Eigen::Vector2i(640, 480));
  params.SetUndistortedSize(Eigen::Vector2i(640, 480));

  // Over-ride focal length for the tests below to pass
  params.SetFocalLength(Eigen::Vector2d::Constant(1.0 / (tan(M_PI_2 / 2) / 320)));

  Eigen::Vector3d pos(10, -10, 5);
  Eigen::Matrix3d rotation;
  rotation = Eigen::AngleAxisd(1.0 * M_PI, Eigen::Vector3d::UnitX())
             * Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitY())
             * Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitZ());
  camera::CameraModel camera(pos, rotation, M_PI_2, 640, 480);
  camera::CameraModel camera2(pos, rotation, params);

  // check that two constructors give same camera
  EXPECT_EQ(camera.GetPosition(), camera2.GetPosition());
  EXPECT_EQ(camera.GetRotation(), camera2.GetRotation());
  EXPECT_EQ(camera.GetTransform().matrix(), camera2.GetTransform().matrix());
  EXPECT_VECTOR2D_NEAR(camera.GetParameters().GetUndistortedSize(),
                       camera2.GetParameters().GetUndistortedSize(), 0);
  EXPECT_DOUBLE_EQ(camera.GetParameters().GetFocalLength(), camera2.GetParameters().GetFocalLength());

  // check that camera is as specified
  EXPECT_VECTOR3D_NEAR(camera.GetPosition(), pos, 1e-5);
  EXPECT_VECTOR2D_NEAR(camera.GetParameters().GetUndistortedSize(), params.GetUndistortedSize(), 0);
  EXPECT_DOUBLE_EQ(camera.GetParameters().GetFocalLength(),   params.GetFocalLength());

  // check that the center of rotation is origin of camera coordinates
  Eigen::Vector3d test_point(10, -10, 5);
  EXPECT_VECTOR3D_NEAR(camera.CameraCoordinates(test_point), Eigen::Vector3d(0, 0, 0), 1e-5);
  EXPECT_VECTOR3D_NEAR(camera.CameraCoordinates(test_point.x(), test_point.y(), test_point.z()),
                       Eigen::Vector3d(0, 0, 0), 1e-5);

  // check that a point at (0, 0, 1) in camera coordinates is in the center of the image
  EXPECT_VECTOR3D_NEAR(camera.CameraCoordinates(10, -11, 5), Eigen::Vector3d(0, 0, 1), 1e-5);
  EXPECT_VECTOR2D_NEAR(camera.ImageCoordinates(Eigen::Vector3d(10, -11, 5)), Eigen::Vector2d(0, 0), 1e-5);
  EXPECT_VECTOR2D_NEAR(camera.ImageCoordinates(10, -11, 5), Eigen::Vector2d(0, 0), 1e-5);
  // and in FOV
  EXPECT_TRUE(camera.IsInFov(10, -11, 5));
  // point behind camera isn't in FOV
  EXPECT_FALSE(camera.IsInFov(Eigen::Vector3d(10, -9, 5)));

  // check points on border and right beyond border of FOV
  Eigen::Vector3d p;
  Eigen::Vector2d t;

  p << 10, -15, 10-1e-5;
  t = camera.ImageCoordinates(p);
  EXPECT_NEAR(t.x(), -320, 1);
  EXPECT_NEAR(t.y(), 0, 1);
  EXPECT_TRUE(camera.IsInFov(p));
  EXPECT_FALSE(camera.IsInFov(p + Eigen::Vector3d(0, 0, 1e-1)));

  p << 10, -15, 0+1e-2;  // due to rounding, 1e-2
  t = camera.ImageCoordinates(p);
  EXPECT_NEAR(t.x(), 320, 1);
  EXPECT_NEAR(t.y(), 0, 1);
  EXPECT_TRUE(camera.IsInFov(p));
  EXPECT_FALSE(camera.IsInFov(p + Eigen::Vector3d(0, 0, -1e-1)));

  p << 6.25+1e-5, -15, 5;
  t = camera.ImageCoordinates(p);
  EXPECT_NEAR(t.x(), 0, 1);
  EXPECT_NEAR(t.y(), -240, 1);
  EXPECT_TRUE(camera.IsInFov(p));
  EXPECT_FALSE(camera.IsInFov(p + Eigen::Vector3d(-1e-1, 0, 0)));

  p << 13.75-1e-2, -15, 5;  // due to rounding, 1e-2
  t = camera.ImageCoordinates(p);
  EXPECT_NEAR(t.x(), 0, 1);
  EXPECT_NEAR(t.y(), 240, 1);
  EXPECT_TRUE(camera.IsInFov(p));
  EXPECT_FALSE(camera.IsInFov(p + Eigen::Vector3d(1e-1, 0, 1e-2)));

  // check SetTransform
  Eigen::Affine3d new_transform;
  new_transform.setIdentity();
  camera.SetTransform(new_transform);
  EXPECT_EQ(camera.CameraCoordinates(1, 2, 3), Eigen::Vector3d(1, 2, 3));
}

TEST(camera_model, DistortionCoeffs) {
  std::string data_dir = std::string(TEST_DIR) + "/../../sparse_mapping/test/data/";

  // Test with our three camera models
  std::vector<std::string> files {"p2_camera.xml", "iss_tango_camera.xml",
    "earth_tango_camera.xml", "iss_tango_undistorted.xml", "earth_tango_undistorted.xml"};
  for (std::string const& file : files) {
    camera::CameraParameters camera_params(data_dir + file);

    for (float x = -320; x < 320; x += 30) {
      for (float y = -240; y < 240; y += 30) {
        Eigen::Vector2d distort_location, loopback_location;
        camera_params.Convert<camera::UNDISTORTED_C, camera::DISTORTED_C>
          (Eigen::Vector2d(x, y), &distort_location);
        camera_params.Convert<camera::DISTORTED_C, camera::UNDISTORTED_C>
          (distort_location, &loopback_location);
        EXPECT_NEAR(x, loopback_location[0], 1e-4) << file;
        EXPECT_NEAR(y, loopback_location[1], 1e-4) << file;
      }
    }
  }

  // Verify that the optical center is not at the center of the image
  // in undistort location
  camera::CameraParameters camera_params(data_dir + "iss_tango_camera.xml");
  Eigen::Vector2d optical_center;
  camera_params.Convert<camera::UNDISTORTED_C, camera::DISTORTED_C>(Eigen::Vector2d::Zero(), &optical_center);
  EXPECT_NEAR(3.92, optical_center[0], 1e-3);
  EXPECT_NEAR(0.827, optical_center[1], 1e-3);
}


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

#include <boost/simd/arithmetic/functions/round2even.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/bitwise_xor.hpp>
#include <boost/simd/include/functions/simd/is_less.hpp>
#include <boost/simd/include/functions/simd/bitofsign.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/constants/twotonmb.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( round2even_
                                    , tag::cpu_, (A0)(X)
                                    , ((simd_<integer_<A0>,X>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return a0;
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( round2even_
                                    , boost::simd::tag::simd_
                                    , (A0)(X)
                                    , ((simd_<floating_<A0>,X>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      const result_type s   = bitofsign(a0);
      const result_type v   = a0 ^ s;
      const result_type t2n = boost::simd::Twotonmb<A0>();
      const result_type d0  = v+t2n;
      const result_type d   = d0-t2n;
      const result_type d1  = if_else(lt(v,t2n),d,v);
      return (d1^s);
    }
  };
} } }

#endif


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/
#include "itkGibbsTrackingFilter.h"

// MITK
#include <itkOrientationDistributionFunction.h>
#include <itkDiffusionQballGeneralizedFaImageFilter.h>
#include <mitkStandardFileLocations.h>
#include <mitkFiberBuilder.h>
#include <mitkMetropolisHastingsSampler.h>
//#include <mitkEnergyComputer.h>
#include <itkTensorImageToQBallImageFilter.h>
#include <mitkGibbsEnergyComputer.h>

// ITK
#include <itkImageDuplicator.h>
#include <itkResampleImageFilter.h>
#include <itkTimeProbe.h>

// MISC
#include <fstream>
// #include <QFile>
#include <tinyxml.h>
#include <math.h>
#include <boost/progress.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string.hpp>

namespace itk{

template< class ItkQBallImageType >
GibbsTrackingFilter< ItkQBallImageType >::GibbsTrackingFilter():
    m_StartTemperature(0.1),
    m_EndTemperature(0.001),
    m_Iterations(500000),
    m_ParticleWeight(0),
    m_ParticleWidth(0),
    m_ParticleLength(0),
    m_ConnectionPotential(10),
    m_InexBalance(0),
    m_ParticlePotential(0.2),
    m_MinFiberLength(10),
    m_AbortTracking(false),
    m_NumConnections(0),
    m_NumParticles(0),
    m_NumAcceptedFibers(0),
    m_CurrentStep(0),
    m_BuildFibers(false),
    m_Steps(10),
    m_ProposalAcceptance(0),
    m_CurvatureThreshold(0.7),
    m_DuplicateImage(true),
    m_RandomSeed(-1),
    m_LoadParameterFile(""),
    m_LutPath(""),
    m_IsInValidState(true)
{

}

template< class ItkQBallImageType >
GibbsTrackingFilter< ItkQBallImageType >::~GibbsTrackingFilter()
{

}

// fill output fiber bundle datastructure
template< class ItkQBallImageType >
typename GibbsTrackingFilter< ItkQBallImageType >::FiberPolyDataType GibbsTrackingFilter< ItkQBallImageType >::GetFiberBundle()
{
    if (!m_AbortTracking)
    {
        m_BuildFibers = true;
        while (m_BuildFibers){}
    }

    return m_FiberPolyData;
}

template< class ItkQBallImageType >
void
GibbsTrackingFilter< ItkQBallImageType >
::EstimateParticleWeight()
{
    MITK_INFO << "GibbsTrackingFilter: estimating particle weight";

    float minSpacing;
    if(m_QBallImage->GetSpacing()[0]<m_QBallImage->GetSpacing()[1] && m_QBallImage->GetSpacing()[0]<m_QBallImage->GetSpacing()[2])
        minSpacing = m_QBallImage->GetSpacing()[0];
    else if (m_QBallImage->GetSpacing()[1] < m_QBallImage->GetSpacing()[2])
        minSpacing = m_QBallImage->GetSpacing()[1];
    else
        minSpacing = m_QBallImage->GetSpacing()[2];
    float m_ParticleLength = 1.5*minSpacing;
    float m_ParticleWidth = 0.5*minSpacing;

    // seed random generators
    Statistics::MersenneTwisterRandomVariateGenerator::Pointer randGen = Statistics::MersenneTwisterRandomVariateGenerator::New();
    if (m_RandomSeed>-1)
        randGen->SetSeed(m_RandomSeed);
    else
        randGen->SetSeed();

    // instantiate all necessary components
    SphereInterpolator* interpolator = new SphereInterpolator(m_LutPath);
    // handle lookup table not found cases
    if( !interpolator->IsInValidState() )
    {
      m_IsInValidState = false;
      m_AbortTracking = true;
      m_BuildFibers = false;
      mitkThrow() << "Unable to load lookup tables.";
    }
    ParticleGrid* particleGrid = new ParticleGrid(m_MaskImage, m_ParticleLength, m_ParticleGridCellCapacity);
    GibbsEnergyComputer* encomp = new GibbsEnergyComputer(m_QBallImage, m_MaskImage, particleGrid, interpolator, randGen);

    //    EnergyComputer* encomp = new EnergyComputer(m_QBallImage, m_MaskImage, particleGrid, interpolator, randGen);
    MetropolisHastingsSampler* sampler = new MetropolisHastingsSampler(particleGrid, encomp, randGen, m_CurvatureThreshold);

    float alpha = log(m_EndTemperature/m_StartTemperature);
    m_ParticleWeight = 0.01;
    int ppv = 0;
    // main loop
    int neededParts = 3000;
    while (ppv<neededParts)
    {
        if (ppv<1000)
            m_ParticleWeight /= 2;
        else
            m_ParticleWeight = ppv*m_ParticleWeight/neededParts;

        encomp->SetParameters(m_ParticleWeight,m_ParticleWidth,m_ConnectionPotential*m_ParticleLength*m_ParticleLength,m_CurvatureThreshold,m_InexBalance,m_ParticlePotential);
        for( int step = 0; step < 10; step++ )
        {
            // update temperatur for simulated annealing process
            float temperature = m_StartTemperature * exp(alpha*(((1.0)*step)/((1.0)*10)));
            sampler->SetTemperature(temperature);

            for (unsigned long i=0; i<10000; i++)
                sampler->MakeProposal();
        }
        ppv = particleGrid->m_NumParticles;
        particleGrid->ResetGrid();
    }
    delete sampler;
    delete encomp;
    delete particleGrid;
    delete interpolator;

    MITK_INFO << "GibbsTrackingFilter: finished estimating particle weight";
}

// perform global tracking
template< class ItkQBallImageType >
void GibbsTrackingFilter< ItkQBallImageType >::GenerateData()
{
    TimeProbe preClock; preClock.Start();
    // check if input is qball or tensor image and generate qball if necessary
    if (m_QBallImage.IsNull() && m_TensorImage.IsNotNull())
    {
        TensorImageToQBallImageFilter<float,float>::Pointer filter = TensorImageToQBallImageFilter<float,float>::New();
        filter->SetInput( m_TensorImage );
        filter->Update();
        m_QBallImage = filter->GetOutput();
    }
    else if (m_DuplicateImage) // generate local working copy of QBall image (if not disabled)
    {
        typedef itk::ImageDuplicator< ItkQBallImageType > DuplicateFilterType;
        typename DuplicateFilterType::Pointer duplicator = DuplicateFilterType::New();
        duplicator->SetInputImage( m_QBallImage );
        duplicator->Update();
        m_QBallImage = duplicator->GetOutput();
    }

    // perform mean subtraction on odfs
    typedef ImageRegionIterator< ItkQBallImageType > InputIteratorType;
    InputIteratorType it(m_QBallImage, m_QBallImage->GetLargestPossibleRegion() );
    it.GoToBegin();
    while (!it.IsAtEnd())
    {
        itk::OrientationDistributionFunction<float, QBALL_ODFSIZE> odf(it.Get().GetDataPointer());
        float mean = odf.GetMeanValue();
        odf -= mean;
        it.Set(odf.GetDataPointer());
        ++it;
    }

    // check if mask image is given if it needs resampling
    PrepareMaskImage();

    // load parameter file
    LoadParameters();

    // prepare parameters
    float minSpacing;
    if(m_QBallImage->GetSpacing()[0]<m_QBallImage->GetSpacing()[1] && m_QBallImage->GetSpacing()[0]<m_QBallImage->GetSpacing()[2])
        minSpacing = m_QBallImage->GetSpacing()[0];
    else if (m_QBallImage->GetSpacing()[1] < m_QBallImage->GetSpacing()[2])
        minSpacing = m_QBallImage->GetSpacing()[1];
    else
        minSpacing = m_QBallImage->GetSpacing()[2];

    if(m_ParticleLength == 0)
        m_ParticleLength = 1.5*minSpacing;
    if(m_ParticleWidth == 0)
        m_ParticleWidth = 0.5*minSpacing;

    if(m_ParticleWeight == 0)
        EstimateParticleWeight();

    float alpha = log(m_EndTemperature/m_StartTemperature);
    m_Steps = m_Iterations/10000;
    if (m_Steps<10)
        m_Steps = 10;
    if (m_Steps>m_Iterations)
    {
        MITK_INFO << "GibbsTrackingFilter: not enough iterations!";
        m_AbortTracking = true;
    }
    if (m_CurvatureThreshold < mitk::eps)
        m_CurvatureThreshold = 0;
    unsigned long singleIts = (unsigned long)((1.0*m_Iterations) / (1.0*m_Steps));

    // seed random generators
    Statistics::MersenneTwisterRandomVariateGenerator::Pointer randGen = Statistics::MersenneTwisterRandomVariateGenerator::New();
    if (m_RandomSeed>-1)
        randGen->SetSeed(m_RandomSeed);
    else
        randGen->SetSeed();

    // load sphere interpolator to evaluate the ODFs
    SphereInterpolator* interpolator = new SphereInterpolator(m_LutPath);

    // handle lookup table not found cases
    if( !interpolator->IsInValidState() )
    {
      m_IsInValidState = false;
      m_AbortTracking = true;
      m_BuildFibers = false;
      mitkThrow() << "Unable to load lookup tables.";
    }
    // initialize the actual tracking components (ParticleGrid, Metropolis Hastings Sampler and Energy Computer)
    ParticleGrid* particleGrid;
    GibbsEnergyComputer* encomp;
    MetropolisHastingsSampler* sampler;
    try{
        particleGrid = new ParticleGrid(m_MaskImage, m_ParticleLength, m_ParticleGridCellCapacity);
        encomp = new GibbsEnergyComputer(m_QBallImage, m_MaskImage, particleGrid, interpolator, randGen);
        encomp->SetParameters(m_ParticleWeight,m_ParticleWidth,m_ConnectionPotential*m_ParticleLength*m_ParticleLength,m_CurvatureThreshold,m_InexBalance,m_ParticlePotential);
        sampler = new MetropolisHastingsSampler(particleGrid, encomp, randGen, m_CurvatureThreshold);
    }
    catch(...)
    {
        MITK_ERROR  << "Particle grid allocation failed. Not enough memory? Try to increase the particle length.";
        m_IsInValidState = false;
        m_AbortTracking = true;
        m_BuildFibers = false;
        return;
    }

    MITK_INFO << "----------------------------------------";
    MITK_INFO << "Iterations: " << m_Iterations;
    MITK_INFO << "Steps: " << m_Steps;
    MITK_INFO << "Particle length: " << m_ParticleLength;
    MITK_INFO << "Particle width: " << m_ParticleWidth;
    MITK_INFO << "Particle weight: " << m_ParticleWeight;
    MITK_INFO << "Start temperature: " << m_StartTemperature;
    MITK_INFO << "End temperature: " << m_EndTemperature;
    MITK_INFO << "In/Ex balance: " << m_InexBalance;
    MITK_INFO << "Min. fiber length: " << m_MinFiberLength;
    MITK_INFO << "Curvature threshold: " << m_CurvatureThreshold;
    MITK_INFO << "Random seed: " << m_RandomSeed;
    MITK_INFO << "----------------------------------------";

    // main loop
    preClock.Stop();
    TimeProbe clock; clock.Start();
    m_NumAcceptedFibers = 0;
    unsigned long counter = 1;

    boost::progress_display disp(m_Steps*singleIts);
    if (!m_AbortTracking)
    for( m_CurrentStep = 1; m_CurrentStep <= m_Steps; m_CurrentStep++ )
    {
        // update temperatur for simulated annealing process
        float temperature = m_StartTemperature * exp(alpha*(((1.0)*m_CurrentStep)/((1.0)*m_Steps)));
        sampler->SetTemperature(temperature);

        for (unsigned long i=0; i<singleIts; i++)
        {
            ++disp;
            if (m_AbortTracking)
                break;

            sampler->MakeProposal();

            if (m_BuildFibers || (i==singleIts-1 && m_CurrentStep==m_Steps))
            {
                m_ProposalAcceptance = (float)sampler->GetNumAcceptedProposals()/counter;
                m_NumParticles = particleGrid->m_NumParticles;
                m_NumConnections = particleGrid->m_NumConnections;

                FiberBuilder fiberBuilder(particleGrid, m_MaskImage);
                m_FiberPolyData = fiberBuilder.iterate(m_MinFiberLength);
                m_NumAcceptedFibers = m_FiberPolyData->GetNumberOfLines();
                m_BuildFibers = false;
            }
            counter++;
        }

        m_ProposalAcceptance = (float)sampler->GetNumAcceptedProposals()/counter;
        m_NumParticles = particleGrid->m_NumParticles;
        m_NumConnections = particleGrid->m_NumConnections;

        if (m_AbortTracking)
            break;
    }
    if (m_AbortTracking)
    {
        FiberBuilder fiberBuilder(particleGrid, m_MaskImage);
        m_FiberPolyData = fiberBuilder.iterate(m_MinFiberLength);
        m_NumAcceptedFibers = m_FiberPolyData->GetNumberOfLines();
    }
    clock.Stop();

    delete sampler;
    delete encomp;
    delete interpolator;
    delete particleGrid;
    m_AbortTracking = true;
    m_BuildFibers = false;

    int h = clock.GetTotal()/3600;
    int m = ((int)clock.GetTotal()%3600)/60;
    int s = (int)clock.GetTotal()%60;
    MITK_INFO << "GibbsTrackingFilter: finished gibbs tracking in " << h << "h, " << m << "m and " << s << "s";
    m = (int)preClock.GetTotal()/60;
    s = (int)preClock.GetTotal()%60;
    MITK_INFO << "GibbsTrackingFilter: preparation of the data took " << m << "m and " << s << "s";
    MITK_INFO << "GibbsTrackingFilter: " << m_NumAcceptedFibers << " fibers accepted";

    SaveParameters();
}

template< class ItkQBallImageType >
void GibbsTrackingFilter< ItkQBallImageType >::PrepareMaskImage()
{
    if(m_MaskImage.IsNull())
    {
        MITK_INFO << "GibbsTrackingFilter: generating default mask image";
        m_MaskImage = ItkFloatImageType::New();
        m_MaskImage->SetSpacing( m_QBallImage->GetSpacing() );
        m_MaskImage->SetOrigin( m_QBallImage->GetOrigin() );
        m_MaskImage->SetDirection( m_QBallImage->GetDirection() );
        m_MaskImage->SetRegions( m_QBallImage->GetLargestPossibleRegion() );
        m_MaskImage->Allocate();
        m_MaskImage->FillBuffer(1.0);
    }
    else if ( m_MaskImage->GetLargestPossibleRegion().GetSize()[0]!=m_QBallImage->GetLargestPossibleRegion().GetSize()[0] ||
         m_MaskImage->GetLargestPossibleRegion().GetSize()[1]!=m_QBallImage->GetLargestPossibleRegion().GetSize()[1] ||
         m_MaskImage->GetLargestPossibleRegion().GetSize()[2]!=m_QBallImage->GetLargestPossibleRegion().GetSize()[2] ||
         m_MaskImage->GetSpacing()[0]!=m_QBallImage->GetSpacing()[0] ||
         m_MaskImage->GetSpacing()[1]!=m_QBallImage->GetSpacing()[1] ||
         m_MaskImage->GetSpacing()[2]!=m_QBallImage->GetSpacing()[2] )
    {
        MITK_INFO << "GibbsTrackingFilter: resampling mask image";
        typedef itk::ResampleImageFilter< ItkFloatImageType, ItkFloatImageType, float > ResamplerType;
        ResamplerType::Pointer resampler = ResamplerType::New();
        resampler->SetOutputSpacing( m_QBallImage->GetSpacing() );
        resampler->SetOutputOrigin( m_QBallImage->GetOrigin() );
        resampler->SetOutputDirection( m_QBallImage->GetDirection() );
        resampler->SetSize( m_QBallImage->GetLargestPossibleRegion().GetSize() );

        resampler->SetInput( m_MaskImage );
        resampler->SetDefaultPixelValue(0.0);
        resampler->Update();
        m_MaskImage = resampler->GetOutput();
        MITK_INFO << "GibbsTrackingFilter: resampling finished";
    }
}

// load tracking paramters from xml file (.gtp)
template< class ItkQBallImageType >
bool GibbsTrackingFilter< ItkQBallImageType >::LoadParameters()
{
    m_AbortTracking = true;
    try
    {
        if( m_LoadParameterFile.length()==0 )
        {
            m_AbortTracking = false;
            return true;
        }

        MITK_INFO << "GibbsTrackingFilter: loading parameter file " << m_LoadParameterFile;

        TiXmlDocument doc( m_LoadParameterFile );
        doc.LoadFile();

        TiXmlHandle hDoc(&doc);
        TiXmlElement* pElem;
        TiXmlHandle hRoot(0);

        pElem = hDoc.FirstChildElement().Element();
        hRoot = TiXmlHandle(pElem);
        pElem = hRoot.FirstChildElement("parameter_set").Element();

        string iterations(pElem->Attribute("iterations"));
        m_Iterations = boost::lexical_cast<unsigned long>(iterations);

        string particleLength(pElem->Attribute("particle_length"));
        m_ParticleLength = boost::lexical_cast<float>(particleLength);

        string particleWidth(pElem->Attribute("particle_width"));
        m_ParticleWidth = boost::lexical_cast<float>(particleWidth);

        string partWeight(pElem->Attribute("particle_weight"));
        m_ParticleWeight = boost::lexical_cast<float>(partWeight);

        string startTemp(pElem->Attribute("temp_start"));
        m_StartTemperature = boost::lexical_cast<float>(startTemp);

        string endTemp(pElem->Attribute("temp_end"));
        m_EndTemperature = boost::lexical_cast<float>(endTemp);

        string inExBalance(pElem->Attribute("inexbalance"));
        m_InexBalance = boost::lexical_cast<float>(inExBalance);

        string fiberLength(pElem->Attribute("fiber_length"));
        m_MinFiberLength = boost::lexical_cast<float>(fiberLength);

        string curvThres(pElem->Attribute("curvature_threshold"));
        m_CurvatureThreshold = cos(boost::lexical_cast<float>(curvThres)*M_PI/180);
        m_AbortTracking = false;
        MITK_INFO << "GibbsTrackingFilter: parameter file loaded successfully";
        return true;
    }
    catch(...)
    {
        MITK_INFO << "GibbsTrackingFilter: could not load parameter file";
        return false;
    }
}

// save current tracking paramters to xml file (.gtp)
template< class ItkQBallImageType >
bool GibbsTrackingFilter< ItkQBallImageType >::SaveParameters()
{
    try
    {
        if( m_SaveParameterFile.length()==0 )
        {
            MITK_INFO << "GibbsTrackingFilter: no filename specified to save parameters";
            return true;
        }

        MITK_INFO << "GibbsTrackingFilter: saving parameter file " << m_SaveParameterFile;

        TiXmlDocument documentXML;
        TiXmlDeclaration* declXML = new TiXmlDeclaration( "1.0", "", "" );
        documentXML.LinkEndChild( declXML );

        TiXmlElement* mainXML = new TiXmlElement("global_tracking_parameter_file");
        mainXML->SetAttribute("file_version",  "0.1");
        documentXML.LinkEndChild(mainXML);

        TiXmlElement* paramXML = new TiXmlElement("parameter_set");
        paramXML->SetAttribute("iterations", boost::lexical_cast<string>(m_Iterations));
        paramXML->SetAttribute("particle_length", boost::lexical_cast<string>(m_ParticleLength));
        paramXML->SetAttribute("particle_width", boost::lexical_cast<string>(m_ParticleWidth));
        paramXML->SetAttribute("particle_weight", boost::lexical_cast<string>(m_ParticleWeight));
        paramXML->SetAttribute("temp_start", boost::lexical_cast<string>(m_StartTemperature));
        paramXML->SetAttribute("temp_end", boost::lexical_cast<string>(m_EndTemperature));
        paramXML->SetAttribute("inexbalance", boost::lexical_cast<string>(m_InexBalance));
        paramXML->SetAttribute("fiber_length", boost::lexical_cast<string>(m_MinFiberLength));
        paramXML->SetAttribute("curvature_threshold", boost::lexical_cast<string>(m_CurvatureThreshold));
        mainXML->LinkEndChild(paramXML);

        if(!boost::algorithm::ends_with(m_SaveParameterFile, ".gtp"))
            m_SaveParameterFile.append(".gtp");
        documentXML.SaveFile( m_SaveParameterFile );

        MITK_INFO << "GibbsTrackingFilter: parameter file saved successfully";
        return true;
    }
    catch(...)
    {
        MITK_INFO << "GibbsTrackingFilter: could not save parameter file";
        return false;
    }
}

}


/*
 * Plane.hpp
 *
 *  Created on: Oct 17, 2014
 *      Author: Péter Fankhauser
 *   Institute: ETH Zurich, Autonomous Systems Lab
 */

#pragma once

#include "foothold_finder/VectorAverage.hpp"
#include "foothold_finder/NormalVectorAverage.hpp"

// Eigen
#include <Eigen/Core>

namespace foothold_finder {

/*!
 * Plane parameterization and handling of averages.
 */
class Plane
{
 public:
  /*!
   * Constructor.
   */
  Plane();

  /*!
   * Destructor
   */
  virtual ~Plane();

  /*!
   * Set the plane parameters.
   * @param position
   * @param normal
   */
  void set(const Eigen::Vector3d& position, const Eigen::Vector3d& normal);

  /*!
   * Average the current plane parameters with new values.
   * @param position the position to be added to the average.
   * @param normal the normal to be added to the average.
   */
  void averageWith(const Eigen::Vector3d& position, const Eigen::Vector3d& normal);

  /*!
   * Get position of the plane.
   * @return the position (centroid) of the plane.
   */
  const Eigen::Vector3d& getPosition() const;

  /*!
   * Get surface normal of the plane.
   * @return the surface normal.
   */
  const Eigen::Vector3d& getNormal();

  /*!
   * Get height (z-coordinate) at position.
   * @param[in] position the position to get the height at.
   * @return the height.
   */
  double getHeight(const Eigen::Vector2d& position);

  /*!
   * Clears the data and averages.
   */
  void clear();

 private:

  //! Position of the centroid.
  VectorAverage positionAverage_;

  //! Surface normal.
  NormalVectorAverage normalAverage_;

  //! If normal average is up to date.
  bool isNormalAverageCurrent_;

  //! Surface normal for caching reasons.
  Eigen::Vector3d normal_;
};

} /* namespace foothold_finder */


#pragma once

#include <polyfem/ElementBases.hpp>
#include <polyfem/Mesh2D.hpp>
#include <polyfem/LocalBoundary.hpp>
#include <polyfem/InterfaceData.hpp>
#include <polyfem/Navigation.hpp>

#include <polyfem/MeshNodes.hpp>

#include <Eigen/Dense>
#include <vector>

namespace polyfem
{
	class FEBasis2d
	{
	public:

		///
		/// @brief      Builds FE basis functions over the entire mesh (P1, P2 over triangles, Q1,
		///             Q2 over quads). Polygonal facets with > 4 vertices are dealt later on by the
		///             PolygonalBasis2d class.
		///
		/// @param[in]  mesh               The input surface mesh
		/// @param[in]  quadrature_order   The quadrature order
		/// @param[in]  discr_order        The order of the elements (1 or 2)
		/// @param[out] bases              List of basis functions per element
		/// @param[out] local_boundary     List of descriptor per element, indicating which edge of
		///                                the canonical elements lie on the boundary of the mesh
		/// @param[out] poly_edge_to_data  Data for edges at the interface with a polygon (used to
		///                                build the harmonics inside polygons)
		///
		/// @return     The number of basis functions created.
		///
		static int build_bases(
			const Mesh2D &mesh,
			const int quadrature_order,
			const int discr_order,
			const bool serendipity,
			const bool has_polys,
			const bool is_geom_bases,
			std::vector<ElementBases> &bases,
			std::vector<LocalBoundary> &local_boundary,
			std::map<int, InterfaceData> &poly_edge_to_data);

		static int build_bases(
			const Mesh2D &mesh,
			const int quadrature_order,
			const Eigen::VectorXi &discr_order,
			const bool serendipity,
			const bool has_polys,
			const bool is_geom_bases,
			std::vector<ElementBases> &bases,
			std::vector<LocalBoundary> &local_boundary,
			std::map<int, InterfaceData> &poly_edge_to_data);

		static Eigen::VectorXi tri_edge_local_nodes(const int p, const Mesh2D &mesh, Navigation::Index index);
		static Eigen::VectorXi quad_edge_local_nodes(const int q, const Mesh2D &mesh, Navigation::Index index);
	};
}


//////////////////////////////////////////////////////////////////////////////
// data::unit::estimation::units::cross_validation.hpp						//
//                                                                          //
//                                                                          //
//  (C) Copyright 2010 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DATA_UNIT_ESTIMATION_UNITS_CROSS_VALIDATION_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_DATA_UNIT_ESTIMATION_UNITS_CROSS_VALIDATION_HPP_ER_2010
#include <boost/statistics/detail/data/field/estimation/fields/estimate.hpp>
#include <boost/statistics/detail/data/field/estimation/fields/input.hpp>
#include <boost/statistics/detail/data/field/estimation/fields/output.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace data{
namespace unit{
namespace estimation{

    typeded data::unit<
        field::float_valued::estimate,
        field::float_valued::input,
        field::float_valued::output
    > cross_validation;

}// estimation
}// unit
}// data
}// detail
}// statistics
}// boost

#endif


#include "hoCuNDArray_utils.h"
#include "radial_utilities.h"
#include "hoNDArray_fileio.h"
#include "cuNDArray.h"
#include "imageOperator.h"
#include "identityOperator.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuConebeamProjectionOperator.h"
#include "cuConvolutionOperator.h"
#include "hoCuNDArray_blas.h"
#include "hoCuNDArray_elemwise.h"
#include "hoCuNDArray_blas.h"
#include "cgSolver.h"
#include "CBCT_acquisition.h"
#include "complext.h"
#include "encodingOperatorContainer.h"
#include "vector_td_io.h"
#include "hoCuPartialDerivativeOperator.h"
#include "hoCuTvOperator.h"
#include "hoCuTvPicsOperator.h"
#include "hoCuSbcCgSolver.h"
#include "hoNDArray_utils.h"
#include "GPUTimer.h"

#include <iostream>
#include <algorithm>
#include <sstream>
#include <math_constants.h>
#include <boost/program_options.hpp>

using namespace std;
using namespace Gadgetron;

namespace po = boost::program_options;

class mySbcCgSolver : public hoCuSbcCgSolver<float>
{
public:
    mySbcCgSolver() : hoCuSbcCgSolver<float>() {
        this->dumpFreq_ = 5;
        this->counter_ = 1;
    }
    ~mySbcCgSolver() {}

  virtual bool post_linear_solver_callback( hoCuNDArray<float> *u ) {
    printf("Iteration: %d\n", counter_);
    if( (counter_ % dumpFreq_) == 0 ){
      printf("Dumping frame\n");
      char filename[19];
      sprintf(filename, "img-itr-%04i.real", counter_);
      write_nd_array<float>(u, filename);
    }
    counter_++;
    return true;
  }
  void set_dump_frequency(unsigned int dumpFreq) {
    if( dumpFreq == 0 )
      this->dumpFreq_ = 9999999; // Not sure how modulus 0 behaves, so just make it a large number that is never reached...
    else
      this->dumpFreq_ = dumpFreq;
  }
protected:
  unsigned int counter_;
  unsigned int dumpFreq_;
};

int main(int argc, char** argv)
{
	string acquisition_filename;
	string outputFile;
	uintd3 imageSize;
	floatd3 voxelSize;
	int device;
  unsigned int dump;
	unsigned int downsamples;
	unsigned int iterations;
	unsigned int inner_iterations;
	float non_negativity_weight;
	float mu;

	po::options_description desc("Allowed options");
	desc.add_options()
    ("help", "produce help message")
    ("acquisition,a", po::value<string>(&acquisition_filename)->default_value("acquisition.hdf5"), "Acquisition data")
    ("samples,n",po::value<unsigned int>(),"Number of samples per ray")
    ("output,f", po::value<string>(&outputFile)->default_value("reconstruction.real"), "Output filename")
    ("size,s",po::value<uintd3>(&imageSize)->default_value(uintd3(512,512,1)),"Image size in pixels")
    ("binning,b",po::value<string>(),"Binning file for 4d reconstruction")
    ("SAG","Use exact SAG correction if present")
    ("voxelSize,v",po::value<floatd3>(&voxelSize)->default_value(floatd3(0.488f,0.488f,1.0f)),"Voxel size in mm")
    ("dimensions,d",po::value<floatd3>(),"Image dimensions in mm. Overwrites voxelSize.")
    ("iterations,i",po::value<unsigned int>(&iterations)->default_value(10),"Number of iterations")
    ("inner-iterations",po::value<unsigned int>(&inner_iterations)->default_value(5),"Number of iterations in the inner solver")
    ("TV,T",po::value<float>(),"TV Weight ")
	  ("mu",po::value<float>(&mu)->default_value(1),"Mu weight for encoding operator")
    ("non-negativity,N",po::value<float>(&non_negativity_weight)->default_value(1.0f),"Weight for the non-negativity (soft) constraint ")
    ("prior", po::value<std::string>(),"Prior image filename")
    ("PICCS",po::value<float>(),"TV Weight of the prior image (Prior image constrained compressed sensing)")
    ("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
    ("dump",po::value<unsigned int>(&dump)->default_value(0),"Dump image every N iterations")
    ("downsample,D",po::value<unsigned int>(&downsamples)->default_value(0),"Downsample projections this factor")
    ;

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

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}
	std::cout << "Command line options:" << std::endl;
	for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
		boost::any a = it->second.value();
		std::cout << it->first << ": ";
		if (a.type() == typeid(std::string)) std::cout << it->second.as<std::string>();
		else if (a.type() == typeid(int)) std::cout << it->second.as<int>();
		else if (a.type() == typeid(unsigned int)) std::cout << it->second.as<unsigned int>();
		else if (a.type() == typeid(float)) std::cout << it->second.as<float>();
		else if (a.type() == typeid(vector_td<float,3>)) std::cout << it->second.as<vector_td<float,3> >();
		else if (a.type() == typeid(vector_td<int,3>)) std::cout << it->second.as<vector_td<int,3> >();
		else if (a.type() == typeid(vector_td<unsigned int,3>)) std::cout << it->second.as<vector_td<unsigned int,3> >();
		else std::cout << "Unknown type" << std::endl;
		std::cout << std::endl;
	}
	cudaSetDevice(device);
	cudaDeviceReset();

	//Really weird stuff. Needed to initialize the device?? Should find real bug.
	cudaDeviceManager::Instance()->lockHandle();
	cudaDeviceManager::Instance()->unlockHandle();

	boost::shared_ptr<CBCT_acquisition> ps(new CBCT_acquisition());
	ps->load(acquisition_filename);
	ps->get_geometry()->print(std::cout);
	ps->downsample(downsamples);

	float SDD = ps->get_geometry()->get_SDD();
	float SAD = ps->get_geometry()->get_SAD();

	boost::shared_ptr<CBCT_binning> binning(new CBCT_binning());
	if (vm.count("binning")){
		std::cout << "Loading binning data" << std::endl;
		binning->load(vm["binning"].as<string>());
	} else
    binning->set_as_default_3d_bin(ps->get_projections()->get_size(2));

	binning->print(std::cout);

	floatd3 imageDimensions;
	if (vm.count("dimensions")){
		imageDimensions = vm["dimensions"].as<floatd3>();
		voxelSize = imageDimensions/imageSize;
	}
	else imageDimensions = voxelSize*imageSize;

	float lengthOfRay_in_mm = norm(imageDimensions);
	unsigned int numSamplesPerPixel = 3;
	float minSpacing = min(voxelSize)/numSamplesPerPixel;

	unsigned int numSamplesPerRay;
	if (vm.count("samples")) numSamplesPerRay = vm["samples"].as<unsigned int>();
	else numSamplesPerRay = ceil( lengthOfRay_in_mm / minSpacing );

	float step_size_in_mm = lengthOfRay_in_mm / numSamplesPerRay;
	size_t numProjs = ps->get_projections()->get_size(2);
	size_t needed_bytes = 2 * prod(imageSize) * sizeof(float);

	std::vector<size_t> is_dims = to_std_vector((uint64d3)imageSize);
	std::cout << "IS dimensions " << is_dims[0] << " " << is_dims[1] << " " << is_dims[2] << std::endl;
	std::cout << "Image size " << imageDimensions << std::endl;

	is_dims.push_back(binning->get_number_of_bins());

	hoCuNDArray<float> projections(*ps->get_projections());

	// Define encoding matrix
	boost::shared_ptr< hoCuConebeamProjectionOperator >
    E( new hoCuConebeamProjectionOperator() );
	E->set_weight(mu);
	E->setup(ps,binning,imageDimensions);
	E->set_domain_dimensions(&is_dims);
	E->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());
	//E->set_use_offset_correction(false);

	if (E->get_use_offset_correction())
	  E->offset_correct(&projections);

	mySbcCgSolver solver;
	solver.set_normalization_mode(mySbcCgSolver::SB_NO_NORMALIZATION);

	solver.set_encoding_operator(E);
	solver.set_max_outer_iterations(iterations);
	solver.get_inner_solver()->set_max_iterations(inner_iterations);
	solver.get_inner_solver()->set_tc_tolerance(1e-6);
  solver.get_inner_solver()->set_output_mode(hoCuCgSolver<float>::OUTPUT_VERBOSE);
	solver.set_non_negativity_filter(non_negativity_weight);
	solver.set_output_mode(hoCuSbcCgSolver<float>::OUTPUT_VERBOSE);
  solver.set_dump_frequency(dump);

	if (vm.count("TV")){
		boost::shared_ptr<hoCuPartialDerivativeOperator<float,4> > dx (new hoCuPartialDerivativeOperator<float,4>(0) );
		boost::shared_ptr<hoCuPartialDerivativeOperator<float,4> > dy (new hoCuPartialDerivativeOperator<float,4>(1) );
		boost::shared_ptr<hoCuPartialDerivativeOperator<float,4> > dz (new hoCuPartialDerivativeOperator<float,4>(2) );
		boost::shared_ptr<hoCuPartialDerivativeOperator<float,4> > dt (new hoCuPartialDerivativeOperator<float,4>(3) );

		dx->set_codomain_dimensions(&is_dims);
		dy->set_codomain_dimensions(&is_dims);
		dz->set_codomain_dimensions(&is_dims);
		dt->set_codomain_dimensions(&is_dims);

		dx->set_domain_dimensions(&is_dims);
		dy->set_domain_dimensions(&is_dims);
		dz->set_domain_dimensions(&is_dims);
		dt->set_domain_dimensions(&is_dims);

		dx->set_weight(vm["TV"].as<float>());
		dy->set_weight(vm["TV"].as<float>());
		dz->set_weight(vm["TV"].as<float>());
		dt->set_weight(vm["TV"].as<float>());

		solver.add_regularization_group_operator(dx);
		solver.add_regularization_group_operator(dy);
		solver.add_regularization_group_operator(dz);
		//solver.add_regularization_group_operator(dt);
		solver.add_group(1);
		solver.add_regularization_operator(dt,1);
	}

	if (vm.count("PICCS")){
		std::cout << "PICCS in used" << std::endl;
		CBCT_binning *binning_pics = new CBCT_binning();
		binning_pics->set_as_default_3d_bin(ps->get_projections()->get_size(2));
		std::vector<size_t> is_dims3d = to_std_vector((uint64d3)imageSize);
		boost::shared_ptr< hoCuConebeamProjectionOperator >
		Ep( new hoCuConebeamProjectionOperator() );
		Ep->setup(ps,binning,imageDimensions);
		Ep->set_use_filtered_backprojection(true);
		Ep->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());
		Ep->set_domain_dimensions(&is_dims3d);

		boost::shared_ptr<hoCuNDArray<float> > prior3d(new hoCuNDArray<float>(&is_dims3d));
		Ep->mult_MH(&projections,prior3d.get());

		hoCuNDArray<float> tmp_proj(projections);
		Ep->mult_M(prior3d.get(),&tmp_proj);

		float s = dot(&projections,&tmp_proj)/dot(&tmp_proj,&tmp_proj);
		*prior3d *= s;
		boost::shared_ptr<hoCuNDArray<float> > prior(new hoCuNDArray<float>(*expand( prior3d.get(), is_dims.back() )));
		boost::shared_ptr<hoCuPartialDerivativeOperator<float,4> > dx (new hoCuPartialDerivativeOperator<float,4>(0) );
		boost::shared_ptr<hoCuPartialDerivativeOperator<float,4> > dy (new hoCuPartialDerivativeOperator<float,4>(1) );
		boost::shared_ptr<hoCuPartialDerivativeOperator<float,4> > dz (new hoCuPartialDerivativeOperator<float,4>(2) );
		boost::shared_ptr<hoCuPartialDerivativeOperator<float,4> > dt (new hoCuPartialDerivativeOperator<float,4>(3) );

		dx->set_weight(vm["PICCS"].as<float>());
		dy->set_weight(vm["PICCS"].as<float>());
		dz->set_weight(vm["PICCS"].as<float>());
		dt->set_weight(vm["PICCS"].as<float>());

		dx->set_codomain_dimensions(&is_dims);
		dy->set_codomain_dimensions(&is_dims);
		dz->set_codomain_dimensions(&is_dims);
		dt->set_codomain_dimensions(&is_dims);

		dx->set_domain_dimensions(&is_dims);
		dy->set_domain_dimensions(&is_dims);
		dz->set_domain_dimensions(&is_dims);
		dt->set_domain_dimensions(&is_dims);

		solver.add_regularization_group_operator(dx);
		solver.add_regularization_group_operator(dy);
		solver.add_regularization_group_operator(dz);
		solver.add_regularization_group_operator(dt);
		solver.add_group(prior,1);

		delete binning_pics;
	}

	// Run solver
	//

	boost::shared_ptr< hoCuNDArray<float> > result;

	{
		GPUTimer timer("\nRunning Split Bregman solver");
		result = solver.solve(&projections);
	}

	write_nd_array<float>( result.get(), outputFile.c_str());
}


/*
The MIT License

Copyright (c) 2015-2017 Albert Murienne

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include "tensor_solver.h"
#include "tensor_operations.h"

#include "common/network_random.h"

#include <boost/iterator/zip_iterator.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

namespace neurocl { namespace convnet {

inline void _assert_multiple( const tensor& t, const size_t& divider )
{
    if ( ( ( t.w() % divider ) != 0 ) || ( ( t.h() % divider ) != 0 ) )
        throw network_exception( "invalid tensor subsampling" );
}

inline void _assert_no_replication( const tensor& t )
{
    if ( t.d1() != 1 )
        throw network_exception( "operation not supported for replicated tensors" );
}

// check that t1.depth2 == t2.depth1
inline void _assert_cross_depths21( const tensor& t1, const tensor& t2 )
{
    if ( t1.d2() != t2.d1() )
        throw network_exception( "inconsistent tensor number of feature maps (t1.depth2 != t2.depth1)" );
}

// check that t1.depth2 == t2.depth2
inline void _assert_cross_depths22( const tensor& t1, const tensor& t2 )
{
    if ( t1.d2() != t2.d2() )
        throw network_exception( "inconsistent tensor number of feature maps (t1.depth2 != t2.depth2)" );
}

inline void _assert_muladd_sizes( const tensor& t1, const tensor& t2, const tensor& t3 )
{
    if ( ( t1.h() != t2.w() ) ||
        ( t1.w() != t3.w() ) ||
        ( t2.h() != t3.h() ) ||
        ( t1.d1() != t2.d1() ) ||
        ( t1.d2() != t2.d2() ) ||
        ( t1.d1() != t3.d1() ) ||
        ( t1.d2() != t3.d2() ) )
        throw network_exception( "inconsistent tensor multiply/add size" );
}

inline void _assert_multrans1_sizes( const tensor& t1, const tensor& t2 )
{
    if ( ( t1.w() != t2.w() ) ||
        ( t1.d1() != t2.d1() ) ||
        ( t1.d2() != t2.d2() ) )
        throw network_exception( "inconsistent tensor multiply/trans1 size" );
}

inline void _assert_multrans2_sizes( const tensor& t1, const tensor& t2 )
{
    if ( ( t1.h() != t2.h() ) ||
        ( t1.d1() != t2.d1() ) ||
        ( t1.d2() != t2.d2() ) )
        throw network_exception( "inconsistent tensor multiply/trans2 size" );
}

inline void _assert_same_sizes( const tensor& t1, const tensor& t2 )
{
    if ( ( t1.w() != t2.w() ) ||
        ( t1.h() != t2.h() ) ||
        ( t1.d1() != t2.d1() ) ||
        ( t1.d2() != t2.d2() ) )
        throw network_exception( "inconsistent tensor sizes" );
}

tensor tensor_operation::scale( const float& val, const tensor& input )
{
    tensor output;
    output.resize( input );

    tensor_foreach_p( input.d1(), input.d2() ) {
        output.m_tensor_array[d1][d2] = val * input.m_tensor_array[d1][d2];
    }

    return output;
}

tensor tensor_operation::plus( const float& val, const tensor& input )
{
    tensor output;
    output.resize( input );

    tensor_foreach_p( input.d1(), input.d2() ) {
        output.m_tensor_array[d1][d2] =
            boost::numeric::ublas::scalar_matrix<float>( input.w(), input.h(), val ) + input.m_tensor_array[d1][d2];
    }

    return output;
}

tensor tensor_operation::minus( const float& val, const tensor& input )
{
    tensor output;
    output.resize( input );

    tensor_foreach_p( input.d1(), input.d2() ) {
        output.m_tensor_array[d1][d2] =
            boost::numeric::ublas::scalar_matrix<float>( input.w(), input.h(), val ) - input.m_tensor_array[d1][d2];
    }

    return output;
}

tensor tensor_operation::add( const tensor& inputA, const tensor& inputB )
{
    _assert_same_sizes( inputA, inputB );

    tensor output;
    output.resize( inputA );

    tensor_foreach_p( inputA.d1(), inputA.d2() ) {
        output.m_tensor_array[d1][d2] = inputA.m_tensor_array[d1][d2]
                + inputB.m_tensor_array[d1][d2];
    }

    return output;
}

tensor tensor_operation::sub( const tensor& inputA, const tensor& inputB )
{
    _assert_same_sizes( inputA, inputB );

    tensor output;
    output.resize( inputA );

    tensor_foreach_p( inputA.d1(), inputA.d2() ) {
        output.m_tensor_array[d1][d2] = inputA.m_tensor_array[d1][d2]
                - inputB.m_tensor_array[d1][d2];
    }

    return output;
}

tensor tensor_operation::group( const tensor& input )
{
    _assert_no_replication( input );

    tensor output;
    output.resize( input.d2() * input.w() * input.h(), 1, 1, 1 );

    auto output_mat_iter = output.m_tensor_array[0][0].data().begin();

    size_t _offset = 0;

	// TODO-CNN : could be written in a smarter way
	// ie using tensor iterators and C++11

    tensor_foreach_p( 1, input.d2() ) {

        auto input_mat_iter = input.m_tensor_array[d1][d2].data().begin();
        const size_t _size = input.m_tensor_array[d1][d2].size1() * input.m_tensor_array[d1][d2].size2();

        std::copy( input_mat_iter, input_mat_iter + _size, output_mat_iter + _offset );

        _offset += _size;
    }

    return output;
}

void tensor_operation::ungroup( const tensor& input, tensor& output )
{
    _assert_no_replication( output );

    auto input_mat_iter = input.m_tensor_array[0][0].data().begin();

	// TODO-CNN : could be written in a smarter way
	// ie using tensor iterators and C++11

    tensor_foreach_p( 1, output.d2() ) {

        const size_t _size = output.m_tensor_array[d1][d2].size1() * output.m_tensor_array[d1][d2].size2();

        std::copy( input_mat_iter, input_mat_iter + _size, output.m_tensor_array[d1][d2].data().begin() );

        input_mat_iter += _size;
    }
}

tensor tensor_operation::elediv( const tensor& inputA, const tensor& inputB )
{
    using namespace boost::numeric::ublas;

    _assert_same_sizes( inputA, inputB );

    tensor output;
    output.resize( inputA );

    tensor_foreach_p( inputA.d1(), inputA.d2() ) {
        output.m_tensor_array[d1][d2] =
            element_div( inputA.m_tensor_array[d1][d2], inputB.m_tensor_array[d1][d2] );
    }

    return output;
}

tensor tensor_operation::elemul( const tensor& inputA, const tensor& inputB )
{
    using namespace boost::numeric::ublas;

    _assert_same_sizes( inputA, inputB );

    tensor output;
    output.resize( inputA );

    tensor_foreach_p( inputA.d1(), inputA.d2() ) {
        output.m_tensor_array[d1][d2] =
            element_prod( inputA.m_tensor_array[d1][d2], inputB.m_tensor_array[d1][d2] );
    }

    return output;
}

tensor tensor_operation::mul( const tensor& inputA, const tensor& inputB )
{
    using namespace boost::numeric::ublas;

    _assert_same_sizes( inputA, inputB );

    tensor output;
    output.resize( inputA );

    tensor_foreach_p( inputA.d1(), inputA.d2() ) {
        output.m_tensor_array[d1][d2] =
            prod( inputA.m_tensor_array[d1][d2], inputB.m_tensor_array[d1][d2] );
    }

    return output;
}

tensor tensor_operation::muladd( const tensor& inputA, const tensor& inputB, const tensor& inputC )
{
    using namespace boost::numeric::ublas;

    _assert_muladd_sizes( inputA, inputB, inputC );

    tensor output;
    output.resize( inputC ); // output is homogenous to inputC

    tensor_foreach_p( inputA.d1(), inputA.d2() ) {
        output.m_tensor_array[d1][d2] =
            prod( inputA.m_tensor_array[d1][d2], inputB.m_tensor_array[d1][d2] )
                + inputC.m_tensor_array[d1][d2];
    }

    return output;
}

tensor tensor_operation::multrans1( const tensor& inputA, const tensor& inputB )
{
    using namespace boost::numeric::ublas;

    _assert_multrans1_sizes( inputA, inputB );

    tensor output;
    output.resize( inputA.h(), inputB.h(), inputA.d1(), inputA.d2() );

    tensor_foreach_p( inputA.d1(), inputA.d2() ) {
        output.m_tensor_array[d1][d2] =
            prod( trans( inputA.m_tensor_array[d1][d2] ), inputB.m_tensor_array[d1][d2] );
    }

    return output;
}

tensor tensor_operation::multrans2( const tensor& inputA, const tensor& inputB )
{
    using namespace boost::numeric::ublas;

    _assert_multrans2_sizes( inputA, inputB );

    tensor output;
    output.resize( inputA.w(), inputB.w(), inputA.d1(), inputA.d2() );

    tensor_foreach_p( inputA.d1(), inputA.d2() ) {
        output.m_tensor_array[d1][d2] =
            prod( inputA.m_tensor_array[d1][d2], trans( inputB.m_tensor_array[d1][d2] ) );
    }

    return output;
}

tensor tensor_operation::sqrt( const tensor& input )
{
    tensor output;
    output.resize( input );

    tensor_foreach_p( output.d1(), output.d2() ) {
        matrixF& _output = output.m_tensor_array[d1][d2];
        _output = input.m_tensor_array[d1][d2];
        std::transform( _output.data().begin(), _output.data().end(), _output.data().begin(), std::ptr_fun<float,float>( std::sqrt ) );
    }

    return output;
}

struct flipper
{
    flipper( int sx, int sy ) { m_flipped = matrixF(sx,sy); }
    const matrixF& flipped( const matrixF& in )
    { m_flipped = in; std::reverse( m_flipped.data().begin(), m_flipped.data().end() ); return m_flipped; }
    matrixF m_flipped;
};

// name reflects the feature maps feed forwarding specifity of this method
template <>
tensor tensor_operation::convolve_add_forward<tensor_operation::kernel_mode::flip,tensor_operation::pad_mode::valid>(
    const tensor& input, const tensor& filter, const int stride )
{
    using namespace boost::numeric::ublas;

    _assert_cross_depths21( input, filter );

    tensor output;

    // W2 = W1 - F + 1
    auto stepsX = input.w() - filter.w() + 1;
    auto stepsY = input.h() - filter.h() + 1;

    // no replication in output features
    output.resize( stepsX, stepsY, 1, filter.d2() );

    flipper f( filter.w(), filter.h() );
    matrixF conv( filter.w(), filter.h() );

    // NOTE : tricky thing is that filter tensor replication level (filter.d1)
    // is equal to input tensor feature maps level (prev_layer.d2);
    // whereas filter feature maps level is equal to output tensor feature maps level

    for ( auto d2 = size_t(0); d2 < filter.d2(); d2++ )
    {
    	for ( auto d1 = size_t(0); d1 < filter.d1(); d1++ )
        {
            for ( auto j=size_t(0); j<stepsY; j++ )
            {
                for ( auto i=size_t(0); i<stepsX; i++ )
                {
                    // multiply
                    noalias(conv) = element_prod( f.flipped( filter.m_tensor_array[d1][d2] ),
                        project( input.m_tensor_array[0][d1],
                            range( i, i+filter.w() ),
                            range( j, j+filter.h() ) ) );

                    // accumulate + add
                    output.m_tensor_array[0][d2](i,j) +=
                        std::accumulate( conv.data().begin(), conv.data().end(), 0.f );
                }
            }
        }
    }

    return output;
}

// name reflects the error back propagation specifity of this method
template <>
tensor tensor_operation::convolve_add_backward<tensor_operation::kernel_mode::std,tensor_operation::pad_mode::full>(
    const tensor& input, const tensor& filter, const int stride )
{
    using namespace boost::numeric::ublas;

    _assert_cross_depths22( input, filter );

    auto _FmX = filter.w() - 1;
    auto _FmY = filter.h() - 1;

    // W1 = W2 + F - 1
    auto stepsX = input.w() + _FmX;
    auto stepsY = input.h() + _FmY;

    tensor output;
    output.resize( stepsX, stepsY, 1, filter.d1() );

    // W3 = W2 + 2F - 2
    auto padX = stepsX + _FmX;
    auto padY = stepsY + _FmY;

    // TODO-CNN : could be optimized with a tensor_tank::instance().get( padX, padY, 1, filter.d2() )
    tensor padded_input;
    padded_input.resize( padX, padY, 1, filter.d2() );

    matrixF conv( filter.w(), filter.h() );

    for ( auto d2 = size_t(0); d2 < filter.d2(); d2++ )
    {
        // update padded matrix
        project(    padded_input.m_tensor_array[0][d2],
                    range( _FmX, _FmX + input.w() ),
                    range( _FmY, _FmY + input.h() ) )
            = input.m_tensor_array[0][d2];

        for ( auto d1 = size_t(0); d1 < filter.d1(); d1++ )
        {
            for ( auto j=size_t(0); j<stepsY; j++ )
            {
                for ( auto i=size_t(0); i<stepsX; i++ )
                {
					// multiply
                    noalias(conv) = element_prod( filter.m_tensor_array[d1][d2],
                        project( padded_input.m_tensor_array[0][d2],
                            range( i, i+filter.w() ),
                            range( j, j+filter.h() ) ) );

                    // accumulate + add
                    // question was raised about the necessity to divide proportionnaly to forward feed accumulation filter replication
                    output.m_tensor_array[0][d1](i,j) +=
                        std::accumulate( conv.data().begin(), conv.data().end(), 0.f );
                }
            }
        }
    }

    return output;
}

// name reflects the filters gradient update specifity of this method
template <>
tensor tensor_operation::convolve_update<tensor_operation::kernel_mode::std,tensor_operation::pad_mode::valid>(
    const tensor& input, const tensor& filter, const int stride )
{
    using namespace boost::numeric::ublas;

    tensor output;

    // W2 = W1 - F + 1
    auto stepsX = input.w() - filter.w() + 1;
    auto stepsY = input.h() - filter.h() + 1;

    // no replication in output features
    output.resize( stepsX, stepsY, input.d2(), filter.d2() );

    matrixF conv( input.w(), filter.h() );

    for ( auto d1 = size_t(0); d1 < input.d2(); d1++ )
    {
        for ( auto d2 = size_t(0); d2 < filter.d2(); d2++ )
        {
            for ( auto j=size_t(0); j<stepsY; j++ )
            {
                for ( auto i=size_t(0); i<stepsX; i++ )
                {
					// multiply
                    noalias(conv) = element_prod( filter.m_tensor_array[0][d2],
                        project( input.m_tensor_array[0][d1],
                            range( i, i+filter.w() ),
                            range( j, j+filter.h() ) ) );

                    // accumulate
                    output.m_tensor_array[d1][d2](i,j) =
                        std::accumulate( conv.data().begin(), conv.data().end(), 0.f );
                }
            }
        }
    }

    return output;
}

tensor tensor_operation::subsample( const tensor& input, const size_t subsample )
{
    _assert_multiple( input, subsample );

    tensor output;
    output.resize( input.w() / subsample, input.h() / subsample, input.d1(), input.d2() );

    tensor_foreach_p( input.d1(), input.d2() )
    {
        matrixF& feature_map = output.m_tensor_array[d1][d2];

        const matrixF& prev_feature_map = input.m_tensor_array[d1][d2];
        auto prev_width = prev_feature_map.size1();
        auto prev_it1 = prev_feature_map.cbegin1();

        for( auto it1 = feature_map.begin1(); it1 != feature_map.end1(); it1++, prev_it1 += subsample )
        {
            auto prev_it2 = prev_it1.cbegin();
            for( auto it2 = it1.begin(); it2 != it1.end(); it2++, prev_it2 += subsample )
            {
                float max_value = std::numeric_limits<float_t>::lowest();

                // could use ublas::project + std::accumulate + std::max for more compact expression

                // compute max in subsampling zone
                for ( auto j = size_t(0); j<subsample; j++ )
                	for ( auto i = size_t(0); i<subsample; i++ )
                    {
                        const auto& value = *(prev_it2 + i + (j*prev_width) );
                        if ( value > max_value )
                            max_value = value;
                    }

                // update value in the destination feature map
                *it2 = max_value;
            }
        }
    }

    return output;
}

tensor tensor_operation::d_subsample( const tensor& input, const tensor& input_ref, const size_t subsample )
{
    _assert_multiple( input_ref, subsample );

    tensor output;
    output.resize( input.w() * subsample, input.h() * subsample, input.d1(), input.d2() );

    tensor_foreach_p( input.d1(), input.d2() )
    {
        // Initialize iterators
        const matrixF& prev_feature_map = input_ref.m_tensor_array[d1][d2];
        auto prev_width = prev_feature_map.size1();

        const matrixF& error_map = input.m_tensor_array[d1][d2];
        auto err_it1 = error_map.cbegin1();

        matrixF& prev_error_map = output.m_tensor_array[d1][d2];
        auto prev_err_iter1 = prev_error_map.begin1();

        // Iterate
        for( auto prev_it1 = prev_feature_map.cbegin1(); prev_it1 != prev_feature_map.cend1();
            prev_it1 += subsample, prev_err_iter1 += subsample, ++err_it1 )
        {
            auto prev_err_it2 = prev_err_iter1.begin();
            auto err_it2 = err_it1.begin();
            for( auto prev_it2 = prev_it1.begin(); prev_it2 != prev_it1.end();
                prev_it2 += subsample, prev_err_it2 += subsample, ++err_it2 )
            {
                auto max_value = std::numeric_limits<float_t>::lowest();

                int max_offset = 0;

                // compute max in subsampling zone
                for ( auto j = size_t(0); j<subsample; j++ )
                	for ( auto i = size_t(0); i<subsample; i++ )
                    {
                        const auto& value = *(prev_it2 + i + (j*prev_width) );
                        if ( value > max_value )
                        {
                            max_value = value;
                            max_offset = i + (j*prev_width);
                        }
                    }

                // update error on last layer max value pixel
                *(prev_err_it2 + max_offset) = *err_it2;
            }
        }
    }

    return output;
}

tensor tensor_operation::uniform_sum( const tensor& input )
{
    tensor output;
    output.resize( input );

    tensor_foreach_p( input.d1(), input.d2() ) {
        float _acc = std::accumulate( input.m_tensor_array[d1][d2].data().begin(), input.m_tensor_array[d1][d2].data().end(), 0.f );
        output.m_tensor_array[d1][d2] = boost::numeric::ublas::scalar_matrix<float>( input.w(), input.h(), _acc );
    }

    return output;
}

void tensor_operation::bernoulli( tensor& input, const float p )
{
    random::rand_bernoulli_generator bernoulli( p );

    tensor_foreach_p( input.d1(), input.d2() ) {
        std::for_each(  input.m_tensor_array[d1][d2].data().begin(),
                        input.m_tensor_array[d1][d2].data().end(),
                        [&bernoulli]( float& a ) { a = bernoulli.gen<float>(); } );
    }
}

tensor tensor_operation::binary_operator( const tensor& inputA, const tensor& inputB, std::function<float (const float&,const float&)> op )
{
    _assert_same_sizes( inputA, inputB );

    tensor output;
    output.resize( inputA );

    tensor_foreach_p( inputA.d1(), inputA.d2() ) {
        auto iter = boost::make_zip_iterator( boost::make_tuple( inputA.m_tensor_array[d1][d2].data().begin(),
            inputB.m_tensor_array[d1][d2].data().begin(), output.m_tensor_array[d1][d2].data().begin() ) );
        auto end = boost::make_zip_iterator( boost::make_tuple( inputA.m_tensor_array[d1][d2].data().end(),
            inputB.m_tensor_array[d1][d2].data().end(), output.m_tensor_array[d1][d2].data().end() ) );

            for( ; iter != end ; ++iter )
            {
                const float& ia = iter->get<0>();
                const float& ib = iter->get<1>();
                float& o = iter->get<2>();

                o = op( ia, ib );
            }
    }

    return output;
}

template<>
void tensor_operation::optimize<tensor_operation::optimize_mode::redux>( const std::shared_ptr<tensor_solver_iface>& solver, tensor* input, tensor** input_cache, const tensor* deltas )
{
    solver->update_redux( *input, input_cache, *deltas );
}

template<>
void tensor_operation::optimize<tensor_operation::optimize_mode::std>( const std::shared_ptr<tensor_solver_iface>& solver, tensor* input, tensor** input_cache, const tensor* deltas )
{
    solver->update( *input, input_cache, *deltas );
}

} /*namespace neurocl*/ } /*namespace convnet*/


/*
 parse_fcsxml.cpp

 Copyright (c) 2021 Terumasa Tadano

 This file is distributed under the terms of the MIT license.
 Please see the file 'LICENCE.txt' in the root directory
 or http://opensource.org/licenses/mit-license.php for information.
*/

#include "parse_fcsxml.h"
#include "xml_parser.h"
#include <iostream>
#include <fstream>
#include <cstdlib>
#include <map>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/foreach.hpp>
#include <boost/version.hpp>
#include <boost/lexical_cast.hpp>
#include "memory.h"
#include "../include/mathfunctions.h"

using namespace std;

int main(int argc, char *argv[])
{
    std::string file_xml;
    int maxorder;

    file_xml = argv[1];
    maxorder = boost::lexical_cast<int>(argv[2]);

    if (maxorder <= 1) {
        std::cout << "Maxorder should be larger than 1. " << std::endl;
        exit(EXIT_FAILURE);
    }

    maxorder -= 1;

    std::vector <FcsArrayWithCell> *fc;
    StructureProperty structure;

    allocate(fc, maxorder);
    load_fcs_xml(file_xml, maxorder, structure, fc);

    std::string::size_type const p(file_xml.find_last_of('.'));
    std::string prefix = file_xml.substr(0, p);

    int **map_p2s;
    double ***x_image;

    allocate(map_p2s, structure.natmin, structure.ntran);
    allocate(x_image, 27, structure.nat, 3);

    auto icell = 0;
    for (auto i = 0; i < structure.nat; ++i) {
        x_image[0][i][0] = structure.atoms[i].x;
        x_image[0][i][1] = structure.atoms[i].y;
        x_image[0][i][2] = structure.atoms[i].z;
    }
    // Convert to Cartesian coordinate
    frac2cart(x_image[0], structure.nat, structure.lattice_vector);

    for (auto ia = -1; ia <= 1; ++ia) {
        for (auto ja = -1; ja <= 1; ++ja) {
            for (auto ka = -1; ka <= 1; ++ka) {

                if (ia == 0 && ja == 0 && ka == 0) continue;

                ++icell;
                for (auto i = 0; i < structure.nat; ++i) {
                    x_image[icell][i][0] = structure.atoms[i].x + static_cast<double>(ia);
                    x_image[icell][i][1] = structure.atoms[i].y + static_cast<double>(ja);
                    x_image[icell][i][2] = structure.atoms[i].z + static_cast<double>(ka);
                }
                // Convert to Cartesian coordinate
                frac2cart(x_image[icell], structure.nat, structure.lattice_vector);
            }
        }
    }

    for (auto i = 0; i < structure.nat; ++i) {
        map_p2s[structure.atoms[i].atom][structure.atoms[i].tran] = i;
    }

    for (auto order = 0; order < maxorder; ++order) {
        std::string fname_fc = prefix + ".fc" + std::to_string(order + 2);
        write_fcs_to_file(fname_fc,
                          order,
                          structure,
                          x_image,
                          map_p2s,
                          fc[order]);
    }

    deallocate(fc);
    deallocate(x_image);
    deallocate(map_p2s);
}

void load_fcs_xml(const std::string file_in,
                  const int maxorder,
                  StructureProperty &StructProp,
                  std::vector <FcsArrayWithCell> *force_constant_with_cell)
{
    using namespace boost::property_tree;
    ptree pt;
    map<string, int> dict_atomic_kind;
    std::stringstream ss;

    try {
        read_xml(file_in, pt);
    }
    catch (exception &e) {
        cout << "Cannot open file " + file_in << endl;
        exit(EXIT_FAILURE);
    }

    StructProp.nat = boost::lexical_cast<unsigned int>(
            get_value_from_xml(pt,
                               "Data.Structure.NumberOfAtoms"));
    StructProp.nspecies = boost::lexical_cast<unsigned int>(
            get_value_from_xml(pt,
                               "Data.Structure.NumberOfElements"));

    StructProp.ntran = boost::lexical_cast<unsigned int>(
            get_value_from_xml(pt,
                               "Data.Symmetry.NumberOfTranslations"));

    for (auto i = 0; i < 3; ++i) {
        ss.str("");
        ss.clear();
        ss << get_value_from_xml(pt,
                                 "Data.Structure.LatticeVector.a"
                                 + boost::lexical_cast<string>(i + 1));
        ss >> StructProp.lattice_vector[0][i]
           >> StructProp.lattice_vector[1][i]
           >> StructProp.lattice_vector[2][i];
    }

    ss.str("");
    ss.clear();
    ss << get_value_from_xml(pt, "Data.Structure.Periodicity");
    ss >> StructProp.is_periodic[0]
       >> StructProp.is_periodic[1]
       >> StructProp.is_periodic[2];

    // Parse atomic elements and coordinates

    StructProp.kd_symbol.resize(StructProp.nspecies);
    StructProp.atoms.resize(StructProp.nat);

    int i = 0;

    BOOST_FOREACH(
    const ptree::value_type &child_, pt.get_child("Data.Structure.AtomicElements")) {
        const ptree &child = child_.second;
        const unsigned int icount_kd = child.get<unsigned int>("<xmlattr>.number");
        dict_atomic_kind[boost::lexical_cast<string>(child_.second.data())] = icount_kd - 1;
        StructProp.kd_symbol[i++] = boost::lexical_cast<string>(child_.second.data());
    }

    unsigned int index;

    BOOST_FOREACH(
    const ptree::value_type &child_, pt.get_child("Data.Structure.Position")) {
        const ptree &child = child_.second;
        const string str_index = child.get<string>("<xmlattr>.index");
        const string str_element = child.get<string>("<xmlattr>.element");

        ss.str("");
        ss.clear();
        ss << child.data();

        index = boost::lexical_cast<unsigned int>(str_index) - 1;

        if (index >= StructProp.nat) {
            cout << "index is out of range" << endl;
            exit(EXIT_FAILURE);
        }

        StructProp.atoms[index].kind = dict_atomic_kind[str_element];
        ss >> StructProp.atoms[index].x
           >> StructProp.atoms[index].y
           >> StructProp.atoms[index].z;
    }

    dict_atomic_kind.clear();

    // Parse mapping information

    StructProp.natmin = StructProp.nat / StructProp.ntran;

    unsigned int tran, atom_p, atom_s;

    BOOST_FOREACH(
    const ptree::value_type &child_, pt.get_child("Data.Symmetry.Translations")) {
        const ptree &child = child_.second;
        const string str_tran = child.get<string>("<xmlattr>.tran");
        const string str_atom = child.get<string>("<xmlattr>.atom");

        tran = boost::lexical_cast<unsigned int>(str_tran) - 1;
        atom_p = boost::lexical_cast<unsigned int>(str_atom) - 1;
        atom_s = boost::lexical_cast<unsigned int>(child.data()) - 1;

        if (tran >= StructProp.ntran || atom_p >= StructProp.natmin || atom_s >= StructProp.nat) {
            cout << "index is out of range" << endl;
            exit(EXIT_FAILURE);
        }

        StructProp.atoms[atom_s].atom = atom_p;
        StructProp.atoms[atom_s].tran = tran;
    }

    // Parse force constants

    std::vector <AtomCellSuper> ivec_with_cell;
    std::string str_tag;
    double fcs_val;
    unsigned int atmn, xyz, cell_s;
    std::string str_pairs;
    std::string str_attr;
    AtomCellSuper ivec_tmp;

    for (auto order = 0; order < maxorder; ++order) {

        if (order == 0) {
            str_tag = "Data.ForceConstants.HARMONIC";
        } else {
            str_tag = "Data.ForceConstants.ANHARM" + std::to_string(order + 2);
        }

        boost::optional < ptree &> child_ = pt.get_child_optional(str_tag);

        if (!child_) {
            std::string str_tmp = str_tag + " flag not found in the XML file";
            exit(EXIT_FAILURE);
        }

        BOOST_FOREACH(
        const ptree::value_type &child_, pt.get_child(str_tag)) {
            const ptree &child = child_.second;

            fcs_val = boost::lexical_cast<double>(child.data());

            ivec_with_cell.clear();

            for (i = 0; i < order + 2; ++i) {
                str_attr = "<xmlattr>.pair" + std::to_string(i + 1);
                str_pairs = child.get<std::string>(str_attr);

                ss.str("");
                ss.clear();
                ss << str_pairs;

                if (i == 0) {
                    ss >> atmn >> xyz;
                    ivec_tmp.index = 3 * (atmn - 1) + xyz - 1;
                    ivec_tmp.cell_s = 0;
                    ivec_tmp.tran = 0; // dummy
                    ivec_with_cell.push_back(ivec_tmp);

                } else {
                    ss >> atmn >> xyz >> cell_s;
                    ivec_tmp.index = 3 * (atmn - 1) + xyz - 1;
                    ivec_tmp.cell_s = cell_s - 1;
                    ivec_tmp.tran = 0; // dummy
                    ivec_with_cell.push_back(ivec_tmp);
                }
            }

            force_constant_with_cell[order].emplace_back(fcs_val, ivec_with_cell);
        }
    }
}

void write_fcs_to_file(const std::string fname_fc,
                       const int order,
                       const StructureProperty &structure,
                       double ***x_image,
                       int **map_p2s,
                       const std::vector <FcsArrayWithCell> &fc_in)
{

    std::ofstream ofs;
    ofs.open(fname_fc, std::ios::out);

    const auto nelems = order + 2;

    ofs << '#';
    for (auto i = 0; i < nelems; ++i) {
        ofs << std::setw(6) << "atom" + std::to_string(i + 1);
        ofs << std::setw(5) << "xyz" +  std::to_string(i + 1);
    }
    ofs << std::setw(20) << "IFC" + std::to_string(nelems) + " (Ry/bohr^" + std::to_string(nelems) + ")";
    ofs << std::setw(20) << "distances (bohr)";
    ofs << '\n';

    for (const auto &it: fc_in) {
        ofs << std::setw(7) << map_p2s[it.pairs[0].index / 3][0] + 1;
        ofs << std::setw(5) << it.pairs[0].index % 3 + 1;

        for (auto i = 1; i < nelems; ++i) {
            ofs << std::setw(6) << it.pairs[i].index / 3 + 1;
            ofs << std::setw(5) << it.pairs[i].index % 3 + 1;
        }

        ofs << std::setw(20) << it.fcs_val;

        double distance;
        double xdiff[3];

        for (auto i = 1; i < nelems; ++i) {
            for (auto j = 0; j < 3; ++j) {
                xdiff[j] = x_image[it.pairs[i].cell_s][it.pairs[i].index / 3][j]
                        - x_image[0][map_p2s[it.pairs[0].index / 3][0]][j];
            }
            distance = std::sqrt(xdiff[0]*xdiff[0]+xdiff[1]*xdiff[1]+xdiff[2]*xdiff[2]);
            ofs << std::setw(10) << distance;
        }
        ofs << '\n';
    }

    ofs.close();
}

void frac2cart(double **xf,
               const int nat,
               const double lattice_vector[3][3])
{
    // x_cartesian = A x_fractional

    double *x_tmp;
    allocate(x_tmp, 3);

    for (size_t i = 0; i < nat; ++i) {

        rotvec(x_tmp, xf[i], lattice_vector);

        for (auto j = 0; j < 3; ++j) {
            xf[i][j] = x_tmp[j];
        }
    }
    deallocate(x_tmp);
}