#include "nav_c_api.h"
#include <move_base_core/navigation.h>
#include <move_base_core/common.h>
#include <move_base/move_base.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Pose2D.h>
#include <robot_geometry_msgs/Point.h>
#include <robot_geometry_msgs/Vector3.h>
#include <robot_std_msgs/Header.h>
#include <robot_nav_2d_msgs/Twist2DStamped.h>
#include <tf3/buffer_core.h>
#include <string>
#include <vector>
#include <cstring>
#include <cstdlib>
#include <stdexcept>
#include <robot/robot.h>


// ============================================================================
// Internal Helper Functions
// ============================================================================

namespace {
    // Convert C PoseStamped to C++ PoseStamped
    robot_geometry_msgs::PoseStamped c_to_cpp_pose_stamped(const PoseStamped* c_pose) {
        robot_geometry_msgs::PoseStamped cpp_pose;
        cpp_pose.header.seq = c_pose->header.seq;
        cpp_pose.header.stamp.sec = c_pose->header.sec;
        cpp_pose.header.stamp.nsec = c_pose->header.nsec;
        if (c_pose->header.frame_id) {
            cpp_pose.header.frame_id = c_pose->header.frame_id;
        }
        cpp_pose.pose.position.x = c_pose->pose.position.x;
        cpp_pose.pose.position.y = c_pose->pose.position.y;
        cpp_pose.pose.position.z = c_pose->pose.position.z;
        cpp_pose.pose.orientation.x = c_pose->pose.orientation.x;
        cpp_pose.pose.orientation.y = c_pose->pose.orientation.y;
        cpp_pose.pose.orientation.z = c_pose->pose.orientation.z;
        cpp_pose.pose.orientation.w = c_pose->pose.orientation.w;
        return cpp_pose;
    }

    // Convert C++ PoseStamped to C PoseStamped
    void cpp_to_c_pose_stamped(const robot_geometry_msgs::PoseStamped& cpp_pose, PoseStamped* c_pose) {
        c_pose->header.seq = cpp_pose.header.seq;
        c_pose->header.sec = cpp_pose.header.stamp.sec;
        c_pose->header.nsec = cpp_pose.header.stamp.nsec;
        if (!cpp_pose.header.frame_id.empty()) {
            c_pose->header.frame_id = strdup(cpp_pose.header.frame_id.c_str());
        } else {
            c_pose->header.frame_id = nullptr;
        }
        c_pose->pose.position.x = cpp_pose.pose.position.x;
        c_pose->pose.position.y = cpp_pose.pose.position.y;
        c_pose->pose.position.z = cpp_pose.pose.position.z;
        c_pose->pose.orientation.x = cpp_pose.pose.orientation.x;
        c_pose->pose.orientation.y = cpp_pose.pose.orientation.y;
        c_pose->pose.orientation.z = cpp_pose.pose.orientation.z;
        c_pose->pose.orientation.w = cpp_pose.pose.orientation.w;
    }

    // Convert C++ Pose2D to C Pose2D
    void cpp_to_c_pose_2d(const robot_geometry_msgs::Pose2D& cpp_pose, Pose2D* c_pose) {
        c_pose->x = cpp_pose.x;
        c_pose->y = cpp_pose.y;
        c_pose->theta = cpp_pose.theta;
    }

    void cpp_to_c_header(const robot_std_msgs::Header& cpp_header, Header* c_header) {
        c_header->seq = cpp_header.seq;
        c_header->sec = cpp_header.stamp.sec;
        c_header->nsec = cpp_header.stamp.nsec;
        if (!cpp_header.frame_id.empty()) {
            c_header->frame_id = strdup(cpp_header.frame_id.c_str());
        } else {
            c_header->frame_id = nullptr;
        }
    }

    void cpp_to_c_twist2d_stamped(const robot_nav_2d_msgs::Twist2DStamped& cpp_twist,
                                  Twist2DStamped* c_twist) {
        cpp_to_c_header(cpp_twist.header, &c_twist->header);
        c_twist->velocity.x = cpp_twist.velocity.x;
        c_twist->velocity.y = cpp_twist.velocity.y;
        c_twist->velocity.theta = cpp_twist.velocity.theta;
    }

