/* Correction du TP 3 (partie 3, ROS) -- services et parametres.
 *
 * Reprend le projet du TP 2. Ajouts CubeMX : PC13 en sortie (BUZZER).
 * colcon.meta (microros_static_library_ide/library_generation) :
 * -DRMW_UXRCE_MAX_SERVICES=8 (2 services + 5 du serveur de parametres + 1 de
 * marge), puis suppression de microros_static_library_ide/libmicroros et
 * recompilation.
 * Pour rester courte, cette correction n'a pas la machine a etats du TP 2 :
 * creation une fois, puis boucle.
 */

/* ===== USER CODE BEGIN Includes ============================================= */
#include <string.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <rclc_parameter/rclc_parameter.h>
#include <uxr/client/transport.h>
#include <rmw_microxrcedds_c/config.h>
#include <rmw_microros/rmw_microros.h>
#include <std_srvs/srv/set_bool.h>
#include <std_srvs/srv/trigger.h>
#include <std_msgs/msg/int32.h>
/* USER CODE END Includes */

/* ===== USER CODE BEGIN PD =================================================== */
#define POSTE_DOMAIN_ID  11u     /* 10 + numero du poste ; JAMAIS 77 */
#define BUZZER_GPIO_Port GPIOC
#define BUZZER_Pin       GPIO_PIN_13
#define TMP102_ADDR      (0x48u << 1)
/* 2 services + le serveur de parametres */
#define EXEC_HANDLES     (2u + RCLC_EXECUTOR_PARAMETER_SERVER_HANDLES)
/* USER CODE END PD */

/* ===== USER CODE BEGIN 0 ==================================================== */
bool   cubemx_transport_open(struct uxrCustomTransport *transport);
bool   cubemx_transport_close(struct uxrCustomTransport *transport);
size_t cubemx_transport_write(struct uxrCustomTransport *transport,
                              const uint8_t *buf, size_t len, uint8_t *err);
size_t cubemx_transport_read(struct uxrCustomTransport *transport,
                             uint8_t *buf, size_t len, int timeout, uint8_t *err);
void *microros_allocate(size_t size, void *state);
void  microros_deallocate(void *pointer, void *state);
void *microros_reallocate(void *pointer, size_t size, void *state);
void *microros_zero_allocate(size_t number_of_elements, size_t size_of_element,
                             void *state);

static rclc_parameter_server_t params;
static rcl_service_t srv_buzzer;
static rcl_service_t srv_reset;
static std_srvs__srv__SetBool_Request   req_buzzer;
static std_srvs__srv__SetBool_Response  res_buzzer;
static std_srvs__srv__Trigger_Request   req_reset;
static std_srvs__srv__Trigger_Response  res_reset;
static char res_buzzer_text[32];
static char res_reset_text[32];
static volatile int32_t g_counter;

/* Copie un texte dans une chaine micro-ROS dont la memoire est a nous. */
static void set_text(rosidl_runtime_c__String *s, char *buf, size_t cap, const char *txt)
{
    strncpy(buf, txt, cap - 1u);
    buf[cap - 1u] = '\0';
    s->data     = buf;
    s->size     = strlen(buf);
    s->capacity = cap;
}

/* Etape 1 : buzzer, si le parametre buzzer_enabled l'autorise (etape 3). */
static void buzzer_cb(const void *req_in, void *res_out)
{
    const std_srvs__srv__SetBool_Request *req = (const std_srvs__srv__SetBool_Request *)req_in;
    std_srvs__srv__SetBool_Response      *res = (std_srvs__srv__SetBool_Response *)res_out;
    bool enabled = true;

    if ((rclc_parameter_get_bool(&params, "buzzer_enabled", &enabled) == RCL_RET_OK)
        && !enabled)
    {
        res->success = false;
        set_text(&res->message, res_buzzer_text, sizeof res_buzzer_text, "buzzer disabled");
        return;
    }
    HAL_GPIO_WritePin(BUZZER_GPIO_Port, BUZZER_Pin, req->data ? GPIO_PIN_SET : GPIO_PIN_RESET);
    res->success = true;
    set_text(&res->message, res_buzzer_text, sizeof res_buzzer_text,
             req->data ? "buzzer on" : "buzzer off");
}

