#include "rclcpp/rclcpp.hpp"
#include <orbbec_camera/ob_camera_node_driver.h>
#include <orbbec_camera/utils.h>
#include <iostream>
#include <sstream>
#include <string>
#include <chrono>
#include <thread>

using namespace ob;

bool parseIpString(const std::string &ip_str, uint8_t ip[4]) {
  std::stringstream ss(ip_str);
  std::string item;
  int i = 0;
  while (std::getline(ss, item, '.')) {
    if (i >= 4) return false;
    try {
      int num = std::stoi(item);
      if (num < 0 || num > 255) return false;
      ip[i++] = static_cast<uint8_t>(num);
    } catch (...) {
      return false;
    }
  }
  return i == 4;
}

bool isParamProvided(int argc, char **argv, const std::string &key) {
  const std::string pattern = key + ":=";
  for (int i = 1; i < argc; ++i) {
    const std::string arg(argv[i]);
    if (arg.find(pattern) != std::string::npos) {
      return true;
    }
  }
  return false;
}

void printHelp() {
  std::cout
      << "Usage:\n"
      << "  ros2 run orbbec_camera ip_config_tool --ros-args [params]\n"
      << "  (legacy alias: set_device_ip)\n\n"
      << "Parameters:\n"
      << "  -p old_ip:=<ip>            Current device IP (default: 192.168.1.10)\n"
      << "  -p port:=<port>            Device port (default: 8090)\n"
      << "  -p enable_lla:=<bool>      Set LLA switch directly (true: enable, false: disable, "
         "default: false)\n"
      << "                              Note: LLA is applied only when this parameter is "
         "explicitly provided.\n"
      << "  -p enable_set_ip:=<bool>   Enable set-ip operation (default: false)\n"
      << "  -p dhcp:=<bool>            DHCP flag for set-ip/force-ip config (default: false)\n"
      << "  -p new_ip:=<ip>            Static IP for set-ip/force-ip (default: 192.168.1.200)\n"
      << "  -p mask:=<ip>              Subnet mask for set-ip/force-ip (default: 255.255.255.0)\n"
      << "  -p gateway:=<ip>           Gateway for set-ip/force-ip (default: 192.168.1.1)\n"
      << "  -p enable_force_ip:=<bool> Enable force-ip operation (default: false)\n"
      << "  -p force_ip_mac:=<mac>     Target MAC for force-ip (required, e.g. "
         "54:14:FD:06:07:DA)\n\n"
      << "Examples:\n"
      << "\n"
      << "  [LLA]\n"
      << "    enable:  ros2 run orbbec_camera ip_config_tool --ros-args -p old_ip:=192.168.1.10 -p "
         "enable_lla:=true\n"
      << "    disable: ros2 run orbbec_camera ip_config_tool --ros-args -p old_ip:=192.168.1.10 -p "
         "enable_lla:=false\n"
      << "\n"
      << "  [Set IP]\n"
      << "    DHCP:    ros2 run orbbec_camera ip_config_tool --ros-args \\\n"
      << "             -p old_ip:=192.168.1.10 -p enable_set_ip:=true -p dhcp:=true\n"
      << "    Static:  ros2 run orbbec_camera ip_config_tool --ros-args \\\n"
      << "             -p old_ip:=192.168.1.10 -p enable_set_ip:=true -p dhcp:=false \\\n"
      << "             -p new_ip:=192.168.1.200 -p mask:=255.255.255.0 -p gateway:=192.168.1.1\n"
      << "\n"
      << "  [Force IP]\n"
      << "    by MAC:  ros2 run orbbec_camera ip_config_tool --ros-args \\\n"
      << "             -p enable_force_ip:=true \\\n"
      << "             -p force_ip_mac:=54:14:FD:06:07:DA -p dhcp:=false \\\n"
      << "             -p new_ip:=192.168.1.200 -p mask:=255.255.255.0 -p gateway:=192.168.1.1\n";
}

