#include "cyphal_hardware_interface/cyphal_system.hpp"

#include <chrono>
#include <cmath>
#include <cstddef>
#include <hardware_interface/hardware_info.hpp>
#include <limits>
#include <memory>
#include <set>
#include <sstream>
#include <string>
#include <vector>

//#include "serial.hpp"

#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"

//#include "cyphal.hpp"

namespace cyphal_hardware_interface {

cyphalSystemHardware::~cyphalSystemHardware() {
  if (serial_port_handle) {
    RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                "Closing port '%s'", serial_port_.c_str());
    close(serial_port_handle);
  }
}

hardware_interface::CallbackReturn cyphalSystemHardware::on_init(
    const hardware_interface::HardwareInfo &info) {
  if (hardware_interface::SystemInterface::on_init(info) !=
      hardware_interface::CallbackReturn::SUCCESS) {
    return hardware_interface::CallbackReturn::ERROR;
  }

  serial_port_ = default_serial_port_;

  if (info_.hardware_parameters.count("port") > 0) {
    serial_port_ = info_.hardware_parameters.at("port");
  } else {
    RCLCPP_WARN(rclcpp::get_logger("cyphalSystemHardware"),
                "No port specified in urdf, using default value");
  }
  if (info_.hardware_parameters.count("try_next_port_number") > 0) {
    std::string try_next_str =
        info_.hardware_parameters.at("try_next_port_number");
    try_next_port_number =
        std::set<std::string>({"True", "true", "1", "Yes", "yes", "Y", "y"})
            .count(try_next_str) > 0;
    RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                "Trying next ports if the one provided is not openable");
  }
  RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Port set as %s",
              serial_port_.c_str());

  std::vector<hardware_interface::ComponentInfo> components;
  components.insert(components.begin(), info_.joints.begin(),
                    info_.joints.end());
  components.insert(components.begin(), info_.sensors.begin(),
                    info_.sensors.end());
  components.insert(components.begin(), info_.gpios.begin(), info_.gpios.end());
  for (const hardware_interface::ComponentInfo &joint : components) {
    // StaubiSystem has exactly two states and one command interface on each
    // joint
    /*if (joint.command_interfaces.size() != 1) {
      RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
                   "Joint '%s' has %zu command interfaces found. 1 expected.",
                   joint.name.c_str(), joint.command_interfaces.size());
      return hardware_interface::CallbackReturn::ERROR;
    }*/
    for (const auto &i : joint.command_interfaces) {
      std::string param_name = "command:" + i.name;
      if (joint.parameters.count(param_name) <= 0) {
        RCLCPP_FATAL(
            rclcpp::get_logger("cyphalSystemHardware"),
            "command interface '%s' of joint/gpio '%s' has no parameter "
            "'command:%s'.",
            i.name.c_str(), joint.name.c_str(), i.name.c_str());
        return hardware_interface::CallbackReturn::ERROR;
      }
      auto interface_name = joint.parameters.at(param_name);
      if (interfaces_map.count(interface_name) > 0) {
        RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
                     "Sync Interface '%s' used twice. FATAL ERROR.",
                     interface_name.c_str());
        return hardware_interface::CallbackReturn::ERROR;
      }
      interfaces_map[interface_name] = {
          std::numeric_limits<double>::quiet_NaN(),
          (joint.parameters.count("factor") > 0)
              ? std::stod(joint.parameters.at("factor"))
              : 1.0,
          joint.name, true, i.name};
    }

    for (const auto &i : joint.state_interfaces) {
      std::string param_name = "state:" + i.name;
      if (joint.parameters.count(param_name) <= 0) {
        RCLCPP_FATAL(
            rclcpp::get_logger("cyphalSystemHardware"),
            "state interface '%s' of joint/sensor/gpio '%s' has no parameter "
            "'state:%s'.",
            i.name.c_str(), joint.name.c_str(), i.name.c_str());
        return hardware_interface::CallbackReturn::ERROR;
      }
      auto interface_name = joint.parameters.at(param_name);
      if (interfaces_map.count(interface_name) > 0) {
        RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
                     "Sync Interface '%s' used twice. FATAL ERROR.",
                     interface_name.c_str());
        return hardware_interface::CallbackReturn::ERROR;
      }
      interfaces_map[interface_name] = {
          std::numeric_limits<double>::quiet_NaN(),
          (joint.parameters.count("factor") > 0)
              ? std::stod(joint.parameters.at("factor"))
              : 1.0,
          joint.name, false, i.name};
    }
  }
  // for (const auto &[key, value] : interfaces_map) {
  // RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
  //             "Interface %s added (factor: %f).", key.c_str(), value.factor);
  // }

  return hardware_interface::CallbackReturn::SUCCESS;
} // namespace cyphal_hardware_interface