/* Etape 2 : remise a zero du compteur. */
static void reset_cb(const void *req_in, void *res_out)
{
    std_srvs__srv__Trigger_Response *res = (std_srvs__srv__Trigger_Response *)res_out;

    (void)req_in;
    g_counter = 0;
    res->success = true;
    set_text(&res->message, res_reset_text, sizeof res_reset_text, "counter reset");
}

/* Etape 3 : validation. old_param NULL = creation, new_param NULL =
 * suppression : on accepte la premiere, on refuse la seconde. */
static bool on_param_changed(const Parameter *old_param, const Parameter *new_param,
                             void *context)
{
    (void)context;
    if (old_param == NULL) { return true; }
    if (new_param == NULL) { return false; }

    if (strcmp(new_param->name.data, "blink_period_ms") == 0)
    {
        int64_t v = new_param->value.integer_value;

        return (v >= 50) && (v <= 5000);
    }
    if (strcmp(new_param->name.data, "temp_alarm") == 0)
    {
        double v = new_param->value.double_value;

        return (v > 0.0) && (v < 100.0);
    }
    return true;     /* buzzer_enabled : tout booleen convient */
}

static int tmp102_temp_mC(int32_t *out)
{
    uint8_t b[2];
    int32_t raw;

    if (HAL_I2C_Mem_Read(&hi2c2, TMP102_ADDR, 0x00u, I2C_MEMADD_SIZE_8BIT,
                         b, 2u, 5u) != HAL_OK) { return 0; }
    raw = (int32_t)((((uint16_t)b[0] << 8) | b[1]) >> 4);
    if (raw & 0x800) { raw -= 4096; }
    *out = (raw * 625) / 10;
    return 1;
}

static void stop_on_error(void)
{
    HAL_GPIO_WritePin(LED_4_GPIO_Port, LED_4_Pin, GPIO_PIN_SET);
    for (;;) { osDelay(1000); }
}
#define CHECK(call)  do { if ((call) != RCL_RET_OK) { stop_on_error(); } } while (0)
/* USER CODE END 0 */