    // Convert C++ NavFeedback to C NavFeedback
    void cpp_to_c_nav_feedback(const robot::move_base_core::NavFeedback& cpp_feedback, NavFeedback* c_feedback) {
        c_feedback->navigation_state = static_cast<NavigationState>(static_cast<int>(cpp_feedback.navigation_state));
        if (!cpp_feedback.feed_back_str.empty()) {
            c_feedback->feed_back_str = strdup(cpp_feedback.feed_back_str.c_str());
        } else {
            c_feedback->feed_back_str = nullptr;
        }
        c_feedback->current_pose.x = cpp_feedback.current_pose.x;
        c_feedback->current_pose.y = cpp_feedback.current_pose.y;
        c_feedback->current_pose.theta = cpp_feedback.current_pose.theta;
        c_feedback->goal_checked = cpp_feedback.goal_checked;
        c_feedback->is_ready = cpp_feedback.is_ready;
    }
}

// ============================================================================
// String Management
// ============================================================================

extern "C" void nav_c_api_free_string(char* str) {
    if (str) {
        free(str);
    }
}

// ============================================================================
// State Conversion
// ============================================================================

extern "C" char* navigation_state_to_string(NavigationState state) {
    robot::move_base_core::State cpp_state = static_cast<robot::move_base_core::State>(static_cast<int>(state));
    std::string str = robot::move_base_core::toString(cpp_state);
    return strdup(str.c_str());
}

// ============================================================================
// Helper Functions
// ============================================================================

extern "C" bool navigation_offset_goal_2d(double pose_x, double pose_y, double pose_theta,
                                          const char* frame_id, double offset_distance,
                                          PoseStamped* out_goal) {
    if (!out_goal || !frame_id) {
        return false;
    }

    robot_geometry_msgs::Pose2D pose;
    pose.x = pose_x;
    pose.y = pose_y;
    pose.theta = pose_theta;

    robot_geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(pose, std::string(frame_id), offset_distance);
    cpp_to_c_pose_stamped(result, out_goal);
    return true;
}

extern "C" bool navigation_offset_goal_stamped(const PoseStamped* in_pose, double offset_distance,
                                                PoseStamped* out_goal) {
    if (!in_pose || !out_goal) {
        return false;
    }

    robot_geometry_msgs::PoseStamped cpp_pose = c_to_cpp_pose_stamped(in_pose);
    robot_geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(cpp_pose, offset_distance);
    cpp_to_c_pose_stamped(result, out_goal);
    return true;
}

// ============================================================================
// Navigation Handle Management
// ============================================================================

extern "C" NavigationHandle navigation_create(void) {
    try {
        // Create a concrete MoveBase instance
        // Using default constructor - initialization will be done via navigation_initialize()
        move_base::MoveBase* move_base = new move_base::MoveBase();
        return static_cast<NavigationHandle>(move_base);
    } catch (const std::exception& e) {
        // Log error if possible (in production, use proper logging)
        return nullptr;
    } catch (...) {
        return nullptr;
    }
}

extern "C" void navigation_destroy(NavigationHandle handle) {
    if (handle) {
        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
        delete nav;
    }
}

// ============================================================================
// TF Listener Management
// ============================================================================

extern "C" TFListenerHandle tf_listener_create(void) {
    try {
        auto tf = std::make_shared<tf3::BufferCore>();
        return new std::shared_ptr<tf3::BufferCore>(tf);
    } catch (...) {
        return nullptr;
    }
}

extern "C" void tf_listener_destroy(TFListenerHandle handle) {
    if (handle) {
        auto* tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore>*>(handle);
        delete tf_ptr;
    }
}

