/*
 * Copyright (c) 2013, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <move_base/move_base.h>
#include <iostream>
#include <robot/robot.h>
#include <robot_nav_2d_utils/parameters.h>
#include <boost/dll/import.hpp>
#include <tf3/buffer_core.h>
#include <tf3/transform_datatypes.h>

int main(int argc, char** argv)
{
  try {
    std::shared_ptr<tf3::BufferCore> tf = std::make_shared<tf3::BufferCore>();
    
    // Lambda function để set static transform (tương tự tf_listener_set_static_transform trong nav_c_api.cpp)
    auto set_static_transform = [&tf](const std::string& parent_frame,
                                       const std::string& child_frame,
                                       double x, double y, double z,
                                       double qx, double qy, double qz, double qw) -> bool {
      if (!tf || parent_frame.empty() || child_frame.empty()) {
        return false;
      }
      
      try {
        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->setTransform(st, "move_base_main(static)", true /*is_static*/);
      } catch (...) {
        return false;
      }
    };
    
    if (set_static_transform("map", "odom", 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)) {
      robot::log_info("[%s:%d]\n Static transform set: map -> odom", __FILE__, __LINE__);
    } else {
      robot::log_error("[%s:%d]\n Failed to set static transform: map -> odom", __FILE__, __LINE__);
    }

    if (set_static_transform("odom", "base_footprint", 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)) {
      robot::log_info("[%s:%d]\n Static transform set: odom -> base_footprint", __FILE__, __LINE__);
    } else {
      robot::log_error("[%s:%d]\n Failed to set static transform: odom -> base_footprint", __FILE__, __LINE__);
    }

    if (set_static_transform("base_footprint", "base_link", 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)) {
      robot::log_info("[%s:%d]\n Static transform set: base_footprint -> base_link", __FILE__, __LINE__);
    } else {
      robot::log_error("[%s:%d]\n Failed to set static transform: base_footprint -> base_link", __FILE__, __LINE__);
    }

    robot::log_info("[%s:%d]\n Loading MoveBase library...", __FILE__, __LINE__);
    robot::PluginLoaderHelper loader;
    std::string path_file_so = loader.findLibraryPath("MoveBase");
    auto create_plugin = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>(
      path_file_so,
      "MoveBase", boost::dll::load_mode::append_decorations);
    
    robot::log_info("[%s:%d]\n Creating MoveBase instance...", __FILE__, __LINE__);
    robot::move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin();
    
    if (nav_ptr == nullptr) {
      robot::log_error("[%s:%d]\n Error: Failed to create navigation", __FILE__, __LINE__);
      return 1;
    }
    
    robot::log_info("[%s:%d]\n Navigation created, initializing...", __FILE__, __LINE__);
    nav_ptr->initialize(tf);
    robot::log_info("[%s:%d]\n Navigation initialized successfully", __FILE__, __LINE__);

    std::cout << "Move Base Main Function :" << robot::Time::now().toSec() << std::endl;
    // Explicitly reset before create_plugin goes out of scope to ensure proper cleanup order
    robot::log_info("[%s:%d]\n Cleaning up...", __FILE__, __LINE__);
    nav_ptr.reset();
    robot::log_info("[%s:%d]\n Cleanup complete", __FILE__, __LINE__);
    
    return 0;
  } catch (const std::exception &e) {
    robot::log_error("[%s:%d]\n Exception: %s", __FILE__, __LINE__, e.what());
    return 1;
  } catch (...) {
    robot::log_error("[%s:%d]\n Unknown exception occurred", __FILE__, __LINE__);
    return 1;
  }
  return 0;
}