// Copyright 2021 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef CYPHAL_HARDWARE_INTERFACE__CYPHAL_SYSTEM_HPP_
#define CYPHAL_HARDWARE_INTERFACE__CYPHAL_SYSTEM_HPP_

#include <map>
#include <memory>
#include <mutex>
#include <string>
#include <vector>

#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "rclcpp/clock.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"

#include "cyphal_hardware_interface/visibility_control.h"

extern "C" {
#include "canard.h"
#include <uavcan/_register/Access_1_0.h>
#include <uavcan/_register/List_1_0.h>
#include <uavcan/_register/Value_1_0.h>
#include <uavcan/node/port/List_0_1.h>
#include <reg/udral/physics/kinematics/rotation/Planar_0_1.h>
}

#include <thread>

#define CAN_REDUNDANCY_FACTOR 1

void *canardAllocate(void *const user_reference, const size_t size) {
  (void)user_reference;
  return malloc(size);
}

void canardDeallocate(void *const user_reference, const long unsigned int size,
                      void *const pointer) {
  (void)user_reference;
  (void)size;
  free(pointer);
}

namespace cyphal_hardware_interface {
enum arming_state {
  DISARMED = 0,
  ARMED,
};
class cyphalUdralServo {
public:
  unsigned char node_id = 0xFF;
  std::string name;
  mutable std::mutex mtx;
  std::string cookie = "";
  std::map<std::string, uavcan_register_Value_1_0> registers;
  struct {
    bool alive = false;
    bool register_read = false;
    std::string requested_register_name;
    reg_udral_physics_kinematics_rotation_Planar_0_1 kinematics;
  } state;


  enum arming_state arm_target = DISARMED;

  std::string get_non_read_register_name() const;
  bool all_registers_read() const;
};

class cyphalSystemHardware : public hardware_interface::SystemInterface {
public:
  RCLCPP_SHARED_PTR_DEFINITIONS(cyphalSystemHardware);

  constexpr static std::string_view default_if_ = "can0";

  std::string can_if_;

  CYPHAL_HARDWARE_INTERFACE_PUBLIC
  virtual ~cyphalSystemHardware();

  CYPHAL_HARDWARE_INTERFACE_PUBLIC
  hardware_interface::CallbackReturn
  on_init(const hardware_interface::HardwareComponentInterfaceParams &params)
      override;

  CYPHAL_HARDWARE_INTERFACE_PUBLIC
  std::vector<hardware_interface::StateInterface>
  export_state_interfaces() override {
    return {};
  };

  CYPHAL_HARDWARE_INTERFACE_PUBLIC
  std::vector<hardware_interface::CommandInterface>
  export_command_interfaces() override {
    return {};
  };

  CYPHAL_HARDWARE_INTERFACE_PUBLIC
  CYPHAL_HARDWARE_INTERFACE_PUBLIC
  hardware_interface::CallbackReturn
  on_configure(const rclcpp_lifecycle::State &previous_state) override;

  hardware_interface::CallbackReturn
  on_activate(const rclcpp_lifecycle::State &previous_state) override;

  CYPHAL_HARDWARE_INTERFACE_PUBLIC
  hardware_interface::CallbackReturn
  on_deactivate(const rclcpp_lifecycle::State &previous_state) override;

  CYPHAL_HARDWARE_INTERFACE_PUBLIC
  hardware_interface::return_type read(const rclcpp::Time &time,
                                       const rclcpp::Duration &period) override;

  CYPHAL_HARDWARE_INTERFACE_PUBLIC
  hardware_interface::return_type
  write(const rclcpp::Time &time, const rclcpp::Duration &period) override;

private:
  const uint16_t readiness_port = 10;
  const uint16_t dynamics_state_port = 100;

  std::map<int, cyphalUdralServo> servos;

  int can_socket = -1;

  int next_transfer_id = 0;
  struct CanardInstance canard;
  struct CanardTxQueue canard_tx_queues[CAN_REDUNDANCY_FACTOR];

  struct CanardMemoryResource canard_memory;

  std::thread read_thread_handle;

  void send(const CanardMicrosecond tx_deadline_usec,
            const CanardTransferMetadata *const metadata,
            const size_t payload_size, const void *const payload_data,
            const CanardMicrosecond now_usec);

  void processReceivedTransfer(const struct CanardRxTransfer *const transfer,
                               const CanardMicrosecond now_usec);
  void read_register_list(int node_id);
  void read_register_list_idx(unsigned char node_id, int register_idx);
  void read_register(cyphalUdralServo &node_id, std::string name);
  void write_register(cyphalUdralServo &servo, std::string name,
                      uavcan_register_Value_1_0 value);

  void send_readiness(enum arming_state state);

  void write_can();
  /**
   * @brief Reads CAN messages, stops when is_disconnect_requested_ is set to
   * true
   */
  void read_thread();

  /**
   * @brief Static method to call read_thread via std::thread
   * @param context pointer to this
   */
  static void read_thread_static(void *context);
};

} // namespace cyphal_hardware_interface

#endif // CYPHAL_HARDWARE_INTERFACE__CYPHAL_SYSTEM_HPP_
