#pragma once

#include "ProtocolStructures.h"
#include <atv.h>
#include <nlohmann/json.hpp>
#include <optional>
#include <std_msgs/msg/string.h>
#include <type_traits>

template<typename E>
constexpr auto to_underlying(E e) noexcept
{
    return static_cast<std::underlying_type_t<E>>(e);
}


inline std::optional<nlohmann::json> ParseToJSON(const void* msgin)
{
    if (msgin == nullptr)
    {
        return std::nullopt;
    }
    const auto* msg = (const std_msgs__msg__String*)msgin;
    nlohmann::json j = nlohmann::json::parse(msg->data.data, msg->data.data + msg->data.size, nullptr, false);
    if (j.is_discarded())
    {
        return std::nullopt;
    }
    return j;
}


inline Auto_VCU_Vehicle_Mode_Req_t ParseToATV(const Auto_Vehicle_Mode_Req& in)
{
    Auto_VCU_Vehicle_Mode_Req_t out;
    out.Auto_Allow_VRC_Offline_Req = in.Allow_VRC_Offline ? 1 : 0;
    out.Auto_VCU_Work_Mode_Req = to_underlying(in.Mode);
    return out;
}

inline Auto_Direct_Vehicle_Req_t ParseToATV_Direct_V(const Auto_Direct_Req& in)
{
    Auto_Direct_Vehicle_Req_t out;
    out.Direct_Em_Stop_Req = in.E_Stop ? 1 : 0;
    out.Direct_Gear_Req = to_underlying(in.Gear);
    out.Direct_Tank_Mode_Req = to_underlying(in.Tank);
    out.Direct_Brake_Value_Req_ro = ATV_Direct_Brake_Value_Req_ro_toS(in.Brake);
    out.Direct_Parking_Req_ro = ATV_Direct_Parking_Req_ro_toS(in.Parking);
    out.Direct_St_Value_Req_ro = ATV_Direct_St_Value_Req_ro_toS(in.Steer);
    return out;
}

inline Auto_Direct_MCU_Req_A_t ParseToATV_Direct_A(const Auto_Direct_Req& in)
{
    Auto_Direct_MCU_Req_A_t out;
    out.Direct_MCUFL_Tq_Req_ro = ATV_Direct_MCUFL_Tq_Req_ro_toS(in.FL_Tq);
    out.Direct_MCUFR_Tq_Req_ro = ATV_Direct_MCUFR_Tq_Req_ro_toS(in.FR_Tq);
    out.Direct_MCURL_Tq_Req_ro = ATV_Direct_MCURL_Tq_Req_ro_toS(in.RL_Tq);
    out.Direct_MCURR_Tq_Req_ro = ATV_Direct_MCURR_Tq_Req_ro_toS(in.RR_Tq);
    return out;
}

inline Auto_Direct_MCU_Req_B_t ParseToATV_Direct_B(const Auto_Direct_Req& in)
{
    Auto_Direct_MCU_Req_B_t out;
    out.Direct_MCUML_Tq_Req_ro = ATV_Direct_MCUML_Tq_Req_ro_toS(in.ML_Tq);
    out.Direct_MCUMR_Tq_Req_ro = ATV_Direct_MCUMR_Tq_Req_ro_toS(in.MR_Tq);
    return out;
}

inline Auto_Indirect_Vehicle_Req_t ParseToATV(const Auto_Indirect_Req& in)
{
    Auto_Indirect_Vehicle_Req_t out;
    out.Indirect_Em_Stop_Req = in.E_Stop ? 1 : 0;
    out.Indirect_Gear_Req = to_underlying(in.Gear);
    out.Indirect_Tank_Mode_Req = to_underlying(in.Tank);
    out.Indirect_Max_Sp_Limit_Req = in.Sp_Limit;
    out.Indirect_Target_Sp_Req_ro = ATV_Indirect_Target_Sp_Req_ro_toS(in.Target_Sp);
    out.Indirect_Brake_Value_Req_ro = ATV_Indirect_Brake_Value_Req_ro_toS(in.Brake);
    out.Indirect_Parking_Req_ro = ATV_Indirect_Parking_Req_ro_toS(in.Parking);
    out.Indirect_St_Value_Req_ro = ATV_Indirect_St_Value_Req_ro_toS(in.Steer);
    return out;
}

inline VCU_Vehicle_Status Generate_VCU_Vehicle_Status(VCU_Vehicle_Status_FB_t& fb,
                                                      VCU_Vehicle_Status_FB_A_t& fba,
                                                      VCU_Vehicle_Status_FB_B_t& fbb)
{
    VCU_Vehicle_Status out{};
    out.Mode = (Vehicle_Ctrl_Mode)fb.VCU_Vehicle_Ctrl_Mode_FB;
    out.Speed = ATV_VCU_Vehicle_Speed_FB_ro_fromS(fb.VCU_Vehicle_Speed_FB_ro);
    out.Battery = ATV_VCU_Vehicle_SOC_FB_ro_fromS(fb.VCU_Vehicle_SOC_FB_ro);
    out.Ready = (VCU_Veh_Ready_Status)fb.VCU_Veh_Ready_Status_FB;
    out.Gear = (Vehicle_Gear)fb.VCU_Gear_Status_FB;
    out.Parking = (Parking_Status)fb.VCU_Parking_Status_FB;
    out.E_Switch = (VCU_Emergency_Switch)fb.VCU_Emergency_Switch_FB;
    out.FL_Rpm = ATV_VCU_FLMotor_Rpm_FB_ro_fromS(fba.VCU_FLMotor_Rpm_FB_ro);
    out.FR_Rpm = ATV_VCU_FRMotor_Rpm_FB_ro_fromS(fba.VCU_FRMotor_Rpm_FB_ro);
    out.RL_Rpm = ATV_VCU_RLMotor_Rpm_FB_ro_fromS(fba.VCU_RLMotor_Rpm_FB_ro);
    out.RR_Rpm = ATV_VCU_RRMotor_Rpm_FB_ro_fromS(fba.VCU_RRMotor_Rpm_FB_ro);
    out.ML_Rpm = ATV_VCU_MLMotor_Rpm_FB_ro_fromS(fbb.VCU_MLMotor_Rpm_FB_ro);
    out.MR_Rpm = ATV_VCU_MRMotor_Rpm_FB_ro_fromS(fbb.VCU_MRMotor_Rpm_FB_ro);
    out.FSES_Angle = fbb.VCU_FSES_Angle_FB;
    out.Brake_Pre = ATV_VCU_SEB_Brake_Pre_FB_ro_fromS(fbb.VCU_SEB_Brake_Pre_FB_ro);
    return out;
}