#include "can_receiver.hpp"
#include "driver/twai.h"
#include <string.h>

void CANReceiver::Init()
{
    twai_general_config_t g_config =
        TWAI_GENERAL_CONFIG_DEFAULT(
            GPIO_NUM_21,
            GPIO_NUM_22,
            TWAI_MODE_NORMAL
        );

    twai_timing_config_t t_config =
        TWAI_TIMING_CONFIG_500KBITS();

    twai_filter_config_t f_config =
        TWAI_FILTER_CONFIG_ACCEPT_ALL();

    twai_driver_install(
        &g_config,
        &t_config,
        &f_config
    );

    twai_start();
}

bool CANReceiver::Read(uint16_t& ppm)
{
    twai_message_t rx;

    if(twai_receive(&rx, pdMS_TO_TICKS(1000)) == ESP_OK)
    {
        ppm =
            (rx.data[0] << 8) |
            rx.data[1];

        return true;
    }

    return false;
}