/**
 * navigation.
 *
 * Copyright (C) 2019 成都河狸智能科技有限责任公司.
 * All rights reserved.
 *
 * Author: lumin
 */

#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <actionlib/server/simple_action_server.h>
#include <actionlib/client/simple_action_client.h>
#include <std_msgs/Float64.h>

#include <base_msgs/navigation_cmdAction.h>
#include <base_msgs/navigation_cmdFeedback.h>
#include <base_msgs/navigation_cmdResult.h>
#include "Eigen/Core"

#include "obstacle/obstacle.h"
#include "face/diff/diff.h"
#include "face/forklift/forklift.h"
#include "face/mecanum/mecanum.h"
#include "common/common.h"
#include "space_lib/space_lib.h"
#include "core/core.h"
// #include "space_key/space_key.h"

namespace nav { 
    typedef actionlib::SimpleActionServer<base_msgs::navigation_cmdAction> NavigationBaseActionServer;

    ros::Publisher local_path_pub_, global_path_pub_, cmd_pub_;
    ros::NodeHandle* nh_;

    NavigationBaseActionServer* as_;

    std::string motion_model_ = "diff";
    std::string cmd_vel_topic_ = "/cmd_vel";

    std::unique_ptr<planner::Core> nb_ = nullptr;
    std::unique_ptr<obs::ObstacleIdentify> obstacle_function_;
    
    int rate_;

    float minimal_vel_ = 0;
    float minimal_rot_ = 0;
    float location_cost_ = 0;
    float smooth_coeff_ = 0.4;
    float slow_threshold_ = 0.3;
    float stop_threshold_ = 0.15;
    float stop_dist_ = 0.8; 
    float slow_dist_ = 2.0;
    
    bool debug_ = false;

    common::NavState nav_state_;

    bool __executeCycle(const base_msgs::navigation_cmdGoal& cmd_goal);
    bool __getRobotPose(geometry_msgs::PoseStamped& current_pose);
    
    void __sendFeedback(const uint64_t& time_stamp, const std::string& info, int32_t status, int32_t progress, std::string addition_info = "");
    void __executeCb(const base_msgs::navigation_cmdGoalConstPtr& goal);
    void __mapPoseCallback(const geometry_msgs::Pose2DConstPtr& msg);
    void __buildTask(const base_msgs::navigation_cmdGoal& goal);
    void __switchNb(const base_msgs::navigation_cmdGoal& goal);
    void __publishZeroVelocity();

    float __smoothCoeff();

    geometry_msgs::Twist __commonTwist2Twist(common::Twist& twist);
    geometry_msgs::PoseStamped robot_current_position_;

    std::mutex map_pose_mutex_;
    #define LOCK_MAP_POSE() std::lock_guard<std::mutex> lock(map_pose_mutex_);

    std::mutex odom_mutex_;
    #define LOCK_ODOM() std::lock_guard<std::mutex> lock(odom_mutex_);

    static geometry_msgs::PoseStamped setRPY(double z) 
    {
        geometry_msgs::PoseStamped pose_stamp;

        tf::Quaternion q;
        q.setRPY(0, 0, z);

        pose_stamp.pose.orientation.w = q.getW();
        pose_stamp.pose.orientation.x = q.getX();
        pose_stamp.pose.orientation.y = q.getY();
        pose_stamp.pose.orientation.z = q.getZ();

        pose_stamp.pose.position.x = 0;
        pose_stamp.pose.position.y = 0;
        pose_stamp.pose.position.z = 0;

        return pose_stamp;
    }

    static geometry_msgs::PoseStamped __transformCommonPoint2PoseStamped(const common::Point& p) 
    {
        geometry_msgs::PoseStamped pose_stamp;

        tf::Transform transform;
        tf::Quaternion q;
        q.setRPY(0, 0, p.theta);

        pose_stamp.pose.position.x = p.x;
        pose_stamp.pose.position.y = p.y;

        pose_stamp.pose.orientation.w = q.getW();
        pose_stamp.pose.orientation.x = q.getX();
        pose_stamp.pose.orientation.y = q.getY();
        pose_stamp.pose.orientation.z = q.getZ();

        return pose_stamp;
    }

