#pragma once

#include "main.h"
#include "bsp/can.h"
#include "device/motor.h"
#include "device/motor_rm.h"
#include "component/pid.h"

typedef struct
{
    float m1;
    float m2;
    float m3;
    float m4;
} motor_speed_t;

class Chassis {
public:
    Chassis();
    void init();
    void operator()();
private:
    void update_motors();
    void calc_chassis();
    void calc_pid();
    void send_control();
    KPID_t pid[4];
    KPID_Params_t pid_params[4];
    motor_speed_t target_speed = {0};
    MOTOR_RM_Param_t motors_param[4];
    MOTOR_RM_t chassis_motors[4];
    float pid_output[4] = {0};
    uint64_t time = 0;
    uint64_t last_time = 0;
    uint64_t dt = 0;
};