extern "C" bool tf_listener_set_static_transform(TFListenerHandle tf_handle,
                                                 const char* parent_frame,
                                                 const char* child_frame,
                                                 double x, double y, double z,
                                                 double qx, double qy, double qz, double qw)
{
    if (!tf_handle || !parent_frame || !child_frame) {
        return false;
    }

    try {
        auto* tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore>*>(tf_handle);
        if (!tf_ptr || !(*tf_ptr)) {
            return false;
        }

        tf3::TransformStampedMsg st;
        st.header.stamp = tf3::Time::now();
        st.header.frame_id = parent_frame;
        st.child_frame_id = child_frame;
        st.transform.translation.x = x;
        st.transform.translation.y = y;
        st.transform.translation.z = z;
        st.transform.rotation.x = qx;
        st.transform.rotation.y = qy;
        st.transform.rotation.z = qz;
        st.transform.rotation.w = qw;

        return (*tf_ptr)->setTransform(st, "nav_c_api(static)", true /*is_static*/);
    } catch (...) {
        return false;
    }
}

// ============================================================================
// Navigation Interface Methods
// ============================================================================

extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle) 
{
    printf("[%s:%d]\n Begin: navigation_initialize\n", __FILE__, __LINE__);
    if (!handle || !tf_handle) {
        printf("[%s:%d]\n Error: Invalid parameters\n", __FILE__, __LINE__);
        return false;
    }
    try {
        printf("[%s:%d]\n Initialize navigation\n", __FILE__, __LINE__);
        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
        auto* tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore>*>(tf_handle);
        if (!tf_ptr || !(*tf_ptr)) {
            printf("[%s:%d]\n Error: Invalid TF listener\n", __FILE__, __LINE__);
            return false;
        }
        nav->initialize(*tf_ptr);
    
        return true;    
    } catch (const std::exception &e) {
        printf("[%s:%d]\n Error: Failed to initialize navigation: %s\n", __FILE__, __LINE__, e.what());
        return false;
    } catch (...) {
        printf("[%s:%d]\n Error: Failed to initialize navigation\n", __FILE__, __LINE__);
        return false;
    }
}

extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Point* points, size_t point_count) {
    if (!handle || !points || point_count == 0) {
        return false;
    }

    try {
        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
        std::vector<robot_geometry_msgs::Point> footprint;
        footprint.reserve(point_count);
        for (size_t i = 0; i < point_count; ++i) {
            robot_geometry_msgs::Point pt;
            pt.x = points[i].x;
            pt.y = points[i].y;
            pt.z = points[i].z;
            footprint.push_back(pt);
        }
        nav->setRobotFootprint(footprint);
        return true;
    } catch (...) {
        return false;
    }
}

extern "C" bool navigation_get_robot_footprint(NavigationHandle handle, Point** out_points, size_t* out_count) {
    if (!handle || !out_points || !out_count) {
        return false;
    }

    try {
        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
        std::vector<robot_geometry_msgs::Point> footprint = nav->getRobotFootprint();
        if (footprint.empty()) {
            *out_points = nullptr;
            *out_count = 0;
            return true;
        }

        Point* points = static_cast<Point*>(malloc(sizeof(Point) * footprint.size()));
        if (!points) {
            return false;
        }

        for (size_t i = 0; i < footprint.size(); ++i) {
            points[i].x = footprint[i].x;
            points[i].y = footprint[i].y;
            points[i].z = footprint[i].z;
        }

        *out_points = points;
        *out_count = footprint.size();
        return true;
    } catch (...) {
        return false;
    }
}

extern "C" void navigation_free_points(Point* points) {
    if (points) {
        free(points);
    }
}

extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped* goal,
                                    double xy_goal_tolerance, double yaw_goal_tolerance) {
    if (!handle || !goal) {
        return false;
    }

    try {
        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
        robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
        return nav->moveTo(cpp_goal, xy_goal_tolerance, yaw_goal_tolerance);
    } catch (...) {
        return false;
    }
}