void StartDefaultTask(void *argument)
{
    rcl_allocator_t    allocator;
    rcl_init_options_t opts;
    rclc_support_t     support;
    rcl_node_t         node;
    rclc_executor_t    executor;
    rclc_parameter_options_t o = {
        .notify_changed_over_dds     = true,
        .max_params                  = 4,
        .allow_undeclared_parameters = false,
        .low_mem_mode                = false,
    };
    uint32_t last_blink = 0u;
    uint32_t last_temp  = 0u;

    (void)argument;
    rmw_uros_set_custom_transport(true, (void *)&huart5,
                                  cubemx_transport_open, cubemx_transport_close,
                                  cubemx_transport_write, cubemx_transport_read);
    allocator = rcutils_get_zero_initialized_allocator();
    allocator.allocate      = microros_allocate;
    allocator.deallocate    = microros_deallocate;
    allocator.reallocate    = microros_reallocate;
    allocator.zero_allocate = microros_zero_allocate;
    if (!rcutils_set_default_allocator(&allocator)) { stop_on_error(); }

    while (rmw_uros_ping_agent(200, 1) != RMW_RET_OK)
    {
        HAL_GPIO_TogglePin(LED_1_GPIO_Port, LED_1_Pin);
        osDelay(200);
    }
    HAL_GPIO_WritePin(LED_1_GPIO_Port, LED_1_Pin, GPIO_PIN_SET);

    allocator = rcl_get_default_allocator();
    opts      = rcl_get_zero_initialized_init_options();
    CHECK(rcl_init_options_init(&opts, allocator));
    CHECK(rcl_init_options_set_domain_id(&opts, POSTE_DOMAIN_ID));
    CHECK(rclc_support_init_with_options(&support, 0, NULL, &opts, &allocator));
    CHECK(rclc_node_init_default(&node, "ratbot_lab", "tp", &support));

    /* Etapes 1 et 2 : les services ; le second echoue avec MAX_SERVICES = 1 */
    CHECK(rclc_service_init_default(&srv_buzzer, &node,
              ROSIDL_GET_SRV_TYPE_SUPPORT(std_srvs, srv, SetBool), "buzzer"));
    CHECK(rclc_service_init_default(&srv_reset, &node,
              ROSIDL_GET_SRV_TYPE_SUPPORT(std_srvs, srv, Trigger), "reset_counter"));

    /* Etape 3 : le serveur de parametres */
    CHECK(rclc_parameter_server_init_with_option(&params, &node, &o));

    executor = rclc_executor_get_zero_initialized_executor();
    CHECK(rclc_executor_init(&executor, &support.context, EXEC_HANDLES, &allocator));
    CHECK(rclc_executor_add_service(&executor, &srv_buzzer, &req_buzzer,
                                    &res_buzzer, buzzer_cb));
    CHECK(rclc_executor_add_service(&executor, &srv_reset, &req_reset,
                                    &res_reset, reset_cb));
    CHECK(rclc_executor_add_parameter_server(&executor, &params, on_param_changed));

    CHECK(rclc_add_parameter(&params, "blink_period_ms", RCLC_PARAMETER_INT));
    CHECK(rclc_parameter_set_int(&params, "blink_period_ms", 500));
    CHECK(rclc_add_parameter(&params, "temp_alarm", RCLC_PARAMETER_DOUBLE));
    CHECK(rclc_parameter_set_double(&params, "temp_alarm", 35.0));
    CHECK(rclc_add_parameter(&params, "buzzer_enabled", RCLC_PARAMETER_BOOL));
    CHECK(rclc_parameter_set_bool(&params, "buzzer_enabled", true));

    for (;;)
    {
        int64_t period = 500;
        double  alarm  = 35.0;

        (void)rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10));

        (void)rclc_parameter_get_int(&params, "blink_period_ms", &period);
        (void)rclc_parameter_get_double(&params, "temp_alarm", &alarm);

        if (osKernelGetTickCount() - last_blink >= (uint32_t)period)
        {
            last_blink = osKernelGetTickCount();
            HAL_GPIO_TogglePin(LED_2_GPIO_Port, LED_2_Pin);
            g_counter++;
        }
        if (osKernelGetTickCount() - last_temp >= 1000u)
        {
            int32_t t_mC;

            last_temp = osKernelGetTickCount();
            if (tmp102_temp_mC(&t_mC))
            {
                HAL_GPIO_WritePin(LED_3_GPIO_Port, LED_3_Pin,
                                  ((double)t_mC / 1000.0 > alarm) ? GPIO_PIN_SET
                                                                  : GPIO_PIN_RESET);
            }
        }
    }
}

/* Reponses attendues aux questions :
 *  1. success: true, message: 'buzzer on'. Carte debranchee : l'appel attend
 *     indefiniment ("waiting for service to become available..."), le
 *     service n'existe plus dans le graphe.
 *  2. On sait si l'ordre a ete execute (reponse), et par qui ; un topic
 *     peut etre perdu sans que l'emetteur le sache.
 *  3. Le client micro-ROS reserve statiquement toute sa memoire (tableaux de
 *     taille fixe), pour ne jamais allouer au fil de l'eau sur un
 *     microcontroleur : pas de fragmentation, une empreinte connue a la
 *     compilation.
 *  4. "Setting parameter failed" : refus decide par on_param_changed, sur la
 *     carte ; la valeur reste 100.
 *  5. Les valeurs de depart du programme : les parametres vivent en RAM. Le
 *     robot enregistre les siens dans sa flash de donnees (SPI, U7) et les
 *     relit au demarrage.
 *  6. get_parameters, get_parameter_types, set_parameters,
 *     list_parameters, describe_parameters (pas set_parameters_atomically
 *     dans rclc).
 */