int main(int argc, char **argv) {
  for (int i = 1; i < argc; ++i) {
    const std::string arg(argv[i]);
    if (arg == "-h" || arg == "--help") {
      printHelp();
      return 0;
    }
  }

  rclcpp::init(argc, argv);
  auto node = std::make_shared<rclcpp::Node>("ip_config_tool");
  auto logger = node->get_logger();

  std::string device_ip_str = node->declare_parameter<std::string>("old_ip", "192.168.1.10");
  int port = node->declare_parameter<int>("port", 8090);

  bool enable_lla = node->declare_parameter<bool>("enable_lla", false);
  bool do_lla = isParamProvided(argc, argv, "enable_lla");

  bool enable_set_ip = node->declare_parameter<bool>("enable_set_ip", false);
  bool dhcp = node->declare_parameter<bool>("dhcp", false);
  std::string new_ip_str = node->declare_parameter<std::string>("new_ip", "192.168.1.200");
  std::string mask_str = node->declare_parameter<std::string>("mask", "255.255.255.0");
  std::string gateway_str = node->declare_parameter<std::string>("gateway", "192.168.1.1");

  bool enable_force_ip = node->declare_parameter<bool>("enable_force_ip", false);
  std::string force_ip_mac = node->declare_parameter<std::string>("force_ip_mac", "");

  if (!do_lla && !enable_set_ip && !enable_force_ip) {
    RCLCPP_ERROR(logger,
                 "No operation enabled. Please enable at least one of: enable_lla, enable_set_ip, "
                 "enable_force_ip.");
    rclcpp::shutdown();
    return 1;
  }

  OBNetIpConfig ip_config{};
  ip_config.dhcp = dhcp ? 1 : 0;

  if ((enable_set_ip || enable_force_ip) && !dhcp) {
    if (!parseIpString(new_ip_str, ip_config.address)) {
      RCLCPP_ERROR(logger, "Invalid new_ip format: %s", new_ip_str.c_str());
      rclcpp::shutdown();
      return 1;
    }
    if (!parseIpString(mask_str, ip_config.mask)) {
      RCLCPP_ERROR(logger, "Invalid mask format: %s", mask_str.c_str());
      rclcpp::shutdown();
      return 1;
    }
    if (!parseIpString(gateway_str, ip_config.gateway)) {
      RCLCPP_ERROR(logger, "Invalid gateway format: %s", gateway_str.c_str());
      rclcpp::shutdown();
      return 1;
    }
  }

  try {
    ob::Context::setLoggerSeverity(OBLogSeverity::OB_LOG_SEVERITY_OFF);
    auto context = std::make_shared<ob::Context>();

    if (do_lla || enable_set_ip) {
      RCLCPP_INFO(logger, "Connecting to device %s:%d ...", device_ip_str.c_str(), port);
      auto device = context->createNetDevice(device_ip_str.c_str(), port);

      if (do_lla) {
        if (device->isPropertySupported(OB_PROP_DEVICE_NETWORK_LLA_BOOL,
                                        OB_PERMISSION_READ_WRITE)) {
          device->setBoolProperty(OB_PROP_DEVICE_NETWORK_LLA_BOOL, enable_lla);
          RCLCPP_INFO(logger, "LLA set successfully. target=%s",
                      enable_lla ? "enabled" : "disabled");
        } else {
          RCLCPP_WARN(logger, "LLA property is not supported on this device.");
        }
      } else {
        RCLCPP_INFO(logger, "LLA operation skipped (enable_lla not explicitly provided).");
      }

      if (enable_set_ip) {
        RCLCPP_INFO(logger, "Applying set-ip configuration...");
        device->setStructuredData(OB_STRUCT_DEVICE_IP_ADDR_CONFIG,
                                  reinterpret_cast<const uint8_t *>(&ip_config), sizeof(ip_config));

        RCLCPP_INFO(logger, "Set-ip configuration applied successfully.");
        if (dhcp) {
          RCLCPP_INFO(logger, "Set-ip target mode: DHCP.");
        } else {
          RCLCPP_INFO(logger, "Set-ip target static IP: %d.%d.%d.%d", ip_config.address[0],
                      ip_config.address[1], ip_config.address[2], ip_config.address[3]);
          RCLCPP_INFO(logger, "Set-ip target mask: %d.%d.%d.%d", ip_config.mask[0],
                      ip_config.mask[1], ip_config.mask[2], ip_config.mask[3]);
          RCLCPP_INFO(logger, "Set-ip target gateway: %d.%d.%d.%d", ip_config.gateway[0],
                      ip_config.gateway[1], ip_config.gateway[2], ip_config.gateway[3]);
        }
      } else {
        RCLCPP_INFO(logger, "Set-ip operation skipped (enable_set_ip=false).");
      }
    }

    if (enable_force_ip) {
      if (force_ip_mac.empty()) {
        RCLCPP_ERROR(logger, "force_ip_mac is required when enable_force_ip=true.");
        rclcpp::shutdown();
        return 1;
      }

      RCLCPP_INFO(logger, "Applying force-ip to MAC %s ...", force_ip_mac.c_str());
      if (context->forceIp(force_ip_mac.c_str(), ip_config)) {
        RCLCPP_INFO(logger, "Force-ip operation applied successfully.");
        std::this_thread::sleep_for(std::chrono::seconds(5));
        if (dhcp) {
          RCLCPP_INFO(logger, "Force-ip target mode: DHCP.");
        } else {
          RCLCPP_INFO(logger, "Force-ip target static IP: %s", new_ip_str.c_str());
          RCLCPP_INFO(logger, "Force-ip target mask: %s", mask_str.c_str());
          RCLCPP_INFO(logger, "Force-ip target gateway: %s", gateway_str.c_str());
        }
      } else {
        RCLCPP_ERROR(logger, "Force-ip failed (SDK returned false).");
        rclcpp::shutdown();
        return 1;
      }
    }

  } catch (ob::Error &e) {
    RCLCPP_ERROR(logger, "ip_config_tool: %s", orbbec_camera::formatObErrorWithStatus(e).c_str());
    rclcpp::shutdown();
    return 1;
  } catch (const std::exception &e) {
    RCLCPP_ERROR(logger, "ip_config_tool: %s", e.what());
    rclcpp::shutdown();
    return 1;
  } catch (...) {
    RCLCPP_ERROR(logger, "ip_config_tool: unknown error");
    rclcpp::shutdown();
    return 1;
  }

  rclcpp::shutdown();
  return 0;
}