#include "sensor_msgs/BatteryState.h"
#include "sensor_msgs/JoyFeedbackArray.h"
#include "sensor_msgs/PointCloud2.h"
#include <iostream>
#include <yaml-cpp/yaml.h>

bool getParams()
{
    try {
        YAML::Node config = YAML::LoadFile("../cfg/config.yaml");

        double cost_scaling_factor = config["inflation_layer"]["cost_scaling_factor"].as<double>();
        double inflation_radius = config["inflation_layer"]["inflation_radius"].as<double>();

        bool enabled = config["inflation_layer"]["enabled"].as<bool>();
        bool inflate_unknown = config["inflation_layer"]["inflate_unknown"].as<bool>();

        double distance = config["obstacle_layer"]["distance"].as<double>();

        std::cout << "Inflation Layer Parameters:" << std::endl;
        std::cout << "  Cost Scaling Factor: " << cost_scaling_factor << std::endl;
        std::cout << "  Inflation Radius: " << inflation_radius << std::endl;
        std::cout << "  Enabled: " << (enabled ? "true" : "false") << std::endl;
        std::cout << "  Inflate Unknown: " << (inflate_unknown ? "true" : "false") << std::endl;
        std::cout << "Obstacle Layer Parameters:" << std::endl;
        std::cout << "  Distance: " << distance << std::endl;
        
    } 
    catch (const YAML::BadFile& e) {
        std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
        return false;
    }

    return true;

}

int main()
{
  getParams();
  sensor_msgs::BatteryState battery;

  battery.header = std_msgs::Header::now("battery_frame");
  battery.voltage = 12.5f;
  battery.current = -1.2f;
  battery.percentage = 0.85f;
  battery.power_supply_status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING;

  battery.cell_voltage = {4.15f, 4.18f, 4.12f};
  battery.location = "slot_1";
  battery.serial_number = "SN12345";

  std::cout << "Battery header timestamp: " << battery.header.stamp << "\n";
  std::cout << "Battery voltage: " << battery.voltage << " V\n";
  std::cout << "Status: " << static_cast<int>(battery.power_supply_status) << "\n";
  std::cout << "Serial Number: " << battery.serial_number << "\n";

  sensor_msgs::JoyFeedbackArray feedback_array;
  feedback_array.array.push_back(sensor_msgs::JoyFeedback{sensor_msgs::JoyFeedback::TYPE_BUZZER, 0, 5000.0f});

  std::cout << "First feedback type: " << static_cast<int>(feedback_array.array[0].type) << "\n";
  std::cout << "First feedback intensity: " << feedback_array.array[0].intensity << "\n";

  sensor_msgs::PointCloud2 cloud;
  cloud.header = std_msgs::Header::now("pointcloud_frame");
  cloud.width = 100;
  cloud.height = 1;

  std::cout << "PointCloud2 header frame_id: " << cloud.header.frame_id << "\n";
  std::cout << "PointCloud2 header timestamp: " << cloud.header.stamp << "\n";
  std::cout << "PointCloud2 width: " << cloud.width << "\n";
  std::cout << "PointCloud2 height: " << cloud.height << "\n";

  return 0;
}