#include <ros/ros.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/Float32.h>
#include <std_srvs/Trigger.h>
#include <std_srvs/Empty.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Twist.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib_msgs/GoalStatusArray.h>
#include <actionlib/client/simple_action_client.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <visualization_msgs/MarkerArray.h>

#include <yaml-cpp/yaml.h>
#include <waypoint_saver.h>  // include Waypoint class

#include <vector>
#include <fstream>
#include <string>
#include <exception>
#include <math.h>

#ifdef NEW_YAMLCPP
template<typename T>
void operator >> (const YAML::Node& node, T& i)
{
    i = node.as<T>();
}
#endif


class SwitchRunningStatus : public std::exception {
public:
    SwitchRunningStatus() : std::exception() { }
};



class WaypointsNavigation{
public:
    /*
    ++++++++++ Constructer ++++++++++
    */
    WaypointsNavigation() :
        has_activate_(false),
        stopped_(false),
        move_base_action_("move_base", true),
        rate_(10),
        last_moved_time_(0),
        dist_err_(1.0),
        min_dist_err_(0.3),
        min_yaw_err_(0.3)
    {
        while((move_base_action_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true))
        {
            ROS_INFO("Waiting...");
        }
        ros::NodeHandle private_nh("~");
        private_nh.param("robot_frame", robot_frame_, std::string("base_link"));
        private_nh.param("world_frame", world_frame_, std::string("map"));
        
        double max_update_rate;
        private_nh.param("max_update_rate", max_update_rate, 10.0);
        rate_ = ros::Rate(max_update_rate);
        std::string filename = "";
        private_nh.param("filename", filename, filename);
        if (filename != "") {
            ROS_INFO_STREAM("Read waypoints data from " << filename);
            if (!readFile(filename)) {
                ROS_ERROR("Failed loading waypoints file");
            } else {
                finish_pose_ = waypoints_.poses.end()-1;
                computeWpOrientation();
            }
            current_waypoint_ = waypoints_.poses.begin();
            wp_num_.data = 1;
        } else {
            ROS_ERROR("waypoints file doesn't have name");
        }
        
        //Scripts of "start from the middle"
        //Edited mori 2022/10/19
        //#############################################################
        StartFromTheMiddle = false;
        private_nh.getParam("StartFromTheMiddle", StartFromTheMiddle);
        //#############################################################
        
        private_nh.param("min_dist_err", min_dist_err_, min_dist_err_);
        private_nh.param("min_yaw_err", min_yaw_err_, min_yaw_err_);

        ros::NodeHandle nh;
        start_server_ = nh.advertiseService("start_wp_nav", &WaypointsNavigation::startNavigationCallback, this);
        stop_server_ = nh.advertiseService("stop_wp_nav", &WaypointsNavigation::stopNavigationCallback, this);
        resume_server_ = nh.advertiseService("resume_nav", &WaypointsNavigation::resumeNavigationCallback, this);
        cmd_vel_sub_ = nh.subscribe("cmd_vel", 1, &WaypointsNavigation::cmdVelCallback, this);
        wp_pub_ = nh.advertise<geometry_msgs::PoseArray>("waypoints", 10);
        max_vel_pub_ = nh.advertise<std_msgs::Float32>("max_vel", 5);
        wp_num_pub_ = nh.advertise<std_msgs::UInt16>("waypoint_num", 5);
        clear_costmaps_srv_ = nh.serviceClient<std_srvs::Empty>("/move_base/clear_costmaps");
    }



