#pragma once

#include <pcl/memory.h>
#include <pcl/filters/passthrough.h>
#include <pcl/octree/octree_pointcloud_changedetector.h>
#include <pcl/tracking/coherence.h>
#include <pcl/tracking/tracker.h>
#include <pcl/tracking/tracking.h>

#include <Eigen/Dense>


namespace pcl
{
  namespace tracking
  {
    /** \brief @b ParticleFilterTracker tracks the PointCloud which is given by
        setReferenceCloud within the measured PointCloud using particle filter method.
      * \author Ryohei Ueda
      * \ingroup tracking
      */
    template <typename PointInT, typename StateT>
    class ParticleFilterTracker: public Tracker<PointInT, StateT>
    {
      protected:
        using Tracker<PointInT, StateT>::deinitCompute;
        
      public:
        using Tracker<PointInT, StateT>::tracker_name_;
        using Tracker<PointInT, StateT>::search_;
        using Tracker<PointInT, StateT>::input_;
        using Tracker<PointInT, StateT>::indices_;
        using Tracker<PointInT, StateT>::getClassName;

        using Ptr = shared_ptr<ParticleFilterTracker<PointInT, StateT>>;
        using ConstPtr = shared_ptr<const ParticleFilterTracker<PointInT, StateT>>;

        using BaseClass = Tracker<PointInT, StateT>;
        
        using PointCloudIn = typename Tracker<PointInT, StateT>::PointCloudIn;
        using PointCloudInPtr = typename PointCloudIn::Ptr;
        using PointCloudInConstPtr = typename PointCloudIn::ConstPtr;

        using PointCloudState = typename Tracker<PointInT, StateT>::PointCloudState;
        using PointCloudStatePtr = typename PointCloudState::Ptr;
        using PointCloudStateConstPtr = typename PointCloudState::ConstPtr;

        using Coherence = PointCoherence<PointInT>;
        using CoherencePtr = typename Coherence::Ptr;
        using CoherenceConstPtr = typename Coherence::ConstPtr;

        using CloudCoherence = PointCloudCoherence<PointInT>;
        using CloudCoherencePtr = typename CloudCoherence::Ptr;
        using CloudCoherenceConstPtr = typename CloudCoherence::ConstPtr;

        /** \brief Empty constructor. */
        ParticleFilterTracker ()
        : iteration_num_ (1)
        , particle_num_ ()
        , min_indices_ (1)
        , ref_ ()
        , particles_ ()
        , coherence_ ()
        , resample_likelihood_thr_ (0.0)
        , occlusion_angle_thr_ (M_PI / 2.0)
        , alpha_ (15.0)
        , representative_state_ ()
        , use_normal_ (false)
        , motion_ ()
        , motion_ratio_ (0.25)
        , pass_x_ ()
        , pass_y_ ()
        , pass_z_ ()
        , transed_reference_vector_ ()
        , change_detector_ ()
        , changed_ (false)
        , change_counter_ (0)
        , change_detector_filter_ (10)
        , change_detector_interval_ (10)
        , change_detector_resolution_ (0.01)
        , use_change_detector_ (false)
        {
          tracker_name_ = "ParticleFilterTracker";
          pass_x_.setFilterFieldName ("x");
          pass_y_.setFilterFieldName ("y");
          pass_z_.setFilterFieldName ("z");
          pass_x_.setKeepOrganized (false);
          pass_y_.setKeepOrganized (false);
          pass_z_.setKeepOrganized (false);
        }
        
        /** \brief Set the number of iteration.
          * \param[in] iteration_num the number of iteration.
          */
        inline void
        setIterationNum (const int iteration_num) { iteration_num_ = iteration_num; }

        /** \brief Get the number of iteration. */
        inline int
        getIterationNum () const { return iteration_num_; }

        /** \brief Set the number of the particles.
          * \param[in] particle_num the number of the particles.
          */
        inline void
        setParticleNum (const int particle_num) { particle_num_ = particle_num; }

        /** \brief Get the number of the particles. */
        inline int
        getParticleNum () const { return particle_num_; }

        /** \brief Set a pointer to a reference dataset to be tracked.
          * \param[in] ref a pointer to a PointCloud message
          */
        inline void
        setReferenceCloud (const PointCloudInConstPtr &ref) { ref_ = ref; }

        /** \brief Get a pointer to a reference dataset to be tracked. */
        inline PointCloudInConstPtr const
        getReferenceCloud () { return ref_; }

        /** \brief Set the PointCloudCoherence as likelihood.
          * \param[in] coherence a pointer to PointCloudCoherence.
          */
        inline void
        setCloudCoherence (const CloudCoherencePtr &coherence) { coherence_ = coherence; }
        
