/* 
 * File:   poseEstimation.h
 * Author: jorge
 *
 * Created on 20 de noviembre de 2013, 12:14
 */

#ifndef POSEESTIMATION_H
#define	POSEESTIMATION_H

#include <fstream>

#include "pedestrianModel.h"

#include "geometry.h"
#include "colormap.hpp"

#include <boost/assign/list_of.hpp>

#include <pcl/point_cloud.h>

#include <pcl/kdtree/kdtree.h>
#include <pcl/kdtree/kdtree_flann.h>

#include <pcl/filters/extract_indices.h>

#include <pcl/common/impl/centroid.hpp>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
#include <pcl/common/distances.h>
#include <pcl/common/common_headers.h>

#include <pcl/features/feature.h> 

#include <pcl/segmentation/extract_clusters.h>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>

#include "rapidxml.hpp"
#include "rapidxml_print.hpp"

class PoseEstimation
{
    public:
        
        PoseEstimation()
        {
            tree_.reset(new pcl::search::KdTree<pcl::PointXYZRGB>);
        }
    
        void segmentPedestrian(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud_original);
        void getPose();
        
        void upperLegComposedSegmentation();
        void upperArmIndependentSegmentation();
        void lowerArmIndependentSegmentation();
        
        void writeVersion(std::string file);
        void savePoseText(std::string file_name);
        void loadPoseText(std::string file_name);
        
        void savePoseXML(std::string file_name);
        void loadPoseXML(std::string file_name);
        void writeBodyPartXML(rapidxml::xml_node<>* node,BodyPart& part,std::string name);
        void writePointXYZRGBXML(rapidxml::xml_node<>* parent_node,pcl::PointXYZRGB point,std::string name);
        void writeEllipseXML(rapidxml::xml_node<>* parent_node,Ellipse& ellipse,std::string name);
        void loadBodyPartXML(rapidxml::xml_node<>*node,BodyPart& part);
        void loadPointXYZRGBXMP(rapidxml::xml_node<>*node,pcl::PointXYZRGB& point);
        
        PedestrianModel model_;
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_pedestrian_;
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_removed_arms_;
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_accumulation_;
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_bellow_knees_;
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_top_body_;
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_bellow_waist_;
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_knees_;
        
        pcl::PointXYZRGB head_position_preivous_;
        pcl::PointXYZRGB head_position_current_;
        
    private:

        pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree_;
        pcl::ExtractIndices<pcl::PointXYZRGB> extract_indices_;
        pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> euclidean_clustering_;        
        
        rapidxml::xml_document<> doc;
};



#endif	/* POSEESTIMATION_H */