    /*
    ++++++++++ Read waypoints file ++++++++++
    */
    bool readFile(const std::string &filename)
    {
        waypoints_.poses.clear();
        try {
            std::ifstream ifs(filename.c_str(), std::ifstream::in);
            if (ifs.good() == false) return false;

            YAML::Node node;
            #ifdef NEW_YAMLCPP
                node = YAML::Load(ifs);
            #else
                YAML::Parser parser(ifs);
                parser.GetNextDocument(node);
            #endif

            // Read waypoints from yaml file
            #ifdef NEW_YAMLCPP
                const YAML::Node &wp_node_tmp = node["waypoints"];
                const YAML::Node *wp_node = wp_node_tmp ? &wp_node_tmp : NULL;
            #else
                const YAML::Node *wp_node = node.FindValue("waypoints");
            #endif
            geometry_msgs::Pose pose;
            Waypoint point;
            if (wp_node != NULL) {
                for(int i=0; i < wp_node->size(); i++) {
                    (*wp_node)[i]["point"]["x"] >> pose.position.x;
                    (*wp_node)[i]["point"]["y"] >> pose.position.y;
                    (*wp_node)[i]["point"]["z"] >> pose.position.z;
                    waypoints_.poses.push_back(pose);
                    point.x = pose.position.x;
                    point.y = pose.position.y;
                    point.z = pose.position.z;
                    try {
                        (*wp_node)[i]["point"]["vel"]  >> point.vel;
                    } catch(...) {} // Use default value
                    try {
                        (*wp_node)[i]["point"]["rad"]  >> point.rad;
                    } catch(...) {}
                    try {
                        (*wp_node)[i]["point"]["stop"] >> point.stop;
                    } catch(...) {}
                    waypoint_list_.push_back(point);
                }
            } else {
                return false;
            }
            
            // Read finish_pose
            #ifdef NEW_YAMLCPP
                const YAML::Node &fp_node_tmp = node["finish_pose"];
                const YAML::Node *fp_node = fp_node_tmp ? &fp_node_tmp : NULL;
            #else
                const YAML::Node *fp_node = node.FindValue("finish_pose");
            #endif
            if (fp_node != NULL) {
                (*fp_node)["pose"]["position"]["x"] >> pose.position.x;
                (*fp_node)["pose"]["position"]["y"] >> pose.position.y;
                (*fp_node)["pose"]["position"]["z"] >> pose.position.z;
                (*fp_node)["pose"]["orientation"]["x"] >> pose.orientation.x;
                (*fp_node)["pose"]["orientation"]["y"] >> pose.orientation.y;
                (*fp_node)["pose"]["orientation"]["z"] >> pose.orientation.z;
                (*fp_node)["pose"]["orientation"]["w"] >> pose.orientation.w;
                waypoints_.poses.push_back(pose);
                point.x = pose.position.x;
                point.y = pose.position.y;
                point.z = pose.position.z;
                point.stop = true;
                waypoint_list_.push_back(point);
            } else {
                return false;
            }

        } catch(YAML::ParserException &e) {
            return false;

        } catch(YAML::RepresentationException &e) {
            return false;
        }

        return true;
    }



    /*
    ++++++++++ Compute target orientation for each waypoint ++++++++++
    */
    void computeWpOrientation()
    {
        for(std::vector<geometry_msgs::Pose>::iterator it = waypoints_.poses.begin(); it != finish_pose_; it++) {
            double goal_direction = atan2((it+1)->position.y - (it)->position.y,
                                          (it+1)->position.x - (it)->position.x);
            (it)->orientation = tf::createQuaternionMsgFromYaw(goal_direction);
        }
        waypoints_.header.frame_id = world_frame_;
    }