    static common::Point __transformPoseStamped2CommonPoint(const geometry_msgs::PoseStamped& pose) 
    {
        common::Point p;

        p.x = pose.pose.position.x;
        p.y = pose.pose.position.y;
        p.theta = tf::getYaw(pose.pose.orientation);

        return p;
    }

    static common::Path __transformRosPath2CommonPath(const std::vector<geometry_msgs::Pose2D>& ros_path) 
    {
        common::Path path;

        for (const geometry_msgs::Pose2D& pose : ros_path) {
            common::Point p;

            p.x = pose.x;
            p.y = pose.y;
            p.theta = pose.theta;
            path.push_back(p);
        }
        return path;
    }

    static geometry_msgs::PoseStamped __transformCommonPoint2PoseStamped(common::Point& p) 
    {
        geometry_msgs::PoseStamped pst = setRPY(p.theta);

        pst.pose.position.x = p.x;
        pst.pose.position.y = p.y;
        
        return pst;
    }

    static nav_msgs::Path __transformCommonPath2RosPath(const common::Path& path) 
    {
        nav_msgs::Path ros_path;

        for (const common::Point& pose : path) {
            ros_path.poses.push_back(__transformCommonPoint2PoseStamped(pose));
        }
        ros_path.header.stamp = ros::Time::now();
        ros_path.header.frame_id = "/map";
        return ros_path;
    }

    /**
     *将point转到base坐标系, point和base需要在同一个坐标系
     */
    static common::Point __bT(common::Point& point, common::Point& base)
    {
        Eigen::Matrix<float, 2, 1> t2(base.x, base.y);
        Eigen::Rotation2D<float> r2(base.theta);

        Eigen::Matrix<float, 2, 1> t1(point.x, point.y);
        Eigen::Rotation2D<float> r1(point.theta);

        Eigen::Matrix<float, 2, 1> t = r2.inverse() * t1 - r2.inverse() * t2;
        Eigen::Rotation2D<float> r = r2.inverse() * r1;

        common::Point bt;

        bt.x = t(0);
        bt.y = t(1);
        bt.theta = common::norm(r.angle());

        return bt;
    }

    Json::Value __config()
    {
        std::string path = space::home() + "configure/nav_config.json";

        Json::Value c = space::readConfig(path);
        Json::Value cy = c;

        space::jsonValueTo(c["motion_model"], motion_model_) ?
            false : (c["motion_model"] = motion_model_ = "diff");
        space::jsonValueTo(c["minimal_vel"], minimal_vel_) ?
            false : (c["minimal_vel"] = minimal_vel_ = 0);
        space::jsonValueTo(c["minimal_rot"], minimal_rot_) ?
            false : (c["minimal_rot"] = minimal_rot_ = 0);
        space::jsonValueTo(c["rate"], rate_)?
            false : (c["rate"] = rate_ = 50);
        space::jsonValueTo(c["cmd_vel_topic"], cmd_vel_topic_) ?
            false : (c["cmd_vel_topic"] = cmd_vel_topic_ = "cmd_vel");
        space::jsonValueTo(c["debug"], debug_) ?
            false : (c["debug"] = debug_ = false);
        space::jsonValueTo(c["slow_dist"], slow_dist_) ?
            false : (c["slow_dist"] = slow_dist_ = 2.0);
        space::jsonValueTo(c["stop_dist"], stop_dist_) ?
            false : (c["stop_dist"] = stop_dist_ = 0.8);
            
        if(cy != c) {
            space::writeConfig(path, c);
        }
        return c;
    }

    void run(tf::TransformListener& tf)
    {
        nh_ = new ros::NodeHandle();

        as_ = new NavigationBaseActionServer(ros::NodeHandle(), "nav_base", 
                                                boost::bind(&__executeCb, _1), false);

        static ros::Subscriber map_pose_sb = nh_->subscribe<geometry_msgs::Pose2D>(
                "map_pose", 1, &__mapPoseCallback);
        obstacle_function_ = std::unique_ptr<obs::ObstacleIdentify>(new obs::ObstacleIdentify());

        as_->start();

        __config();
        
        LOG(INFO)<<"cmd topic ["<<cmd_vel_topic_<<"]";
        LOG(INFO)<<"rate ["<<rate_<<"]";
        LOG(INFO)<<"motion model ["<<motion_model_<<"]";
        LOG(INFO)<<"!@#:navigation start done";
        ros::spin();
    }

