#ifndef HEXAPOD_ROBOT_HWI_
#define HEXAPOD_ROBOT_HWI_

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

#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/macros.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"

namespace hexapod_robot {

struct PWMOutput {
    uint8_t i2c_address;
    uint8_t channel;
    double counts_intersect;
    double counts_slope;
    double counts_min;
    double counts_max;
};

class HexapodRobotHardwareInterface : public hardware_interface::SystemInterface {

public:
    RCLCPP_SHARED_PTR_DEFINITIONS(HexapodRobotHardwareInterface)
    virtual ~HexapodRobotHardwareInterface();
    
    hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override;
    
    std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
    std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
    
    hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override;
    hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
    
    hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
    hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;

private:
    bool servos_powered_;

    std::map<std::string, double> joint_commands_;
    std::map<std::string, PWMOutput> joint_pwm_mapping_;

    bool i2c_available_;
    int i2c_fd_;

    std::map<uint8_t, bool> pca_available_;

    bool i2c_open_(const std::string& device);

    const uint8_t pca_reg_mode1_        = 0x00;
    const uint8_t pca_reg_mode2_        = 0x01;
    const uint8_t pca_reg_prescale_     = 0xfe;

    // base address for LED0 following, 
    // other channels can be calculated by adding 
    //     channel_number * 4
    // to each base address

    const uint8_t pca_reg_led_on_l_     = 0x06;
    const uint8_t pca_reg_led_on_h_     = 0x07;
    const uint8_t pca_reg_led_off_l_    = 0x08;
    const uint8_t pca_reg_led_off_h_    = 0x09;

    const uint8_t pca_mode1_sleep_      = 0x10;
    const uint8_t pca_mode1_restart_    = 0x80;
    const uint8_t pca_mode2_outdrv_     = 0x04;


    const double pca_osc_frequency_     = 25000000; // 25 Mhz
    const double pca_pwm_frequency_     = 50;       // 50 Hz
    const double pca_prescaler_value_   = (pca_osc_frequency_ / (4096.0 * pca_pwm_frequency_) - 0.5); // according to data sheet
    const double pca_pwm_cycle_usec_    = 1000000 / pca_pwm_frequency_; // microseconds per PWM counter cycle
    const double pca_pwm_count_usec_    = pca_pwm_cycle_usec_ / 4096;   // microseconds per PWM single count

    void pca_initialize_();
    bool pca_set_pwm_(uint8_t i2c_address, uint8_t channel_number, uint16_t counts);

};

} // namespace

#endif // HEXAPOD_ROBOT_HWI_