    /*
    ++++++++++ Callback function for "StartWaypointsNavigation" button on rviz  ++++++++++
    */
    bool startNavigationCallback(std_srvs::Trigger::Request &request, std_srvs::Trigger::Response &response)
    {
        if ((has_activate_) || (current_waypoint_ != waypoints_.poses.begin())) {
            ROS_WARN("Waypoint navigation is already started");
            response.success = false;
            return false;
        }
        //Scripts of "start from the middle"
        //Edited mori 2022/10/19
        //#############################################################
        wp_num_.data = 1;
        init_waypoint_ = waypoints_.poses.begin();
        compare_waypoint_ = waypoints_.poses.begin();
        double min_dist_w = std::numeric_limits<int>::max();
        if(StartFromTheMiddle){
            ROS_INFO("StartFromTheMiddle => True");
            tf::StampedTransform robot_init_pos = getRobotPosGL();
            double init_x = robot_init_pos.getOrigin().x();
            double init_y = robot_init_pos.getOrigin().y();
            double pre_waypoint_x;
            double pre_waypoint_y;
            double convert_x;
            double init_yaw = tf::getYaw(robot_init_pos.getRotation());
            double square_x;
            double square_y;
            double dist_w;
            double dir_w;
            int min_waypoint = 0;
            int i = 0;
            while(compare_waypoint_ != waypoints_.poses.end() - 1){
                i++;
                square_x = (compare_waypoint_ -> position.x - init_x) * (compare_waypoint_ -> position.x - init_x);
                square_y = (compare_waypoint_ -> position.y - init_y) * (compare_waypoint_ -> position.y - init_y);
                dist_w = sqrt(square_x + square_y);
                if(i == 1){
                    dir_w = atan2(compare_waypoint_ -> position.y, compare_waypoint_ -> position.x);
                    convert_x = (compare_waypoint_ -> position.x - init_x) * cos(-init_yaw) - (compare_waypoint_ -> position.y - init_y) * sin(-init_yaw);
                    if(abs(dir_w - init_yaw) < (90 / 180.0 * M_PI) && convert_x > 0){
                        min_dist_w = dist_w;
                        init_waypoint_ = compare_waypoint_;
                        min_waypoint = 1;
                    }
                }else if(min_dist_w > dist_w){
                    dir_w = atan2(compare_waypoint_ -> position.y - pre_waypoint_y, compare_waypoint_ -> position.x- pre_waypoint_x);
                    convert_x = (compare_waypoint_ -> position.x - init_x) * cos(-init_yaw) - (compare_waypoint_ -> position.y - init_y) * sin(-init_yaw);
                    if(abs(dir_w - init_yaw) < (90 / 180.0 * M_PI) && convert_x > 0){
                        min_dist_w = dist_w;
                        init_waypoint_ = compare_waypoint_;
                        min_waypoint = i;
                    }
                }
                pre_waypoint_x = compare_waypoint_ -> position.x;
                pre_waypoint_y = compare_waypoint_ -> position.y;
                compare_waypoint_++;
            }
            wp_num_.data = min_waypoint;
        }
        std_srvs::Empty empty;
        while(!clear_costmaps_srv_.call(empty)) {
            ROS_WARN("Resend clear costmap service");
            sleep();
        }
        if(min_dist_w == std::numeric_limits<int>::max()){
            ROS_WARN("Could not find the closest appropriate waypoint. Please check the direction of the robot again.");
            response.success = false;
            return false;
        }else{
            current_waypoint_ = init_waypoint_;
            has_activate_ = true;
            response.success = true;
            return true;
        }
        //#############################################################
    }



    /*
    ++++++++++ Callback function for "ResumeWaypointsNavigation" button on rviz  ++++++++++
    */
    bool resumeNavigationCallback(std_srvs::Trigger::Request &request, std_srvs::Trigger::Response &response)
    {
        if (has_activate_) {
            ROS_WARN("Navigation is already active");
            response.success = false;
        } else {
            ROS_INFO("Navigation has resumed");
            has_activate_ = true;
            response.success = true;
        }
        return true;
    }


    /*
    ++++++++++ Callback function for service of "stop_wp_nav"  ++++++++++
    */
    bool stopNavigationCallback(std_srvs::Trigger::Request &request, std_srvs::Trigger::Response &response)
    {
        if (has_activate_) {
            ROS_INFO("Waypoint navigation has been stopped");
            has_activate_ = false;
            response.success = true;
        } else {
            ROS_WARN("Waypoint navigation is already stopped");
            response.success = false;
        }
        return true;
    }



    /*
    ++++++++++ Callback function for /cmd_vel topic  ++++++++++
    */
    void cmdVelCallback(const geometry_msgs::Twist &msg)
    {
        if (has_activate_ &&
            msg.linear.x > -0.001 && msg.linear.x < 0.001 &&
            msg.linear.y > -0.001 && msg.linear.y < 0.001 &&
            msg.linear.z > -0.001 && msg.linear.z < 0.001 &&
            msg.angular.x > -0.001 && msg.angular.x < 0.001 &&
            msg.angular.y > -0.001 && msg.angular.y < 0.001 &&
            msg.angular.z > -0.001 && msg.angular.z < 0.001)
        {
            // ROS_INFO("command velocity all zero");
        } else {
            last_moved_time_ = ros::Time::now().toSec();
        }
    }



