#ifndef PIBOT_ROBOT_H_
#define PIBOT_ROBOT_H_

#include "dataframe.h"

#if ROBOT_MODEL == MODEL_TYPE_2WD_DIFF
#include "differential.h"
#endif

#if ROBOT_MODEL == MODEL_TYPE_3WD_OMNI
#include "omni3.h"
#endif

class MotorController;
class Encoder;
class PID;
class Transport;
class Dataframe;
class Model;
class IMU;
class Joystick;

// Robot 类继承Notify接口， 实现一个对接收的消息id处理（update接口中)
class Robot:public Notify
{
public:
    static Robot* get() {
        static Robot robot;
        return &robot;
    }
    
    // 初始化
    void init();

    // 检测串口数据命令
    void check_command();

    // 运动解算
    void do_kinmatics();

    // 计算里程计
    void calc_odom();

    //获取imu数据
    void get_imu_data();

    // 检测joystick的按键
    void check_joystick();
    
    // 实现Notify接口处理  绑定的消息
    virtual void update(const MESSAGE_ID id, void* data);
private:
    Robot();
    //初始化
    void init_imu();
    void init_joystick();
    void init_motor();
    void init_trans();
private:
    // 清除里程计
    void clear_odom();

    // 更新当前机器人的速度
    void update_velocity();

    // 更新编码器值
    void update_encoder_count();

    // 更新设置pwm标志
    void update_pwm_value();
private:
    // 电机控制器接口
    MotorController* motor[MOTOR_COUNT];

    // 电机编码器接口
    Encoder*        encoder[MOTOR_COUNT];

    // PID接口 用于电机调速
    PID*            pid[MOTOR_COUNT];

    // 每个轮子编码器在pid时间间隔内期望变化值(根据输入速度计算得到)
    float           input[MOTOR_COUNT];

    // 每个轮子编码器在pid时间间隔内实际变化值(根据编码器的反馈计算得到)
    float           feedback[MOTOR_COUNT];

    // 用于通讯的接口  (uart/usb)
    Transport*      trans;

    // 用于通讯的通讯协议接口（自定义协议)
    Dataframe*      frame;

    // 机器人模型接口， 用于运动解算和反解算
    Model*          model;

    // 是否进行运动计算的标志 （接收到新的速度时候更新）
    bool            do_kinmatics_flag;

    // 是否进行设置pwm的标志 （接收到新的电机控制命令时候更新）
    bool            do_set_pwm_flag;
    
    // 保存里程计信息
    Odom            odom;

    // 上一次收到命令的时间戳 用于超时计算， 在一段时间超时后仍没收到命令后会停止电机
    unsigned long   last_velocity_command_time;
    
#if IMU_ENABLE
    // imu接口
    IMU* imu;
#endif

#if JOYSTICK_ENABLE
    // 遥控器接口
    Joystick* joystick;
#endif
};

#endif