#pragma once

#ifdef __cplusplus
extern "C" {
#endif

#include "bsp/struct_typedef.h"
#include "component/pid.h"
#include "device/motor_rm.h"
#include <stdint.h>

typedef struct {
  MOTOR_RM_Param_t m2006_left_param;
  MOTOR_RM_Param_t m2006_right_param;
  MOTOR_RM_Param_t m3508_left_param;
  MOTOR_RM_Param_t m3508_right_param;
  MOTOR_RM_Param_t M6020_param;

  KPID_Params_t pid_2006_params;
  KPID_Params_t pid_3508_params;
  KPID_Params_t pid_6020_params;

} Up_params_t;

typedef struct {
  float motor2006_left_out;
  float motor2006_right_out;
  float motor3508_left_out;
  float motor3508_right_out;
  float motor6020_out;
} Motor_out_t;

typedef struct {
  uint32_t last_wakeup;
  float dt;

  struct {
    fp32 motor_2006_left_speed;
    fp32 motor_2006_right_speed;
    fp32 motor_2006_left_angle;
    fp32 motor_2006_right_angle;

    fp32 motor_3508_left_speed;
    fp32 motor_3508_right_speed;
    fp32 motor_3508_left_angle;
    fp32 motor_3508_right_angle;

    fp32 motor_6020_speed;
    fp32 motor_6020_angle;

  } feedback;

  struct {
    KPID_t motor_2006_angle_pid[2];
    KPID_t motor_2006_omega_pid[2];
    KPID_t motor_3508_angle_pid[2];
    KPID_t motor_3508_omega_pid[2];
    KPID_t motor_6020_angle_pid;
    KPID_t motor_6020_omega_pid;
  } pid;

  Up_params_t *params;

} UP_t;

#ifdef __cplusplus
}
#endif