    /*
    ++++++++++ Check if robot reached the goal sent to move_base ++++++++++
    */
    bool navigationFinished()
    {
        if (move_base_action_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
            ROS_WARN("Move base state is Goal reached");
            return true;
        }
        return false;
    }



    /*
    ++++++++++ Check if robot reached current waypoint ++++++++++
    */
    bool onNavigationPoint(const geometry_msgs::Point &dest, double dist_err=1.0)
    {
        tf::StampedTransform robot_gl = getRobotPosGL();
        const double wx = dest.x;
        const double wy = dest.y;
        const double rx = robot_gl.getOrigin().x();
        const double ry = robot_gl.getOrigin().y();
        const double dist = std::sqrt(std::pow(wx - rx, 2) + std::pow(wy - ry, 2));
        if (waypoint_list_[wp_num_.data-1].stop) {
            tf::Quaternion q = robot_gl.getRotation();
            tf::Matrix3x3 m(q);
            double r, p ,y;
            m.getRPY(r, p, y);
            double diff_yaw = std::abs(y - target_yaw_);
            if ((dist < min_dist_err_) && (diff_yaw < min_yaw_err_)) return true;
            else if ((dist < min_dist_err_ * 2) && navigationFinished()) return true;
            return false;
        }
        return dist < dist_err;
    }



    /*
    ++++++++++ Get gloabl position of robot ++++++++++
    */
    tf::StampedTransform getRobotPosGL()
    {
        tf::StampedTransform robot_gl;
        try {
            tf_listener_.lookupTransform(world_frame_, robot_frame_, ros::Time(0.0), robot_gl);
        } catch(tf::TransformException &e) {
            ROS_WARN_STREAM("tf::TransformException: " << e.what());
        }
        return robot_gl;
    }



    /*
    ++++++++++ Sleep function used in main loop function ++++++++++
    */
    void sleep()
    {
        rate_.sleep();
        ros::spinOnce();
        publishPoseArray();
    }



    /*
    ++++++++++ Publish waypoints to be displayed as arrows on rviz ++++++++++
    */
    void publishPoseArray()
    {
        waypoints_.header.stamp = ros::Time::now();
        wp_pub_.publish(waypoints_);
    }



    /*
    ++++++++++ Send goal to move_base ++++++++++
    */
    void startNavigationGL(const geometry_msgs::Pose &dest)
    {
        setMaxVel();
        move_base_msgs::MoveBaseGoal move_base_goal;
        move_base_goal.target_pose.header.stamp = ros::Time::now();
        move_base_goal.target_pose.header.frame_id = world_frame_;
        move_base_goal.target_pose.pose.position = dest.position;
        move_base_goal.target_pose.pose.orientation = dest.orientation;
        move_base_action_.sendGoal(move_base_goal);
        sleep();
        if (waypoint_list_[wp_num_.data-1].stop) {
            tf::Quaternion q(dest.orientation.x, dest.orientation.y, dest.orientation.z, dest.orientation.w);
            tf::Matrix3x3 m(q);
            double r, p ,y;
            m.getRPY(r, p, y);
            target_yaw_ = y;
        }
    }



    /*
    ++++++++++ Publish ratio to maximum speed ++++++++++
    */
    void setMaxVel()
    {
        float max_vel_rate = waypoint_list_[wp_num_.data-1].vel;
        max_vel_msg_.data = max_vel_rate;
        max_vel_pub_.publish(max_vel_msg_);
    }