std::vector<hardware_interface::StateInterface>
cyphalSystemHardware::export_state_interfaces() {
  std::vector<hardware_interface::StateInterface> state_interfaces;
  for (auto &[key, value] : interfaces_map) {
    if (!value.command)
      state_interfaces.emplace_back(hardware_interface::StateInterface(
          value.joint, value.control_interface, &value.value));
  }
  /*  for (auto i = 0u; i < info_.joints.size(); i++) {
      state_interfaces.emplace_back(hardware_interface::StateInterface(
          info_.joints[i].name, hardware_interface::HW_IF_POSITION,
          &hw_positions_[i]));
      state_interfaces.emplace_back(hardware_interface::StateInterface(
          info_.joints[i].name, hardware_interface::HW_IF_VELOCITY,
          &hw_velocities_[i]));
    }
  */
  return state_interfaces;
}

std::vector<hardware_interface::CommandInterface>
cyphalSystemHardware::export_command_interfaces() {
  std::vector<hardware_interface::CommandInterface> command_interfaces;
  for (auto &[key, value] : interfaces_map) {
    if (value.command)
      command_interfaces.emplace_back(hardware_interface::CommandInterface(
          value.joint, value.control_interface, &value.value));
  }
  /* for (auto i = 0u; i < info_.joints.size(); i++) {
     command_interfaces.emplace_back(hardware_interface::CommandInterface(
         info_.joints[i].name, hardware_interface::HW_IF_VELOCITY,
         &hw_commands_[i]));
   }
 */
  return command_interfaces;
}

hardware_interface::CallbackReturn cyphalSystemHardware::on_configure(
    const rclcpp_lifecycle::State & /*previous_state*/) {
  // const hardware_interface::CallbackReturn result =
  //     can_interface_->connect(can_socket_)
  //         ? hardware_interface::CallbackReturn::SUCCESS
  //         : hardware_interface::CallbackReturn::FAILURE;
  // Open the serial port. Change device path as needed (currently set to an
  // standard FTDI USB-UART cable type device)

  // serial_port_handle = open_serial(serial_port_);
  // if (!serial_port_handle) {
  //   RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
  //                "Error opening port '%s': %s", serial_port_.c_str(),
  //                strerror(errno));
  //   if (try_next_port_number) {
  //     for (unsigned int tries = 0; tries < 100; tries += 1) {
  //       size_t digits_idx = serial_port_.size() - 1;
  //       while (digits_idx > 0 && std::isdigit(serial_port_[digits_idx]))
  //         digits_idx -= 1;
  //       digits_idx += 1;
  //       std::string number_str = serial_port_.substr(digits_idx);
  //       unsigned int number = number_str.size() > 0 ? std::stol(number_str) : 0;
  //       serial_port_ =
  //           serial_port_.substr(0, digits_idx) + std::to_string(number + 1);
  //       serial_port_handle = open_serial(serial_port_);
  //       if (!serial_port_handle) {
  //         RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
  //                      "Error opening port '%s': %s", serial_port_.c_str(),
  //                      strerror(errno));
  //       } else
  //         break;
  //     }
  //   }
  //   if (!serial_port_handle)
  //     return hardware_interface::CallbackReturn::FAILURE;
  // }
  // RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
  //             "Successfuly opened port '%s'", serial_port_.c_str());


  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn cyphalSystemHardware::on_activate(
    const rclcpp_lifecycle::State & /*previous_state*/) {
  // BEGIN: This part here is for exemplary purposes - Please do not copy to
  // your production code
  RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
              "Activating ...please wait...");

  /*  for (auto i = 0; i < hw_start_sec_; i++) {
      rclcpp::sleep_for(std::chrono::seconds(1));
      RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                  "%.1f seconds left...", hw_start_sec_ - i);
    }
    // END: This part here is for exemplary purposes - Please do not copy to
    your
    // production code

    // set some default values
    for (auto i = 0u; i < hw_positions_.size(); i++) {
      if (std::isnan(hw_positions_[i])) {
        hw_positions_[i] = 0;
        hw_velocities_[i] = 0;
        hw_commands_[i] = 0;
      }
    }
  */
  RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
              "Successfully activated!");

  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn cyphalSystemHardware::on_deactivate(
    const rclcpp_lifecycle::State & /*previous_state*/) {
  // BEGIN: This part here is for exemplary purposes - Please do not copy to
  // your production code
  RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
              "Deactivating ...please wait...");

  /*  for (auto i = 0; i < hw_stop_sec_; i++) {
      rclcpp::sleep_for(std::chrono::seconds(1));
      RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                  "%.1f seconds left...", hw_stop_sec_ - i);
    }
    // END: This part here is for exemplary purposes - Please do not copy to
    your
    // production code

    RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                "Successfully deactivated!");
  */
  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::return_type