        /** \brief Get the PointCloudCoherence to compute likelihood. */
        inline CloudCoherencePtr
        getCloudCoherence () const { return coherence_; }
        

        /** \brief Set the covariance of step noise.
          * \param[in] step_noise_covariance the diagonal elements of covariance matrix of step noise.
          */
        inline void
        setStepNoiseCovariance (const std::vector<double> &step_noise_covariance)
        {
          step_noise_covariance_ = step_noise_covariance;
        }

        /** \brief Set the covariance of the initial noise. It will be used when initializing the particles.
          * \param[in] initial_noise_covariance the diagonal elements of covariance matrix of initial noise.
          */
        inline void
        setInitialNoiseCovariance (const std::vector<double> &initial_noise_covariance)
        {
          initial_noise_covariance_ = initial_noise_covariance;
        }

        /** \brief Set the mean of the initial noise. It will be used when initializing the particles.
          * \param[in] initial_noise_mean the mean values of initial noise.
          */
        inline void
        setInitialNoiseMean (const std::vector<double> &initial_noise_mean)
        {
          initial_noise_mean_ = initial_noise_mean;
        }

        /** \brief Set the threshold to re-initialize the particles.
          * \param[in] resample_likelihood_thr threshold to re-initialize.
          */
        inline void
        setResampleLikelihoodThr (const double resample_likelihood_thr)
        {
          resample_likelihood_thr_ = resample_likelihood_thr;
        }
        
        /** \brief Set the threshold of angle to be considered occlusion (default: pi/2).
          * ParticleFilterTracker does not take the occluded points into account according to the angle
          * between the normal and the position. 
          * \param[in] occlusion_angle_thr threshold of angle to be considered occlusion.
          */
        inline void
        setOcclusionAngleThe (const double occlusion_angle_thr)
        {
          occlusion_angle_thr_ = occlusion_angle_thr;
        }
        
        /** \brief Set the minimum number of indices (default: 1).
          * ParticleFilterTracker does not take into account the hypothesis
          * whose the number of points is smaller than the minimum indices.
          * \param[in] min_indices the minimum number of indices.
          */
        inline void
        setMinIndices (const int min_indices) { min_indices_ = min_indices; }

        /** \brief Set the transformation from the world coordinates to the frame of the particles.
          * \param[in] trans Affine transformation from the worldcoordinates to the frame of the particles.
          */
        inline void setTrans (const Eigen::Affine3f &trans) { trans_ = trans; }
        
        /** \brief Get the transformation from the world coordinates to the frame of the particles. */
        inline Eigen::Affine3f getTrans () const { return trans_; }
        
	      /** \brief Get an instance of the result of tracking.
	        * This function returns the particle that represents the transform between the reference point cloud at the 
          * beginning and the best guess about its location in the most recent frame.
	        */
        inline StateT getResult () const override { return representative_state_; }
        
        /** \brief Convert a state to affine transformation from the world coordinates frame.
          * \param[in] particle an instance of StateT.
          */
        Eigen::Affine3f toEigenMatrix (const StateT& particle)
        {
          return particle.toEigenMatrix ();
        }

        /** \brief Get a pointer to a pointcloud of the particles. */
        inline PointCloudStatePtr getParticles () const { return particles_; }

        /** \brief Normalize the weight of a particle using \f$ std::exp(1- alpha ( w - w_{min}) / (w_max - w_min)) \f$
          * \note This method is described in [P.Azad et. al, ICRA11].
          * \param[in] w the weight to be normalized
          * \param[in] w_min the minimum weight of the particles
          * \param[in] w_max the maximum weight of the particles
          */
        inline double normalizeParticleWeight (double w, double w_min, double w_max)
        {
          return std::exp (1.0 - alpha_ * (w - w_min) / (w_max - w_min));
        }

        /** \brief Set the value of alpha.
          *  \param[in] alpha the value of alpha
          */
        inline void setAlpha (double alpha) { alpha_ = alpha; }
        
        /** \brief Get the value of alpha. */
        inline double getAlpha () { return alpha_; }

        /** \brief Set the value of use_normal_.
          * \param[in] use_normal the value of use_normal_.
          */
        inline void setUseNormal (bool use_normal) { use_normal_ = use_normal; }

        /** \brief Get the value of use_normal_. */
        inline bool getUseNormal () { return use_normal_; }

        /** \brief Set the value of use_change_detector_.
          * \param[in] use_change_detector the value of use_change_detector_.
          */
        inline void setUseChangeDetector (bool use_change_detector) { use_change_detector_ = use_change_detector; }

        /** \brief Get the value of use_change_detector_. */
        inline bool getUseChangeDetector () { return use_change_detector_; }