    void __executeCb(const base_msgs::navigation_cmdGoalConstPtr& goal) 
    {
        ros::NodeHandle n;
        ros::Rate rate = ros::Rate(rate_);
        base_msgs::navigation_cmdGoal copy_goal = *goal;

        cmd_pub_ = nh_->advertise<geometry_msgs::Twist>(cmd_vel_topic_, 1);

        __buildTask(copy_goal);

        base_msgs::navigation_cmdGoal cmd_goal = *goal;

        while (n.ok() && ros::ok()) {
            if(as_->isPreemptRequested()) {
                if(as_->isNewGoalAvailable()) {
                    cmd_goal = *as_->acceptNewGoal();
                    
                    __buildTask(cmd_goal);
                }
                else {
                    as_->setPreempted();
                    break;
                }
            }
            rate.sleep();
            ros::spinOnce();

            if (__executeCycle(cmd_goal)) {
                base_msgs::navigation_cmdResult result;

                result.result = 1;
                result.type = "done";
                as_->setSucceeded(result, "Goal reached.");
                break;
            }
            if (debug_) {
                common::Path local_path = nb_->getLocalPath();
                common::Path global_path = nb_->getGlobalPath();

                local_path_pub_.publish(__transformCommonPath2RosPath(local_path));
                global_path_pub_.publish(__transformCommonPath2RosPath(global_path));
            }
        }
        LOG(INFO)<<"done !!";

        cmd_pub_.shutdown();
        nb_->destroy();
        obstacle_function_->stop();

        return;
    }

    void __buildTask(const base_msgs::navigation_cmdGoal& goal) 
    {
        __config();
        __switchNb(goal);
        obstacle_function_->start();
        common::Config config = nb_->getConfig();
        
        config.max_linear = goal.max_linear;
        config.max_transla = goal.max_linear_y;
        config.max_angular = goal.max_angular;
        // config.toler_x = goal.x_tolerate;
        // config.toler_y = goal.y_tolerate;
        // config.toler_theta = goal.yaw_tolerate;
        
        nb_->setConfig(config);

        common::Path path;

        path = __transformRosPath2CommonPath(goal.goal);
        if (goal.towards.size() == path.size()) {
            for (int i = 0;i < goal.towards.size();i++) {
                path[i].head = goal.towards[i];
            }
        }
        if (goal.limit_linear_x.size() == path.size()) {
            for (int i = 0;i < goal.limit_linear_x.size();i++) {
                path[i].max_linear = goal.limit_linear_x[i];
            }
        }
        nb_->setPath(path);

        if (debug_) {
            local_path_pub_ = nh_->advertise<nav_msgs::Path>("local_path", 1);
            global_path_pub_ = nh_->advertise<nav_msgs::Path>("global_path", 1);
        }
    }

