/*
 µ×ÅÌÄ£×é
 */
#pragma once


#ifdef __cplusplus
extern "C" {
#endif

/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include "bsp/can.h"
#include "component\filter.h"
#include "component\mixer.h"
#include "component\pid.h"
#include "component\ahrs.h"
#include "device/motor_rm.h"
/* Exported constants ------------------------------------------------------- */
#define CHASSIS_OK (0)        /* ÔËÐÐÕý³£ */
#define CHASSIS_ERR (-1)      /* ÔËÐÐʱ³öÏÖÁËһЩС´íÎó */
#define CHASSIS_ERR_NULL (-2) /* ÔËÐÐʱ·¢ÏÖNULLÖ¸Õë */
#define CHASSIS_ERR_MODE (-3) /* ÔËÐÐʱ³öÅäÖÃÁË´íÎóµÄChassisMode_t */
#define CHASSIS_ERR_TYPE (-4) /* ÔËÐÐʱ³öÅäÖÃÁË´íÎóµÄChassis_Type_t */
	
#define MAX_MOTOR_CURRENT 20.0f 
/* µ×ÅÌ¿ØÖÆģʽ */
typedef enum {
  CHASSIS_MODE_RELAX, /* ·ÅËÉģʽ,µç»ú²»Êä³ö¡£Ò»°ãÇé¿öµ×Å̳õʼ»¯Ö®ºóµÄģʽ */
  CHASSIS_MODE_BREAK, /* ɲ³µÄ£Ê½£¬µç»ú±Õ»·¿ØÖƾ²Ö¹¡£ÓÃÓÚ»úÆ÷ÈËֹͣ״̬ */
  CHASSIS_MODE_FOLLOW_GIMBAL, /* ͨ¹ý±Õ»·¿ØÖÆʹ³µÍ··½Ïò¸úËæÔÆ̨ */
  CHASSIS_MODE_FOLLOW_GIMBAL_35, /* ͨ¹ý±Õ»·¿ØÖÆʹ³µÍ··½Ïò35¡ã¸úËæÔÆ̨ */
  CHASSIS_MODE_ROTOR, /* СÍÓÂÝģʽ£¬Í¨¹ý±Õ»·¿ØÖÆʹµ×Å̲»Í£Ðýת */
  CHASSIS_MODE_INDEPENDENT, /*¶ÀÁ¢Ä£Ê½¡£µ×ÅÌÔËÐв»ÊÜÔÆ̨ӰÏì */
  CHASSIS_MODE_OPEN, /* ¿ª»·Ä£Ê½¡£µ×ÅÌÔËÐв»ÊÜPID¿ØÖÆ£¬Ö±½ÓÊä³öµ½µç»ú */
} Chassis_Mode_t;
	
/* СÍÓÂÝת¶¯Ä£Ê½ */
typedef enum {
  ROTOR_MODE_CW,   /* ˳ʱÕëת¶¯ */
  ROTOR_MODE_CCW,  /* ÄæʱÕëת¶¯ */
  ROTOR_MODE_RAND, /* Ëæ»úת¶¯ */
} Chassis_RotorMode_t;

/* µ×ÅÌ¿ØÖÆÃüÁî */
typedef struct {
  Chassis_Mode_t mode;     /* µ×ÅÌÔËÐÐģʽ */
  Chassis_RotorMode_t mode_rotor; /* СÍÓÂÝת¶¯Ä£Ê½ */
  MoveVector_t ctrl_vec;      /* µ×ÅÌ¿ØÖÆÏòÁ¿ */
} Chassis_CMD_t;

/* ÏÞλ */
typedef struct {
  float max;
  float min;
} Chassis_Limit_t;

	/* µ×ÅÌÀàÐÍ£¨µ×Å̵ÄÎïÀíÉè¼Æ£© */
typedef enum {
  CHASSIS_TYPE_MECANUM,    /* Âó¿ËÄÉÄ·ÂÖ */
  CHASSIS_TYPE_PARLFIX4,   /* ƽÐаڷŵÄËĸöÇý¶¯ÂÖ */
  CHASSIS_TYPE_PARLFIX2,   /* ƽÐаڷŵÄÁ½¸öÇý¶¯ÂÖ */
  CHASSIS_TYPE_OMNI_CROSS, /* ²æÐͰڷŵÄËĸöÈ«ÏòÂÖ */
  CHASSIS_TYPE_OMNI_PLUS,  /* Ê®×ÖÐÍ°ÚÉèµÄËĸöÈ«ÏòÂÖ */
  CHASSIS_TYPE_DRONE,      /* ÎÞÈË»úµ×ÅÌ */
  CHASSIS_TYPE_SINGLE,     /* µ¥¸öĦ²ÁÂÖ */
} Chassis_Type_t;


/* µ×Å̲ÎÊý½á¹¹Ìå,ALL³õʼ»¯²ÎÊý */
typedef struct {
	MOTOR_RM_Param_t motor_param[4];
	  struct {
    KPID_Params_t motor_pid_param;  /* µ×Å̵ç»úPID²ÎÊý */
    KPID_Params_t follow_pid_param; /* ¸úËæÔÆ̨PID²ÎÊý */
  } pid;
  Chassis_Type_t type; /* µ×ÅÌÀàÐÍ£¬µ×Å̵ĻúеÉè¼ÆºÍÂÖ×ÓÑ¡ÐÍ */

  /* µÍͨÂ˲¨Æ÷½ØÖÁƵÂÊ*/
  struct {
    float in;  /* ÊäÈë */
    float out; /* Êä³ö */
  } low_pass_cutoff_freq;

  /* µç»ú·´×°£¬Ó¦¸ÃºÍÔÆ̨ÉèÖÃÏàͬ*/
  struct {
    bool yaw;
  } reverse;
	struct {
        float max_vx, max_vy, max_wz;
        float max_current; 
    } limit; 
} Chassis_Params_t;

typedef struct {
  AHRS_Gyro_t gyro;
  AHRS_Eulr_t eulr;
} Chassis_IMU_t;

typedef struct {
  MOTOR_Feedback_t motor[4];          // Ëĸö 3508µç»ú ·´À¡
	float encoder_gimbalYawMotor;
} Chassis_Feedback_t;

/* µ×ÅÌÊä³ö½á¹¹Ìå*/
typedef struct {
	float motor[4];  
} Chassis_Output_t;

/*
 * ÔËÐеÄÖ÷½á¹¹Ìå£þ
 * °üº¬³õʼ»¯²ÎÊý,Öмä±äÁ¿,Êä³ö±äÁ¿
 */
typedef struct {
  uint64_t last_wakeup;
  float dt;

  Chassis_Params_t *param; /* µ×Å̲ÎÊý,ÓÃChassis_InitÉ趨 */

  /* Ä£¿éͨÓà */
  Chassis_Mode_t mode; /* µ×ÅÌģʽ */


  /* µ×ÅÌÉè¼Æ */
  int8_t num_wheel; /* µ×ÅÌÂÖ×ÓÊýÁ¿ */
  Mixer_t mixer;    /* »ìºÏÆ÷,Òƶ¯ÏòÁ¿->µç»úÄ¿±êÖµ */
  MoveVector_t move_vec; /* µ×ÅÌʵ¼ÊµÄÔ˶¯ÏòÁ¿ */
  MOTOR_RM_t *motors[4];/*Ö¸Ïòµ×ÅÌÿ¸öµç»ú²ÎÊý*/
	float mech_zero;
  float wz_multi; /* СÍÓÂÝÐýתģʽ */

  /* PID¼ÆËãÄ¿±êÖµ */
  struct {
    float motor_rpm[4]; /* µç»úתËٵĶ¯Ì¬Êý×é,µ¥Î»:RPM */
  } setpoint;

  /* ·´À¡¿ØÖÆÓõÄPID */
  struct {
    KPID_t motor[4]; /* ¿ØÖÆÂÖ×Óµç»úÓõÄPIDµÄ¶¯Ì¬Êý×é */
    KPID_t follow; /* ¸úËæÔÆ̨ÓõÄPID */
  } pid;
 
	struct {
        Chassis_Limit_t vx, vy, wz; 
    } limit; 
	
  /* Â˲¨Æ÷ */
  struct {
    LowPassFilter2p_t in[4];  /* ·´À¡ÖµÂ˲¨Æ÷ */
    LowPassFilter2p_t out[4]; /* Êä³öÖµÂ˲¨Æ÷ */
  } filter;

  Chassis_Output_t out; /* µç»úÊä³ö */
	Chassis_Feedback_t feedback;
	//float out_motor[4];
} Chassis_t;

/* Exported functions prototypes -------------------------------------------- */

/**
 * \brief µ×Å̳õʼ»¯
 *
 * \param c °üº¬µ×ÅÌÊý¾ÝµÄ½á¹¹Ìå
 * \param param °üº¬µ×Å̲ÎÊýµÄ½á¹¹ÌåÖ¸Õë
 * \param target_freq ÈÎÎñÔ¤ÆÚµÄÔËÐÐƵÂÊ
 *
 * \return ÔËÐнá¹û
 */
int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
                    float target_freq);

