#include "led_controller.hpp"
#include "devices.hpp"
#include "rcl_util.hpp"

#include <zephyr/drivers/gpio.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/bool.h>

std_msgs__msg__Bool led_msg;
rcl_node_t LedController::node;
rcl_subscription_t LedController::led0_subscription;
/*
rcl_subscription_t LedController::led1_subscription;
rcl_subscription_t LedController::led2_subscription;
*/

void led0_callback(const void *msg){
    gpio_pin_configure_dt(&led0, ((std_msgs__msg__Bool *)msg)->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
}


/*
void led1_callback(const void *msg){
    gpio_pin_configure_dt(&led1, ((std_msgs__msg__Bool *)msg)->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
}


void led2_callback(const void *msg){
    gpio_pin_configure_dt(&led2, ((std_msgs__msg__Bool *)msg)->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
}
*/


rcl_ret_t LedController::setup(rclc_support_t* support, rclc_executor_t* executor) {
    RCL_RETURN_ON_ERROR(rclc_node_init_default(
        &LedController::node, 
        "led_controller",
        "",
        support));

    RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
        &LedController::led0_subscription,
        &LedController::node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
        "/nucleo/led0"));
       
    /*
    RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
        &LedController::led1_subscription,
        &LedController::node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
        "/nucleo/led1"));

    RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
        &LedController::led2_subscription,
        &LedController::node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
        "/nucleo/led2"));
    */

    RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
        executor,
        &LedController::led0_subscription,
        &led_msg,
        led0_callback,
        ON_NEW_DATA
    ));

    /*
    RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
        executor,
        &LedController::led1_subscription,
        &led_msg,
        led1_callback,
        ON_NEW_DATA
    ));

    RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
        executor,
        &LedController::led2_subscription,
        &led_msg,
        led2_callback,
        ON_NEW_DATA
    ));
    */

    return RCL_RET_OK;
}


void LedController::kill() {
    rcl_subscription_fini(&LedController::led0_subscription, &LedController::node);
    //rcl_subscription_fini(&LedController::led1_subscription, &LedController::node);
    //rcl_subscription_fini(&LedController::led2_subscription, &LedController::node);
    rcl_node_fini(&LedController::node);
}