#include "module/chassis.hpp"
#include "bsp/can.h"
#include "device/motor_rm.h"


Chassis::Chassis() {
    motors_param[0] = {
        .can = BSP_CAN_1,
        .id = 0x201,
        .module = MOTOR_M3508,
        .reverse = false,
        .gear = true
    };
    motors_param[1] = {
        .can = BSP_CAN_1,
        .id = 0x202,
        .module = MOTOR_M3508,
        .reverse = false,
        .gear = true
    };
    motors_param[2] = {
        .can = BSP_CAN_1,
        .id = 0x203,
        .module = MOTOR_M3508,
        .reverse = false,
        .gear = true
    };
    motors_param[3] = {
        .can = BSP_CAN_1,
        .id = 0x204,
        .module = MOTOR_M3508,
        .reverse = false,
        .gear = true
    };
}

void Chassis::init() {
    BSP_CAN_Init();
    MOTOR_RM_Register(&motors_param[0]);
    MOTOR_RM_Register(&motors_param[1]);
    MOTOR_RM_Register(&motors_param[2]);
    MOTOR_RM_Register(&motors_param[3]);
}

void Chassis::operator()() {
    MOTOR_RM_UpdateAll();

    MOTOR_RM_t *motor = NULL;
    motor = MOTOR_RM_GetMotor(&motors_param[0]);
    chassis_motors[0] = motor->motor;
    motor = MOTOR_RM_GetMotor(&motors_param[1]);
    chassis_motors[1] = motor->motor;
    motor = MOTOR_RM_GetMotor(&motors_param[2]);
    chassis_motors[2] = motor->motor;
    motor = MOTOR_RM_GetMotor(&motors_param[3]);
    chassis_motors[3] = motor->motor;

    // MOTOR_RM_SetOutput(&motors_param[0], 0.1f);
    // MOTOR_RM_Ctrl(&motors_param[0]);


}