#include "Converter.h"

#define DELAY 2  // ms

HAL_StatusTypeDef spi2can(SPI_HandleTypeDef *hspi, CAN_HandleTypeDef *hcan) {
    // Standard at FaSTTUBe - Sadly. Please fix this with CAN FD for such sensors.
    size_t datalen = 8;
    HAL_StatusTypeDef status = HAL_OK;

    // ==================
    // Hardware Version
    // ==================
    // auto request_hw_version = vn::pkg_request_read_t(vn::HardwareVersion::ID);
    // auto response_hw_version = vn::pkg_response_t<vn::HardwareVersionRegister>();
    // spi_read(hspi, &request_hw_version, &response_hw_version);

    // ==================
    // Hardware Version --> 2001
    // ==================
    // auto request_fw_version = vn::pkg_request_read_t(vn::FirmwareVersion::ID);
    // auto response_fw_version = vn::pkg_response_t<vn::FirmwareVersionRegister>();
    // spi_read(hspi, &request_fw_version, &response_fw_version);

    // ==================
    // ATT YawPitchRoll
    // ==================
    // auto request_att_ypr = vn::pkg_request_read_t(vn::YawPitchRoll::ID);
    // auto response_att_ypr = vn::pkg_response_t<vn::YawPitchRollRegister>();
    // spi_read(hspi, &request_att_ypr, &response_att_ypr);

    // ==================
    // IMU Accel
    // ==================
    // auto request_imu_accel = vn::pkg_request_read_t(vn::ImuAccelerometer::ID);
    // auto response_imu_accel = vn::pkg_response_t<vn::ImuAccelerometerRegister>();
    // spi_read(hspi, &request_imu_accel, &response_imu_accel);

    // ==================
    // IMU Gyro
    // ==================
    auto request_imu_gyro = vn::pkg_request_read_t(vn::ImuGyro::ID);
    auto response_imu_gyro = vn::pkg_response_t<vn::ImuGyroRegister>();
    spi_read(hspi, &request_imu_gyro, &response_imu_gyro);

    // ==================
    // INS
    // ==================
    // EXTRACT INS
    auto request_ins_solution_lla = vn::pkg_request_read_t(vn::InsSolutionLla::ID);
    auto response_ins_solution_lla = vn::pkg_response_t<vn::InsSolutionLlaRegister>();

    spi_read(hspi, &request_ins_solution_lla, &response_ins_solution_lla);

    vn::InsSolutionLlaRegister payload = response_ins_solution_lla.payload;
    // SEND INS YPR
    auto data_ins_ypr = canlib::encode::can1::vn200_ins_ypr(canlib::frame::decoded::can1::vn200_ins_ypr_t(
        (double)payload.yawPitchRoll.x, (double)payload.yawPitchRoll.y, (double)payload.yawPitchRoll.z, (double)payload.attUncertainty));
    uint8_t packed_data_ins_ypr[datalen];
    can1_vn200_ins_ypr_pack(packed_data_ins_ypr, &data_ins_ypr, datalen);
    status = ftcan_transmit(hcan, CAN1_VN200_INS_YPR_FRAME_ID, packed_data_ins_ypr, datalen);

    HAL_Delay(DELAY);

    // SEND INS LLA
    auto data_ins_ll = canlib::encode::can1::vn200_ins_ll(canlib::frame::decoded::can1::vn200_ins_ll_t(
        (double)payload.position.x, (double)payload.position.y, (double)payload.posUncertainty, (double)payload.posUncertainty));
    uint8_t packed_data_ins_ll[datalen];
    can1_vn200_ins_ll_pack(packed_data_ins_ll, &data_ins_ll, datalen);
    status = ftcan_transmit(hcan, CAN1_VN200_INS_LL_FRAME_ID, packed_data_ins_ll, datalen);

    HAL_Delay(DELAY);

    // SEND INS VEL
    auto data_ins_vel = canlib::encode::can1::vn200_ins_vel(canlib::frame::decoded::can1::vn200_ins_vel_t(
        (double)payload.nedVel.x, (double)payload.nedVel.y, (double)payload.velUncertainty, (double)payload.velUncertainty));
    uint8_t packed_data_ins_vel[datalen];
    can1_vn200_ins_vel_pack(packed_data_ins_vel, &data_ins_vel, datalen);
    status = ftcan_transmit(hcan, CAN1_VN200_INS_VEL_FRAME_ID, packed_data_ins_vel, datalen);

    HAL_Delay(DELAY);

    // ==================
    // IMU
    // ==================
    // EXTRACT IMU
    auto request_imu = vn::pkg_request_read_t(vn::MagneticAccelerationAndAngularRates::ID);
    auto response_imu = vn::pkg_response_t<vn::MagneticAccelerationAndAngularRatesRegister>();

    spi_read(hspi, &request_imu, &response_imu);

    vn::MagneticAccelerationAndAngularRatesRegister payload_imu = response_imu.payload;
    // SEND ACC LIN
    const auto data_imu_acc_lin = canlib::encode::can1::vn200_imu_acc_lin(
        canlib::frame::decoded::can1::vn200_imu_acc_lin_t((double)payload_imu.accel.x, (double)payload_imu.accel.y, (double)payload_imu.accel.z));
    uint8_t packed_data_imu_acc_lin[datalen];
    can1_vn200_imu_acc_lin_pack(packed_data_imu_acc_lin, &data_imu_acc_lin, datalen);
    status = ftcan_transmit(hcan, CAN1_VN200_IMU_ACC_LIN_FRAME_ID, packed_data_imu_acc_lin, datalen);

    HAL_Delay(DELAY);

    // SEND ACC ANG
    auto data_imu_acc_ang = canlib::encode::can1::vn200_imu_acc_ang(
        canlib::frame::decoded::can1::vn200_imu_acc_ang_t((double)payload_imu.gyro.x, (double)payload_imu.gyro.y, (double)payload_imu.gyro.z));
    uint8_t packed_data_imu_acc_ang[datalen];
    can1_vn200_imu_acc_ang_pack(packed_data_imu_acc_ang, &data_imu_acc_ang, datalen);
    status = ftcan_transmit(hcan, CAN1_VN200_IMU_ACC_ANG_FRAME_ID, packed_data_imu_acc_ang, datalen);

    HAL_Delay(DELAY);

    // ==================
    // GNSS
    // ==================
    // EXTRACT GNSS LLA
    auto request_gnss = vn::pkg_request_read_t(vn::GpsSolutionLla::ID);
    auto response_gnss = vn::pkg_response_t<vn::GpsSolutionLlaRegister>();

    spi_read(hspi, &request_gnss, &response_gnss);

    vn::GpsSolutionLlaRegister payload_gnss = response_gnss.payload;
    // SEND ACC LIN
    auto data_gnss = canlib::encode::can1::vn200_gnss_ll(canlib::frame::decoded::can1::vn200_gnss_ll_t(
        (double)payload_gnss.lla.x, (double)payload_gnss.lla.y, (double)payload_gnss.nedAcc.x, (double)payload_gnss.nedAcc.y));
    uint8_t packed_data_gnss[datalen];
    can1_vn200_gnss_ll_pack(packed_data_gnss, &data_gnss, datalen);
    status = ftcan_transmit(hcan, CAN1_VN200_GNSS_LL_FRAME_ID, packed_data_gnss, datalen);

    HAL_Delay(DELAY);

    return HAL_OK;
}