    common::NavState __obstacle(common::Point& sta_pose, common::Twist& cmd)
    {
        float distance = 0.0;

        if (obstacle_function_->isClose(sta_pose)) {
            return common::NavState::ING;
        }
        // 已经撞上？
        if (obstacle_function_->isObstacleInArea(0.0, 0.0, 0.0, distance)) {
            return common::NavState::HIT_OBSTACLE;
        }
        if (fabs(cmd.x) < 1e-3) { // 0.001 m/s, 旋转避障判断
            float ff = cmd.angular > 0 ? 0.17 : -0.17;

            if (obstacle_function_->isObstacleInArea(0.0, 0.0, ff, distance)) {
                return common::NavState::HIT_OBSTACLE;
            }
            else {
                return common::NavState::ING;
            }
        }

        common::Path pah = nb_->getLocalPath();

        for (int i = 0;i < (int)pah.size() - 1;i++) {
            pah[i].theta = atan2(pah[i + 1].y  - pah[i].y, pah[i + 1].x  - pah[i].x);
            
            common::Point bt = __bT(pah[i], sta_pose);

            if (obstacle_function_->isObstacleInArea(bt.x, bt.y, bt.theta, distance)) {
                if (distance < slow_dist_) {
                    float k = distance / (slow_dist_ * 2.0);

                    cmd.x *= k;
                    cmd.y *= k;
                    cmd.angular *= k;
                }
                if (distance < stop_dist_) {
                    cmd = common::Twist();
                    return common::NavState::STOP;
                }

                return common::NavState::PATH_BLOCK;
            }
        }

        common::Path pah2 = nb_->interPath(2);

        for (int i = 0;i < (int)pah2.size() - 1;i++) {
            pah2[i].theta = atan2(pah2[i + 1].y  - pah2[i].y, pah2[i + 1].x  - pah2[i].x);
            
            common::Point bt = __bT(pah2[i], sta_pose);

            if (obstacle_function_->isObstacleInArea(bt.x, bt.y, bt.theta, distance)) {
                if (distance < slow_dist_) {
                    float k = distance / (slow_dist_ * 2.0);

                    cmd.x *= k;
                    cmd.y *= k;
                    cmd.angular *= k;
                }
                if (distance < stop_dist_) {
                    cmd = common::Twist();
                    return common::NavState::STOP;
                }
                return common::NavState::PATH_BLOCK;
            }
        }
        return common::NavState::ING;
    }
    
    bool __executeCycle(const base_msgs::navigation_cmdGoal& cmd_goal) 
    {
        geometry_msgs::PoseStamped current_pose = setRPY(0);

        if (!__getRobotPose(current_pose)) {
            __sendFeedback(cmd_goal.time_stamp, "location error", 0, 0);
            return false;
        }

        common::Twist twist;
        common::Point po = __transformPoseStamped2CommonPoint(current_pose);

        nav_state_ =  
            nb_->computeVelocityCommands(po, twist);

        common::NavState obsS = __obstacle(po, twist);

        if (obsS != common::NavState::ING) {
            nav_state_ = obsS;
        }
        geometry_msgs::Twist cmd_vel = __commonTwist2Twist(twist);

        if (std::isnan(cmd_vel.linear.x) || 
            std::isnan(cmd_vel.angular.z) ||
            std::isnan(cmd_vel.linear.y) ||
            std::isinf(cmd_vel.linear.x) || 
            std::isinf(cmd_vel.angular.z) || 
            std::isinf(cmd_vel.linear.y))
        {
            __sendFeedback(cmd_goal.time_stamp, "speed error", 2001, 0);
            __publishZeroVelocity();
            return false;
        }
        if (nav_state_ == common::NavState::ING) {
            __sendFeedback(cmd_goal.time_stamp, "ing", 0, 0);
            cmd_pub_.publish(cmd_vel);
        }
        else if (nav_state_ == common::NavState::ROTATE) {
            __sendFeedback(cmd_goal.time_stamp, "rotate", 0, 0);
            cmd_pub_.publish(cmd_vel);
        }
        else if (nav_state_ == common::NavState::HEAD_WARN) {
            __sendFeedback(cmd_goal.time_stamp, "head wran", 0, 0);
            cmd_pub_.publish(cmd_vel);
        }
        else if (nav_state_ == common::NavState::PATH_BLOCK) {
            __sendFeedback(cmd_goal.time_stamp, "path block", 2002, 0);
            cmd_pub_.publish(cmd_vel);
        }
        else if (nav_state_ == common::NavState::STOP) {
            __sendFeedback(cmd_goal.time_stamp, "path block and stop", 2006, 0);
            cmd_pub_.publish(cmd_vel);
        }
        else if (nav_state_ == common::NavState::HIT_OBSTACLE) {
            __sendFeedback(cmd_goal.time_stamp, "hit obstacle", 2001, 0);
            __publishZeroVelocity();
        }
        else if (nav_state_ == common::NavState::PATH_EMPTY) {
            __sendFeedback(cmd_goal.time_stamp, "path is empty", 2004, 0);
            __publishZeroVelocity();
        }
        else if (nav_state_ == common::NavState::OFF_COURSE) {
            __sendFeedback(cmd_goal.time_stamp, "start too far", 2003, 0);
            __publishZeroVelocity();
        }
        else if (nav_state_ == common::NavState::DONE) {
            float d = hypot(current_pose.pose.position.x - cmd_goal.goal.back().x, 
                                current_pose.pose.position.y - cmd_goal.goal.back().y);

            if (d > cmd_goal.x_tolerate || d > cmd_goal.y_tolerate) {
                __sendFeedback(cmd_goal.time_stamp, "low precision", 2005, 0);
            }
            __publishZeroVelocity();
            
            return true;
        }
        
        return false;
    }