        /** \brief Set the motion ratio
          * \param[in] motion_ratio the ratio of hypothesis to use motion model.
         */
        inline void setMotionRatio (double motion_ratio) { motion_ratio_ = motion_ratio; }

        /** \brief Get the motion ratio. */
        inline double getMotionRatio () { return motion_ratio_;}

        /** \brief Set the number of interval frames to run change detection.
          * \param[in] change_detector_interval the number of interval frames.
          */
        inline void setIntervalOfChangeDetection (unsigned int change_detector_interval)
        {
          change_detector_interval_ = change_detector_interval;
        }

        /** \brief Get the number of interval frames to run change detection. */
        inline unsigned int getIntervalOfChangeDetection ()
        {
          return change_detector_interval_;
        }

        /** \brief Set the minimum amount of points required within leaf node to become serialized in change detection
          * \param[in] change_detector_filter the minimum amount of points required within leaf node
         */
        inline void setMinPointsOfChangeDetection (unsigned int change_detector_filter)
        {
          change_detector_filter_ = change_detector_filter;
        }

        /** \brief Set the resolution of change detection.
          * \param[in] resolution resolution of change detection octree
          */
        inline void setResolutionOfChangeDetection (double resolution) { change_detector_resolution_ = resolution; }

        /** \brief Get the resolution of change detection. */
        inline double getResolutionOfChangeDetection () { return change_detector_resolution_; }
        
        /** \brief Get the minimum amount of points required within leaf node to become serialized in change detection. */
        inline unsigned int getMinPointsOfChangeDetection ()
        {
          return change_detector_filter_;
        }
        
        /** \brief Get the adjustment ratio. */
        inline double
        getFitRatio() const { return fit_ratio_; }
        
        /** \brief Reset the particles to restart tracking*/
        virtual inline void resetTracking ()
        {
          if (particles_)
            particles_->points.clear ();
        }

      protected:

        /** \brief Compute the parameters for the bounding box of hypothesis pointclouds.
          * \param[out] x_min the minimum value of x axis.
          * \param[out] x_max the maximum value of x axis.
          * \param[out] y_min the minimum value of y axis.
          * \param[out] y_max the maximum value of y axis.
          * \param[out] z_min the minimum value of z axis.
          * \param[out] z_max the maximum value of z axis.
          */
        void calcBoundingBox (double &x_min, double &x_max,
                              double &y_min, double &y_max,
                              double &z_min, double &z_max);

        /** \brief Crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud.
          * \param[in] cloud a pointer to pointcloud to be cropped.
          * \param[out] output a pointer to be assigned the cropped pointcloud.
          */
        void cropInputPointCloud (const PointCloudInConstPtr &cloud, PointCloudIn &output);
                                  
        
        
        /** \brief Compute a reference pointcloud transformed to the pose that hypothesis represents.
          * \param[in] hypothesis a particle which represents a hypothesis.
          * \param[in] indices the indices which should be taken into account.
          * \param[out] cloud the resultant point cloud model dataset which is transformed to hypothesis.
         **/
        void computeTransformedPointCloud (const StateT& hypothesis,
                                           std::vector<int>& indices,
                                           PointCloudIn &cloud);

        /** \brief Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate 
          * indices taking occlusion into account.
          * \param[in] hypothesis a particle which represents a hypothesis.
          * \param[in] indices the indices which should be taken into account.
          * \param[out] cloud the resultant point cloud model dataset which is transformed to hypothesis.
          **/
        void computeTransformedPointCloudWithNormal (const StateT& hypothesis,
                                                     std::vector<int>& indices,
                                                     PointCloudIn &cloud);

        /** \brief Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate 
          * indices without taking occlusion into account.
          * \param[in] hypothesis a particle which represents a hypothesis.
          * \param[out] cloud the resultant point cloud model dataset which is transformed to hypothesis.
         **/
        void computeTransformedPointCloudWithoutNormal (const StateT& hypothesis,
                                                        PointCloudIn &cloud);

        
        /** \brief This method should get called before starting the actual computation. */
        bool initCompute () override;
        
        /** \brief Weighting phase of particle filter method. Calculate the likelihood of all of the particles and set the weights. */
        virtual void weight ();
        
        /** \brief Resampling phase of particle filter method. Sampling the particles according to the weights calculated 
          * in weight method. In particular, "sample with replacement" is archieved by walker's alias method.
          */
        virtual void resample ();
        
        /** \brief Calculate the weighted mean of the particles and set it as the result. */
        virtual void update ();

        /** \brief Normalize the weights of all the particels. */
        virtual void normalizeWeight ();

        /** \brief Initialize the particles. initial_noise_covariance_ and initial_noise_mean_ are used for Gaussian sampling. */
        void initParticles (bool reset);
        