/**
 * \brief ¸üе×ÅÌ·´À¡ÐÅÏ¢
 *
 * \param c °üº¬µ×ÅÌÊý¾ÝµÄ½á¹¹Ìå
 * \param can CANÉ豸½á¹¹Ìå
 *
 * \return ÔËÐнá¹û
 */
int8_t Chassis_UpdateFeedback(Chassis_t *c);

/**
 * \brief ÔËÐе×ÅÌ¿ØÖÆÂß¼­
 *
 * \param c °üº¬µ×ÅÌÊý¾ÝµÄ½á¹¹Ìå
 * \param c_cmd µ×ÅÌ¿ØÖÆÖ¸Áî
 * \param dt_sec Á½´Îµ÷ÓõÄʱ¼ä¼ä¸ô
 *
 * \return ÔËÐнá¹û
 */
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd,
                       uint32_t now);


/**
 * \brief ¸´ÖƵ×ÅÌÊä³öÖµ
 *
 * \param s °üº¬µ×ÅÌÊý¾ÝµÄ½á¹¹Ìå
 * \param out CANÉ豸µ×ÅÌÊä³ö½á¹¹Ìå
 */
void Chassis_Output(Chassis_t *c);

/**
 * \brief Çå¿ÕChassisÊä³öÊý¾Ý
 *
 * \param out CANÉ豸µ×ÅÌÊä³ö½á¹¹Ìå
 */
void Chassis_ResetOutput(Chassis_t *c);


/**
 * @brief µ×Å̹¦ÂÊÏÞÖÆ
 *
 * @param c µ×ÅÌÊý¾Ý
 * @param cap µçÈÝÊý¾Ý
 * @param ref ²ÃÅÐϵͳÊý¾Ý
 * @return º¯ÊýÔËÐнá¹û
 */
//»¹Ã»ÓмÓÈ룬waiting¡£¡£¡£¡£¡£¡£int8_t Chassis_PowerLimit(Chassis_t *c, const CAN_Capacitor_t *cap,
//                          const Referee_ForChassis_t *ref);


/**
 * @brief µ¼³öµ×ÅÌÊý¾Ý
 *
 * @param chassis µ×ÅÌÊý¾Ý½á¹¹Ìå
 * @param ui UIÊý¾Ý½á¹¹Ìå
 */
//void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui);


#ifdef __cplusplus
}
#endif