    bool __getRobotPose(geometry_msgs::PoseStamped& current_pose) 
    {
        LOCK_MAP_POSE()
        if ((ros::Time::now() - robot_current_position_.header.stamp).toSec() > 1.0) {
            return false;
        }
        current_pose = robot_current_position_;

        return true;
    }

    geometry_msgs::Twist __commonTwist2Twist(common::Twist& twist) 
    {
        geometry_msgs::Twist cmd_vel;

        cmd_vel.linear.x = twist.x;
        cmd_vel.linear.y = twist.y;
        cmd_vel.angular.z = twist.angular;

        bool xb = fabs(twist.x) < minimal_vel_;
        bool yb = fabs(twist.y) < minimal_vel_;
        bool rb = fabs(twist.angular) < minimal_rot_;
        
        if (xb && yb && rb) {
            float k = 1;

            if (fabs(twist.x) > 1e-3) {
                k = minimal_vel_ / twist.x;
            }else if (fabs(twist.angular) > 1e-3) {
                k = minimal_rot_ / twist.angular;
            }else if (fabs(twist.y) > 1e-3) {
                k = minimal_vel_ / twist.y;
            }

            k = fabs(k);
            cmd_vel.linear.x *= k;
            cmd_vel.linear.y *= k;
            cmd_vel.angular.z *= k; 
        }
        
        return cmd_vel;
    }

    void __mapPoseCallback(const geometry_msgs::Pose2DConstPtr& msg) 
    {
        geometry_msgs::PoseStamped current_pose;

        current_pose = setRPY(msg->theta);
        current_pose.header.stamp = ros::Time::now();
        current_pose.header.frame_id = "map";
        current_pose.pose.position.x = msg->x;
        current_pose.pose.position.y = msg->y;
        
        LOCK_MAP_POSE()
        robot_current_position_ = std::move(current_pose);
    }

    void __sendFeedback(const uint64_t& time_stamp, const std::string& info, int32_t status, int32_t progress, std::string addition_info) 
    {
        base_msgs::navigation_cmdFeedback feedback;

        feedback.time_stamp = time_stamp;
        feedback.info = info;
        feedback.status = status;
        feedback.progress = progress;
        feedback.addition_info = addition_info;
        as_->publishFeedback(feedback);
    }

    void __publishZeroVelocity() 
    {
        geometry_msgs::Twist cmd_vel;
        
        cmd_vel.linear.x = 0.0;
        cmd_vel.linear.y = 0.0;
        cmd_vel.angular.z = 0.0;
        cmd_pub_.publish(cmd_vel);
    }

    void __switchNb(const base_msgs::navigation_cmdGoal& goal)
    {
        if (nb_ != nullptr) {
            nb_->reset();
        }
        if (motion_model_ == "diff") {
            nb_ = std::unique_ptr<diff::Diff>(new diff::Diff());
        }
        else if (motion_model_ == "mecanum") {
            nb_ = std::unique_ptr<mecanum::Mecanum>(new mecanum::Mecanum());
        }
        else if (motion_model_ == "forklift") {
            nb_ = std::unique_ptr<forklift::FL>(new forklift::FL());
        }
    }

}

int main(int argc, char** argv){
    ros::init(argc, argv, "space_nav");

    // if(space::key::chk()) {
    //     LOG(WARNING) << "推荐使用正版软件";
    //     space::key::rt(86400);
    //     return 0;
    // }
    
    tf::TransformListener tf(ros::Duration(10));

    nav::run(tf);

    ros::spin();

    return(0);
}