#ifndef _SPACE_OBSTACLE_AREA_H_
#define _SPACE_OBSTACLE_AREA_H_

#include <vector>
#include <string>
#include <iostream>
#include <fstream>
#include <stdio.h>

#include <ros/ros.h>
#include <ros/package.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/Int32.h>
#include <geometry_msgs/PolygonStamped.h>
#include <sensor_msgs/PointCloud.h>

#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <space_lib/space_lib.h>
#include "obstacle/rigid_transform.h"
#include "common/common.h"
#include <glog/logging.h>

namespace obs
{
    namespace obs_private_common
    {
        typedef struct {
            float x;
            float y;
        }Point;

        typedef struct {
            float x;
            float y;
            float theta;
        }Pose2D;

        typedef struct {
            float x;
            float y;
            float z;
        }Pose3D;

        typedef struct {
            std::vector<Pose2D> poses;
        }Area;

        typedef struct {
            std::vector<Pose2D> poses;
        }Path;

        struct Twist{
            Twist() : translation(0), rotate(0), horizontal(0) 
            {}
            float translation;
            float horizontal;
            float rotate;
        };

        struct Obstacle {
            std::vector<Pose2D> obstacle_pose;
            ros::Time stamp;
        };
    } // namespace obs_private_common
    using namespace obs_private_common;

    class ObstacleIdentify
    {
        public:
            ObstacleIdentify();
            ~ObstacleIdentify();

            void updateFootPrint();
            bool isObstacleInArea(const float& x,
                                    const float& y,
                                    const float& theta,
                                    float& distance);
            bool isClose(common::Point pp);
            void stop()
            {
                for (auto& s : subs_) {
                    s.shutdown();
                }
                on_ = false;
            }
            
            void start();
        private:
            struct IgnoreParam {
                IgnoreParam() 
                {
                    x_upper = 0.0;
                    x_lower = 0.0;
                    y_upper = 0.0;
                    y_lower = 0.0;
                    angle_upper = 0.0;
                    angle_lower = 0.0;
                }
                float x_upper;
                float x_lower;
                float y_upper;
                float y_lower;
                float angle_upper;
                float angle_lower;
            };

            struct LaserParam {
                LaserParam() : min_range(0.0), max_range(1e10) 
                {}
                float min_range;
                float max_range;
                float angle_increment;

                std::vector<IgnoreParam> ignores;
            };

            struct CameraParam {
                CameraParam() : filter_step(3),
                    z_ceil(1e10),
                    z_floor(-1e10),
                    x_head(1e10),
                    x_tail(-1e10),
                    y_left(1e10),
                    y_right(-1e10),
                    pit_top(-1e10),
                    pit_bot(-1e10)
                {}
                int filter_step;

                float z_ceil;
                float z_floor;
                float x_head;
                float x_tail;
                float y_left;
                float y_right;
                float pit_top;
                float pit_bot;

                std::vector<IgnoreParam> ignores;
            };

            struct CloseClose {
                CloseClose() : 
                    x(0.0), 
                    y(0.0),
                    close_dist(0.0)
                {}
                float x;
                float y;
                float close_dist;
            };

            ros::NodeHandle nh_;
            
            void __laserCallback(const sensor_msgs::LaserScanConstPtr& msg);
            void __cameraCallback(const sensor_msgs::PointCloud2ConstPtr& msg);
            void __addObs(std::vector<Pose2D>& obs, const std::string sensor_name, ros::Time time);
            void __registerLaser();
            void __registerCamera();
            void __readConfig();
            void __getPrintFoot();

            std::vector<Pose2D> __getParseObs();
            
            bool __isIgnore(float x, float y, float angle, std::vector<IgnoreParam> ignores);
            bool __poseInArea(Pose2D point,Area area);

            Pose2D __trasformBaseLink(Pose2D pose, const Eigen::Matrix<float, 3, 3>& transform);
            Pose2D __localTransGlobal(Pose2D a, Pose2D b);

            std::vector<ros::Subscriber> subs_;
            ros::Publisher point_cloud_pub_, point_cloud3D_pub_;
            ros::Publisher dynamic_polygon_pub_;
            ros::Publisher static_polygon_pub_;
            
            std::vector<Pose2D> laser_date_;
            std::vector<CloseClose> close_closes_;
            std::map<std::string, Obstacle> obs_;
            std::map<std::string, CameraParam> camera_params_;
            std::map<std::string, LaserParam> laser_params_;

            Area foot_;

            std::map<std::string, Eigen::Matrix<float, 4, 4>> transforms_;

            bool debug_, debug_3d_, turn_off_, on_;

            float min_range_, max_range_;

            boost::mutex mutex_;
    };
}



#endif