        /** \brief Track the pointcloud using particle filter method. */
        void computeTracking () override;
        
        /** \brief Implementation of "sample with replacement" using Walker's alias method.
            about Walker's alias method, you can check the paper below:
             article{355749},
             author = {Walker, Alastair J.},
             title = {An Efficient Method for Generating Discrete
             Random Variables with General Distributions},
             journal = {ACM Trans. Math. Softw.},
             volume = {3},
             number = {3},
             year = {1977},
             issn = {0098-3500},
             pages = {253--256},
             doi = {http://doi.acm.org/10.1145/355744.355749},
             publisher = {ACM},
             address = {New York, NY, USA},
             }
             \param a an alias table, which generated by genAliasTable.
             \param q a table of weight, which generated by genAliasTable.
         */
        int sampleWithReplacement (const std::vector<int>& a, const std::vector<double>& q);
        
        /** \brief Generate the tables for walker's alias method. */
        void genAliasTable (std::vector<int> &a, std::vector<double> &q, const PointCloudStateConstPtr &particles);

        /** \brief Resampling the particle with replacement. */
        void 
        resampleWithReplacement ();
        
        /** \brief Resampling the particle in deterministic way. */
        void 
        resampleDeterministic ();

        /** \brief Run change detection and return true if there is a change.
          * \param[in] input a pointer to the input pointcloud.
          */
        bool 
        testChangeDetection (const PointCloudInConstPtr &input);
        
        /** \brief The number of iteration of particlefilter. */
        int iteration_num_;

        /** \brief The number of the particles. */
        int particle_num_;

        /** \brief The minimum number of points which the hypothesis should have. */
        int min_indices_;
	
        /** \brief Adjustment of the particle filter. */
        double fit_ratio_;

        /** \brief A pointer to reference point cloud. */
        PointCloudInConstPtr ref_;

        /** \brief A pointer to the particles  */
        PointCloudStatePtr particles_;

        /** \brief A pointer to PointCloudCoherence. */
        CloudCoherencePtr coherence_;

        /** \brief The diagonal elements of covariance matrix of the step noise. the covariance matrix is used
          * at every resample method.
          */
        std::vector<double> step_noise_covariance_;

        /** \brief The diagonal elements of covariance matrix of the initial noise. the covariance matrix is used
          * when initialize the particles.
          */
        std::vector<double> initial_noise_covariance_;
        
        /** \brief The mean values of initial noise. */
        std::vector<double> initial_noise_mean_;

        /** \brief The threshold for the particles to be re-initialized. */
        double resample_likelihood_thr_;

        /** \brief The threshold for the points to be considered as occluded. */
        double occlusion_angle_thr_;

        /** \brief The weight to be used in normalization of the weights of the particles. */
        double alpha_;
        
        /** \brief The result of tracking. */
        StateT representative_state_;

        /** \brief An affine transformation from the world coordinates frame to the origin of the particles. */
        Eigen::Affine3f trans_;

        /** \brief A flag to use normal or not. defaults to false. */
        bool use_normal_;

        /** \brief Difference between the result in t and t-1. */
        StateT motion_;

        /** \brief Ratio of hypothesis to use motion model. */
        double motion_ratio_;

        /** \brief Pass through filter to crop the pointclouds within the hypothesis bounding box. */
        pcl::PassThrough<PointInT> pass_x_;
        /** \brief Pass through filter to crop the pointclouds within the hypothesis bounding box. */
        pcl::PassThrough<PointInT> pass_y_;
        /** \brief Pass through filter to crop the pointclouds within the hypothesis bounding box. */
        pcl::PassThrough<PointInT> pass_z_;

        /** \brief A list of the pointers to pointclouds. */
        std::vector<PointCloudInPtr> transed_reference_vector_;

        /** \brief Change detector used as a trigger to track. */
        typename pcl::octree::OctreePointCloudChangeDetector<PointInT>::Ptr change_detector_;

        /** \brief A flag to be true when change of pointclouds is detected. */
        bool changed_;

        /** \brief A counter to skip change detection. */
        unsigned int change_counter_;
        
        /** \brief Minimum points in a leaf when calling change detector. defaults to 10. */
        unsigned int change_detector_filter_;

        /** \brief The number of interval frame to run change detection. defaults to 10. */
        unsigned int change_detector_interval_;

        /** \brief Resolution of change detector. defaults to 0.01. */
        double change_detector_resolution_;
        
        /** \brief The flag which will be true if using change detection. */
        bool use_change_detector_;
    };
  }
}

// #include <pcl/tracking/impl/particle_filter.hpp>
#ifdef PCL_NO_PRECOMPILE
#include <pcl/tracking/impl/particle_filter.hpp>
#endif