extern "C" bool navigation_dock_to(NavigationHandle handle, const char* marker,
                                     const PoseStamped* goal,
                                     double xy_goal_tolerance, double yaw_goal_tolerance) {
    if (!handle || !marker || !goal) {
        return false;
    }

    try {
        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
        robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
        return nav->dockTo(std::string(marker), cpp_goal, xy_goal_tolerance, yaw_goal_tolerance);
    } catch (...) {
        return false;
    }
}

extern "C" bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped* goal,
                                             double xy_goal_tolerance) {
    if (!handle || !goal) {
        return false;
    }

    try {
        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
        robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
        return nav->moveStraightTo(cpp_goal, xy_goal_tolerance);
    } catch (...) {
        return false;
    }
}

extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped* goal,
                                      double yaw_goal_tolerance) {
    if (!handle || !goal) {
        return false;
    }

    try {
        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
        robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
        return nav->rotateTo(cpp_goal, yaw_goal_tolerance);
    } catch (...) {
        return false;
    }
}

extern "C" void navigation_pause(NavigationHandle handle) {
    if (handle) {
        try {
            robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
            nav->pause();
        } catch (...) {
            // Ignore exceptions
        }
    }
}

extern "C" void navigation_resume(NavigationHandle handle) {
    if (handle) {
        try {
            robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
            nav->resume();
        } catch (...) {
            // Ignore exceptions
        }
    }
}

extern "C" void navigation_cancel(NavigationHandle handle) {
    if (handle) {
        try {
            robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
            nav->cancel();
        } catch (...) {
            // Ignore exceptions
        }
    }
}

extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
                                             double linear_x, double linear_y, double linear_z) {
    if (!handle) {
        return false;
    }

    try {
        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
        robot_geometry_msgs::Vector3 linear;
        linear.x = linear_x;
        linear.y = linear_y;
        linear.z = linear_z;
        return nav->setTwistLinear(linear);
    } catch (...) {
        return false;
    }
}

extern "C" bool navigation_set_twist_angular(NavigationHandle handle,
                                              double angular_x, double angular_y, double angular_z) {
    if (!handle) {
        return false;
    }

    try {
        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
        robot_geometry_msgs::Vector3 angular;
        angular.x = angular_x;
        angular.y = angular_y;
        angular.z = angular_z;
        return nav->setTwistAngular(angular);
    } catch (...) {
        return false;
    }
}

extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped* out_pose) {
    if (!handle || !out_pose) {
        return false;
    }

    try {
        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
        robot_geometry_msgs::PoseStamped cpp_pose;
        if (nav->getRobotPose(cpp_pose)) {
            cpp_to_c_pose_stamped(cpp_pose, out_pose);
            return true;
        }
        return false;
    } catch (...) {
        return false;
    }
}

extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* out_pose) {
    if (!handle || !out_pose) {
        return false;
    }

    try {
        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
        robot_geometry_msgs::Pose2D cpp_pose;
        if (nav->getRobotPose(cpp_pose)) {
            cpp_to_c_pose_2d(cpp_pose, out_pose);
            return true;
        }
        return false;
    } catch (...) {
        return false;
    }
}

extern "C" bool navigation_get_twist(NavigationHandle handle, Twist2DStamped* out_twist) {
    if (!handle || !out_twist) {
        return false;
    }

    try {
        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
        robot_nav_2d_msgs::Twist2DStamped cpp_twist = nav->getTwist();
        cpp_to_c_twist2d_stamped(cpp_twist, out_twist);
        return true;
    } catch (...) {
        return false;
    }
}

extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback* out_feedback) {
    if (!handle || !out_feedback) {
        return false;
    }

    try {
        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
        if (nav->getFeedback() != nullptr) {
            cpp_to_c_nav_feedback(*nav->getFeedback(), out_feedback);
            return true;
        } else {
            return false;
        }
    } catch (...) {
        return false;
    }
}

extern "C" void navigation_free_feedback(NavFeedback* feedback) {
    if (feedback) {
        nav_c_api_free_string(feedback->feed_back_str);
        feedback->feed_back_str = nullptr;
    }
}

