#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include <chrono>
#include <functional>
#include <string>

using namespace std::chrono_literals;

class RobotController : public rclcpp::Node {
public:
  RobotController() : Node("robot_controller") {
    command_publisher_ = this->create_publisher<std_msgs::msg::String>("robot_command", 10);
    status_subscription_ = this->create_subscription<std_msgs::msg::String>(
      "robot_status", 10, 
      std::bind(&RobotController::status_callback, this, std::placeholders::_1));
      
    RCLCPP_INFO(this->get_logger(), "Robot controller initialized");
    
    // Start demo sequence after 1 second
    demo_timer_ = this->create_wall_timer(
      1000ms, std::bind(&RobotController::run_demo, this));
    
    demo_step_ = 0;
  }

private:
  void send_command(const std::string& command) {
    auto message = std_msgs::msg::String();
    message.data = command;
    command_publisher_->publish(message);
    RCLCPP_INFO(this->get_logger(), "Sent command: %s", command.c_str());
  }
  
  void status_callback(const std_msgs::msg::String::SharedPtr msg) {
    RCLCPP_INFO(this->get_logger(), "Robot status: %s", msg->data.c_str());
    
    // Example of handling battery status messages
    std::string status = msg->data;
    if (status.find("Battery status") != std::string::npos) {
      if (status.find("CRITICAL") != std::string::npos) {
        RCLCPP_WARN(this->get_logger(), "Battery critically low, robot will sit to prevent damage");
      } else if (status.find("LOW") != std::string::npos) {
        RCLCPP_WARN(this->get_logger(), "Battery low, consider recharging soon");
      }
    }
  }
  
  void run_demo() {
    switch (demo_step_) {
      case 0:
        send_command("stand");
        break;
      case 1:
        send_command("position 20 0 0");
        break;
      case 2:
        send_command("orientation 0 10 0");
        break;
      case 3:
        send_command("walk 1.0");
        break;
      case 8: // After 5 seconds of walking
        send_command("stop");
        break;
      case 9:
        send_command("sit");
        break;
      case 10:
        // End demo sequence
        demo_timer_->cancel();
        RCLCPP_INFO(this->get_logger(), "Demo complete");
        break;
    }
    
    demo_step_++;
  }
  
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr command_publisher_;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr status_subscription_;
  rclcpp::TimerBase::SharedPtr demo_timer_;
  int demo_step_;
};

int main(int argc, char * argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<RobotController>());
  rclcpp::shutdown();
  return 0;
}