cyphalSystemHardware::read(const rclcpp::Time & /*time*/,
                               const rclcpp::Duration & /*period*/) {

  // if (!sync)
  //   return hardware_interface::return_type::ERROR;

  // uint8_t read_buf[1024];
  // int read_buf_used =
  //     read_serial(serial_port_handle, read_buf, sizeof(read_buf));
  // sync->handle_stream((uint8_t *)read_buf, read_buf_used);
  // for (auto &[key, value] : interfaces_map) {
  //   // RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
  //   //             "Interface %s added (factor: %f).", key.c_str(),
  //   //             value.factor);
  //   if (!value.command) {
  //     auto n = sync->get_number(key.c_str());
  //     if (n)
  //       value.value = value.factor * (*n);
  //   }
  // }

  // using namespace std::chrono_literals;
  // if (std::chrono::steady_clock::now() - sync->last_pkg_recived >
  //     std::chrono::milliseconds(1000)) {
  //   RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
  //                "last package recivew more than 1000ms ago. aborting!");
  //   return hardware_interface::return_type::ERROR;
  // }

  // static int counter = 0;
  // if (counter++ % 20 == 0)
  //   for (auto &i : sync->interfaces) {
  //     if (!i)
  //       continue;
  //     auto number_ptr = sync->get_number(i->key.c_str());
  //     auto str_ptr = sync->get_string(i->key.c_str());
  //     if (number_ptr)
  //       RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
  //                   "Sync: %s = %d", i->key.c_str(), *number_ptr);
  //     if (str_ptr)
  //       RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
  //                   "Sync: %s = %s", i->key.c_str(), str_ptr->c_str());
  //   }

  return hardware_interface::return_type::OK;
}

hardware_interface::return_type
cyphal_hardware_interface ::cyphalSystemHardware::write(
    const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
  // // BEGIN: This part here is for exemplary purposes - Please do not copy to
  // // your production code

  // if (sync) {
  //   for (auto &[key, value] : interfaces_map) {
  //     // RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
  //     //             "Interface %s added (factor: %f).", key.c_str(),
  //     //             value.factor);
  //     if (value.command) {
  //       auto ni = sync->get_or_create_interface(0x01, key.c_str());
  //       if (ni) {

  //         *(int *)ni->data = (value.value / value.factor);
  //         ni->send_requested = true;

  //         // RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
  //                     // "update: %s set to %d ", key.c_str(), *(int
  //                     // *)ni->data);
  //       }
  //     }
  //   }
  //   sync->update();
  // }

  /*  for (auto i = 0u; i < hw_commands_.size(); i++) {
      // Simulate sending commands to the hardware
      RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                  "Got command %.5f for '%s'!", hw_commands_[i],
                  info_.joints[i].name.c_str());

      hw_velocities_[i] = hw_commands_[i];
    }
    RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
                "Joints successfully written!");
    // END: This part here is for exemplary purposes - Please do not copy to
    your
    // production code
  */
  return hardware_interface::return_type::OK;
}

} // namespace cyphal_hardware_interface

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(cyphal_hardware_interface::cyphalSystemHardware,
                       hardware_interface::SystemInterface)