    /*
    ++++++++++ Main loop function ++++++++++
    */
    void run()
    {
        ROS_INFO("Waiting for waypoint navigation to start.");
        int resend_goal;
        double start_nav_time, time;
        std_srvs::Empty empty;
        while(ros::ok()) {
            // has_activate_ is false, nothing to do
            if (!has_activate_) {
                sleep();
                continue;
            }
            // go to fianal goal and finish process
            if (current_waypoint_ == finish_pose_) {
                ROS_INFO_STREAM("Go to final goal");
                startNavigationGL(*current_waypoint_);
                start_nav_time = ros::Time::now().toSec();
                while(!onNavigationPoint(finish_pose_->position) && ros::ok()) {
                    time = ros::Time::now().toSec();
                    if (time - start_nav_time > 10.0 && time - last_moved_time_ > 10.0) {
                        ROS_WARN("Resend the navigation goal.");
                        clear_costmaps_srv_.call(empty);
                        startNavigationGL(*current_waypoint_);
                        start_nav_time = time;
                    }
                    sleep();
                }
                ROS_INFO("Final goal reached!!");
                has_activate_ = false;
                continue;
            }
            // go to current waypoint
            ROS_INFO_STREAM("Go to waypoint " << wp_num_.data);
            ROS_INFO_STREAM("x: " << waypoint_list_[wp_num_.data-1].x << ", y: " << waypoint_list_[wp_num_.data-1].y);
            startNavigationGL(*current_waypoint_);
            resend_goal = 0;
            start_nav_time = ros::Time::now().toSec();
            dist_err_ = waypoint_list_[wp_num_.data-1].rad;
            try {
                // loop until reach waypoint
                while(!onNavigationPoint(current_waypoint_->position, dist_err_)) {
                    if (!has_activate_) throw SwitchRunningStatus();
                    time = ros::Time::now().toSec();
                    if (time - start_nav_time > 10.0 && time - last_moved_time_ > 10.0) {
                        ROS_WARN("Resend the navigation goal.");
                        clear_costmaps_srv_.call(empty);
                        startNavigationGL(*current_waypoint_);
                        resend_goal++;
                        if (resend_goal == 3) {
                            ROS_WARN("Skip waypoint.");
                            break;
                        }
                        start_nav_time = time;
                    }
                    wp_num_pub_.publish(wp_num_);
                    sleep();
                }
                // if current waypoint is stop point
                if (waypoint_list_[wp_num_.data-1].stop) {
                    has_activate_ = false;
                    move_base_action_.cancelAllGoals();
                    ROS_INFO("Waiting for navigation to resume...");
                }
                // update current waypoint
                current_waypoint_++;
                wp_num_.data++;
                wp_num_pub_.publish(wp_num_);

            } catch(const SwitchRunningStatus &e) {
                move_base_action_.cancelAllGoals();
                ROS_INFO_STREAM("Running status switched");
            }
            sleep();
        }
    }




private:
    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_action_;
    geometry_msgs::PoseArray waypoints_;
    visualization_msgs::MarkerArray marker_;
    std::vector<Waypoint> waypoint_list_;
    
    //Scripts of "start from the middle"
    //Edited mori 2022/10/19
    //#############################################################
    bool StartFromTheMiddle;
    std::vector<geometry_msgs::Pose>::iterator init_waypoint_;
    std::vector<geometry_msgs::Pose>::iterator compare_waypoint_;
    //#############################################################
    
    std::vector<geometry_msgs::Pose>::iterator current_waypoint_;
    std::vector<geometry_msgs::Pose>::iterator finish_pose_;
    bool has_activate_, stopped_;
    std::string robot_frame_, world_frame_;
    tf::TransformListener tf_listener_;
    ros::Rate rate_;
    ros::ServiceServer start_server_, stop_server_, resume_server_;
    ros::Subscriber cmd_vel_sub_, move_base_status_sub_;
    ros::Publisher wp_pub_, max_vel_pub_, wp_num_pub_;
    ros::ServiceClient clear_costmaps_srv_;
    double last_moved_time_, dist_err_, min_dist_err_, target_yaw_, min_yaw_err_;
    std_msgs::UInt16 wp_num_;
    std_msgs::Float32 max_vel_msg_;
};






int main(int argc, char *argv[]) {
    ros::init(argc, argv, ROS_PACKAGE_NAME);
    WaypointsNavigation w_nav;
    w_nav.run();

    return 0;
}