#include "robot.h"

#include "board.h"
#include "usart_transport.h"
#include "simple_dataframe_slave.h"
#include "data_holder.h"
#include "usart.h"

#if MOTOR_CONTROLLER == COMMON_CONTROLLER
#include "common_motor_controller.h"
#define MAX_PWM_VALUE 5000
#elif MOTOR_CONTROLLER == AF_SHIELD_CONTROLLER
#include "afs_motor_controller.h"
#define MAX_PWM_VALUE 255
#endif

#include "encoder_implement.h"
#include "pid.h"
#include "board_stm32.h"

#if IMU_ENABLE
#include "GY65.h"
#include "GY85.h"
#include "GY87.h"
#endif

#if JOYSTICK_ENABLE
#include "joystick.h"
#endif

#include "print.h"

#define min(a, b) ((a) < (b) ? (a) : (b))
#define max(a, b) ((a) > (b) ? (a) : (b))

Robot::Robot()
{
    for (int i = 0; i < MOTOR_COUNT; i++){
        motor[i]    = NULL;
        encoder[i]  = NULL;
        pid[i]      = NULL;
        input[i]    = 0.0f;
        feedback[i] = 0.0f;
    }

    trans = NULL;
    frame = NULL;
    model = NULL;
    do_kinmatics_flag = false;
    last_velocity_command_time = 0;

    do_set_pwm_flag = false;

#if IMU_ENABLE
    imu = NULL;
#endif

#if JOYSTICK_ENABLE
    joystick = NULL;
#endif
}

void Robot::Init()
{
    // board初始化
    Board::get()->init();

#if DEBUG_ENABLE
    // 调试串口初始化
    Board::get()->Usart_DebugInit();
#endif

    // 加载参数, 该参数一般存储在flash或者eeprom
    DataHolder::get()->loadParameter();

    logi("RobotParameters: %d %d %d %d %d %d %d %d %d %d %d %d %d",
        DataHolder::get()->parameter.params.wheel_diameter, DataHolder::get()->parameter.params.wheel_track, DataHolder::get()->parameter.params.encoder_resolution, DataHolder::get()->parameter.params.motor_ratio,
        DataHolder::get()->parameter.params.do_pid_interval, DataHolder::get()->parameter.params.kp, DataHolder::get()->parameter.params.ki, DataHolder::get()->parameter.params.kd, DataHolder::get()->parameter.params.ko,
        DataHolder::get()->parameter.params.cmd_last_time, DataHolder::get()->parameter.params.max_v_liner_x, DataHolder::get()->parameter.params.max_v_liner_y, DataHolder::get()->parameter.params.max_v_angular_z);

#if IMU_ENABLE
    // imu 初始化
    init_imu();
#endif

    log("init_motor");
    // 电机相关初始化
    InitMotor();

    log("init_trans");
    // 通讯相关初始化
    InitTrans();

    // joystick初始化
    InitJoystick();

    log("gebot init finish.");
}

void Robot::init_imu()
{
#if IMU_ENABLE
    if (DataHolder::get()->parameter.params.imu_type == IMU_TYPE_GY65) {
        log("imu gy65");
        static GY65 gy65;
        imu = &gy65;
    } else if (DataHolder::get()->parameter.params.imu_type == IMU_TYPE_GY85) {
        log("imu gy85");
        static GY85 gy85;
        imu = &gy85;
    } else if (DataHolder::get()->parameter.params.imu_type == IMU_TYPE_GY87) {
        log("imu gy87");
        static GY87 gy87;
        imu = &gy87;
    } else {
        log("imu default null driver");
        imu = NULL;
    }

    if (imu != NULL){
        imu->Init();
    }
#endif
}

void Robot::InitJoystick()
{
#if JOYSTICK_ENABLE
    static Joystick joy;
    joystick = &joy;
    if (joystick)
        joystick->init();
#endif
}

void Robot::InitMotor()
{
    // MOTOR_COUNT为电机数量 该宏定义在相应的机器人模型文件中，  KinematicModels/differential.h
#if MOTOR_COUNT > 0 // 电机1
    // MOTOR_CONTROLLER在params.mk中定义 表示电机控制器类型
    // 当前使用的COMMON_CONTROLLER为2个io控制方向一个PWD控制速度类型的控制器
#if MOTOR_CONTROLLER == COMMON_CONTROLLER
    static CommonMotorController motor1(MOTOR_1, MAX_PWM_VALUE, (DataHolder::get()->parameter.params.motor_nonexchange_flag & MOTOR_ENCODER_1_FLAG) == 0); // MOTOR1_REVERS标识电机方向反向,等同于电机线交换
#elif MOTOR_CONTROLLER == AF_SHIELD_CONTROLLER
    static AFSMotorController motor1(MOTOR_1_PORT_NUM, MAX_PWM_VALUE);
#endif

    // 定义使用的编码器 传入参数为编码器的AB GPIO引脚 定义在params.mk中
    static EncoderImp encoder1(MOTOR_1, (DataHolder::get()->parameter.params.encoder_nonexchange_flag & MOTOR_ENCODER_1_FLAG) == 0);

    // 定义PID控制对象 传入参数为输入和输出的地址及kp ki kd 这里ko为比例
    static PID pid1(&input[0], &feedback[0], float(DataHolder::get()->parameter.params.kp) / DataHolder::get()->parameter.params.ko,
                    float(DataHolder::get()->parameter.params.ki) / DataHolder::get()->parameter.params.ko,
                    float(DataHolder::get()->parameter.params.kd) / DataHolder::get()->parameter.params.ko, MAX_PWM_VALUE);
#endif

#if MOTOR_COUNT > 1 // 电机2
#if MOTOR_CONTROLLER == COMMON_CONTROLLER
    static CommonMotorController motor2(MOTOR_2, MAX_PWM_VALUE, (DataHolder::get()->parameter.params.motor_nonexchange_flag & MOTOR_ENCODER_2_FLAG) == 0); // MOTOR1_REVERS标识电机方向反向,等同于电机线交换
#elif MOTOR_CONTROLLER == AF_SHIELD_CONTROLLER
    static AFSMotorController motor2(MOTOR_2_PORT_NUM, MAX_PWM_VALUE);
#endif
    static EncoderImp encoder2(MOTOR_2, (DataHolder::get()->parameter.params.encoder_nonexchange_flag & MOTOR_ENCODER_2_FLAG) == 0);
    static PID pid2(&input[1], &feedback[1], float(DataHolder::get()->parameter.params.kp) / DataHolder::get()->parameter.params.ko,
                    float(DataHolder::get()->parameter.params.ki) / DataHolder::get()->parameter.params.ko,
                    float(DataHolder::get()->parameter.params.kd) / DataHolder::get()->parameter.params.ko, MAX_PWM_VALUE);
#endif

#if MOTOR_COUNT > 2 // 电机3
#if MOTOR_CONTROLLER == COMMON_CONTROLLER
    static CommonMotorController motor3(MOTOR_3, MAX_PWM_VALUE, (DataHolder::get()->parameter.params.motor_nonexchange_flag & MOTOR_ENCODER_3_FLAG) == 0); // MOTOR1_REVERS标识电机方向反向,等同于电机线交换
#elif MOTOR_CONTROLLER == AF_SHIELD_CONTROLLER
    static AFSMotorController motor3(MOTOR_3);
#endif

    static EncoderImp encoder3(MOTOR_3, (DataHolder::get()->parameter.params.encoder_nonexchange_flag & MOTOR_ENCODER_3_FLAG) == 0);
    static PID pid3(&input[2], &feedback[2], float(DataHolder::get()->parameter.params.kp) / DataHolder::get()->parameter.params.ko,
                    float(DataHolder::get()->parameter.params.ki) / DataHolder::get()->parameter.params.ko,
                    float(DataHolder::get()->parameter.params.kd) / DataHolder::get()->parameter.params.ko, MAX_PWM_VALUE);
#endif

    // 分别把地址保存在统一的数组内， 方便处理
#if MOTOR_COUNT > 0
    motor[0] = &motor1;
    encoder[0] = &encoder1;
    pid[0] = &pid1;
#endif

#if MOTOR_COUNT > 1
    motor[1] = &motor2;
    encoder[1] = &encoder2;
    pid[1] = &pid2;
#endif

#if MOTOR_COUNT > 2
    motor[2] = &motor3;
    encoder[2] = &encoder3;
    pid[2] = &pid3;
#endif

// 根据定义的模型参数ROBOT_MODEL， 创建模型对象及保存地址
#if ROBOT_MODEL == MODEL_TYPE_2WD_DIFF // 2轮差分
    static Differential diff(DataHolder::get()->parameter.params.wheel_diameter * 0.0005, DataHolder::get()->parameter.params.wheel_track * 0.0005);
    model = &diff;
#endif

#if ROBOT_MODEL == MODEL_TYPE_3WD_OMNI // 三轮全向
    static Omni3 omni3(DataHolder::get()->parameter.params.wheel_diameter * 0.0005, DataHolder::get()->parameter.params.wheel_track * 0.0005);
    model = &omni3;
#endif

    // 分别初始化各个控制器
    for (int i = 0; i < MOTOR_COUNT; i++){
        encoder[i]->init();
        motor[i]->init();
    }

    // 初始化各个变量
    do_kinmatics_flag = false;

    memset(&odom, 0, sizeof(odom));
    memset(&input, 0, sizeof(input));
    memset(&feedback, 0, sizeof(feedback));

    last_velocity_command_time = 0;
}

void Robot::InitTrans()
{
    // 定义一个串口通讯对象   ！！！注意这里USART_transport继承实现Transport接口，
    //  实现了一个通讯对象，这么做的原因是，如果我换为其他通讯方式，例如USB，只需要创建一个USB_Transport继承实现实现Transport接口就可以实现
    static USART_transport _trans(MASTER_USART, 115200);

    // 定义一个通讯协议处理对象， 同上， 我们需要更换协议只需要继承实现Dataframe的接口就可以实现
    static SimpleDataFrame _frame(&_trans);

    // 保存对象指针
    trans = &_trans;
    frame = &_frame;

    // 初始化
    trans->init();
    frame->init();

    // 注册消息处理对象   ！！！ 这里把各个消息id的处理注册了一个观察者， 即再收到相应的消息后会回调update函数
    // Robot(this)继承实现了notify
    frame->RegisterNotify(ID_SET_ROBOT_PARAMTER, this);
    frame->RegisterNotify(ID_CLEAR_ODOM, this);
    frame->RegisterNotify(ID_SET_VELOCITY, this);
    frame->RegisterNotify(ID_GET_ENCODER_COUNT, this);
    frame->RegisterNotify(ID_SET_MOTOR_PWM, this);
}

void Robot::CheckCommand()
{
    static unsigned long lastMillis = 0;
    if (Board::get()->getTickCount() - lastMillis >= 50){
        lastMillis = Board::get()->getTickCount();
		Board::get()->setDOState(_RUN_LED, 2);
    }

    uint8_t ch = 0;

    // 从通讯设备(trans)读取数据交给协议处理模块(frame)处理
    if (trans->read(ch)) {
        if (frame->dataRecv(ch)) {
            // 这里检测到正确的一帧命令，做解析
            frame->dataParse();
        }
    }
}

// 实现Notify接口， 上面注册的命令id接收到时候，会回调，执行响应的动作
void Robot::update(const MESSAGE_ID id, void *data)
{
    switch (id)
    {
    case ID_SET_ROBOT_PARAMTER: // 设置参数的回调， 这里会更新pid的参数， model的参数， 最终保存到flash

        log("RobotParameters: %d %d %d %d %d %d %d %d %d %d %d %d %d",
            DataHolder::get()->parameter.params.wheel_diameter, 
						DataHolder::get()->parameter.params.wheel_track, 
						DataHolder::get()->parameter.params.encoder_resolution, 
						DataHolder::get()->parameter.params.motor_ratio,
            DataHolder::get()->parameter.params.do_pid_interval, 
						DataHolder::get()->parameter.params.kp, 
						DataHolder::get()->parameter.params.ki, 
						DataHolder::get()->parameter.params.kd, 
						DataHolder::get()->parameter.params.ko,
            DataHolder::get()->parameter.params.cmd_last_time, 
						DataHolder::get()->parameter.params.max_v_liner_x, 
						DataHolder::get()->parameter.params.max_v_liner_y, 
						DataHolder::get()->parameter.params.max_v_angular_z);
		
        // 更新pid参数， 这样可以保证设置参数实时生效
        for (int i = 0; i < MOTOR_COUNT; i++)
        {
            pid[i]->Update(float(DataHolder::get()->parameter.params.kp) / DataHolder::get()->parameter.params.ko,
                           float(DataHolder::get()->parameter.params.ki) / DataHolder::get()->parameter.params.ko,
                           float(DataHolder::get()->parameter.params.kd) / DataHolder::get()->parameter.params.ko, MAX_PWM_VALUE);
        }

        // 更新模型参数， 这样可以保证设置参数实时生效
        model->set(DataHolder::get()->parameter.params.wheel_diameter * 0.0005, DataHolder::get()->parameter.params.wheel_track * 0.0005);

        // 保存参数到flash
        DataHolder::get()->save_parameter();
        break;
    case ID_CLEAR_ODOM: 
		// 清除里程计信息
        clear_odom();
        break;
    case ID_SET_VELOCITY: 
		// 更新机器人的期望速度
        updateVelocity(); 
        break;
    case ID_GET_ENCODER_COUNT:
		// 更新encoder count
        update_encoder_count(); 
        break;
    case ID_SET_MOTOR_PWM:
		// 设置pwm值
        update_pwm_value(); 
        break;
    default:
        break;
    }
}

// 清除里程计数据及相关变量
void Robot::clear_odom()
{
    for (int i = 0; i < MOTOR_COUNT; i++){
        encoder[i]->clear();
    }

    memset(&odom, 0, sizeof(odom));
    memset(&DataHolder::get()->odom, 0, sizeof(DataHolder::get()->odom));
}

#define __PI 3.1415926535897932384626433832795

// 根据下发的速度更新期望速度， 并转为pid时间间隔内的期望编码器的变化值
void Robot::updateVelocity()
{
    // 下发速度检测  保证限制在设置的最大最小值内
    short vx = min(max(DataHolder::get()->velocity.v_liner_x, -(short(DataHolder::get()->parameter.params.max_v_liner_x))), short(DataHolder::get()->parameter.params.max_v_liner_x));
    short vy = min(max(DataHolder::get()->velocity.v_liner_y, -(short(DataHolder::get()->parameter.params.max_v_liner_y))), short(DataHolder::get()->parameter.params.max_v_liner_y));
    short vz = min(max(DataHolder::get()->velocity.v_angular_z, -(short(DataHolder::get()->parameter.params.max_v_angular_z))), short(DataHolder::get()->parameter.params.max_v_angular_z));

    // 保存速度转换单位,传入的速度值为cm/s  0.01rad/s  转为m/s 和rad/s
    float vel[3] = {vx / 100.0, vy / 100.0, vz / 100.0};
    float motor_speed[MOTOR_COUNT] = {0};

    model->motionSolver(vel, motor_speed); // 期望的机器人的速度转换为各个电机的速度(通过设置的机器人模型接口)

    // 把得到的期望电机速度 (m/s)转换为期望pid时间间隔内编码器反馈的输入
    // 该值即为PID的输入
    for (int i = 0; i < MOTOR_COUNT; i++){
        input[i] = motor_speed[i] * short(DataHolder::get()->parameter.params.encoder_resolution) * short(DataHolder::get()->parameter.params.motor_ratio) / (2 * __PI) * short(DataHolder::get()->parameter.params.do_pid_interval) * 0.001;
    }

    // log("vx=%d %d motor_speed=%ld %ld", vx, vz, long(motor_speed[0]*1000), long(motor_speed[1]*1000));

    // 保存时间戳
    last_velocity_command_time = Board::get()->getTickCount();

    // 更新变量， 激活pid运算和控制电机
    do_kinmatics_flag = true;
}

void Robot::update_encoder_count()
{
    for (int i = 0; i < MOTOR_COUNT; i++){
	    // 输出累计编码器值 用于调试
        DataHolder::get()->encoder_count[i] = encoder[i]->get_total_count(); 
    }
}

void Robot::update_pwm_value()
{
    do_set_pwm_flag = true;
}

// pid运算， 控制电机
void Robot::DoKinmatics()
{
    if (do_set_pwm_flag){
        do_set_pwm_flag = false;
        for (int i = 0; i < MOTOR_COUNT; i++){
            motor[i]->control(DataHolder::get()->pwm.value[i]);
        }
    }

    if (!do_kinmatics_flag)
    {   // 停止后
        for (int i = 0; i < MOTOR_COUNT; i++)
        {
            // 清除pid
            pid[i]->Clear(); 
            // 实时更新用于pid的encoder值                            
            encoder[i]->get_increment_count_for_dopid(); 
        }
        return; // do_kinmatics_flag false表示电机停止 不需要做pid 故返回
    }

    static unsigned long last_millis = 0;
    // 判断时间间隔做pid运算
    if (Board::get()->getTickCount() - last_millis >= DataHolder::get()->parameter.params.do_pid_interval)
    {
        last_millis = Board::get()->getTickCount();

        // 得到编码器的反馈差值， 即在do_pid_interval这段间隔时间内编码器的差值， 该值作为pid的反馈
        for (int i = 0; i < MOTOR_COUNT; i++)
        {
            feedback[i] = encoder[i]->get_increment_count_for_dopid();
        }
#if PID_DEBUG_OUTPUT
#if MOTOR_COUNT == 2
        log("input=%ld %ld feedback=%ld %ld", long(input[0] * 1000), long(input[1] * 1000),
            long(feedback[0]), long(feedback[1]));
#endif
#if MOTOR_COUNT == 3
        log("input=%ld %ld %ld feedback=%ld %ld %ld", long(input[0] * 1000), long(input[1] * 1000), long(input[2] * 1000),
            long(feedback[0]), long(feedback[1]), long(feedback[2]));
#endif
#if MOTOR_COUNT == 4
        log("input=%ld %ld %ld %ld feedback=%ld %ld %ld %ld", long(input[0] * 1000), long(input[1] * 1000), long(input[2] * 1000), long(input[3] * 1000),
            long(feedback[0]), long(feedback[1]), long(feedback[2]), long(feedback[3]));
#endif
#endif
        bool stoped = true;
        // 当期望和反馈都为0 置位stoped标志
        for (int i = 0; i < MOTOR_COUNT; i++)
        {
            if (input[i] != 0 || feedback[i] != 0)
            {
                stoped = false;
                break;
            }
        }

        short output[MOTOR_COUNT] = {0};
        if (stoped){
            do_kinmatics_flag = false;
        }else{
            // 没有需要停止则做pid运算，得到out值，该值作为电机模块控制器输入
            for (int i = 0; i < MOTOR_COUNT; i++){
                output[i] = pid[i]->Compute(DataHolder::get()->parameter.params.do_pid_interval * 0.001);
            }
        }

        // 当次计算完成 重置变量
        for (int i = 0; i < MOTOR_COUNT; i++){
            DataHolder::get()->pid_data.input[i] = int(input[i]);
            DataHolder::get()->pid_data.output[i] = int(feedback[i]);
        }

#if PID_DEBUG_OUTPUT
#if MOTOR_COUNT == 2
        log("output=%ld %ld", output[0], output[1]);
#endif
#if MOTOR_COUNT == 3
        log("output=%ld %ld %ld", output[0], output[1], output[2]);
#endif
#if MOTOR_COUNT == 4
        log("output=%ld %ld %ld %ld", output[0], output[1], output[2], output[3]);
#endif
#endif

        long elapsed_ms = Board::get()->getTickCount() - last_velocity_command_time;
        // 判断上次下发的时间戳，如果超时，把各个电机期望的输入置零，pid下次会根据改期望计算pid输出转速，慢慢停止点击
        if (elapsed_ms > DataHolder::get()->parameter.params.cmd_last_time){
            memset(input, 0, sizeof(input));
            if (elapsed_ms > DataHolder::get()->parameter.params.cmd_last_time * 2){
                memset(output, 0, sizeof(output));
            }
        }

        // 把pid得到的值交给电机控制器，该值有符号表示正反转
        for (int i = 0; i < MOTOR_COUNT; i++){
            motor[i]->control(output[i]);
        }
    }
}

// 计算里程计
void Robot::CalcOdom()
{
    static unsigned long last_millis = 0;
    // 根据实际间隔计算轮子里程
    if (Board::get()->getTickCount() - last_millis >= CALC_ODOM_INTERVAL)
    {
        last_millis = Board::get()->getTickCount();
#if ODOM_DEBUG_OUTPUT
        long total_count[MOTOR_COUNT] = {0};
        for (int i = 0; i < MOTOR_COUNT; i++){
            total_count[i] = encoder[i]->get_total_count(); // 输出累计编码器值 用于调试
        }

#if MOTOR_COUNT == 2
        log("total_count=[%ld %ld]", total_count[0], total_count[1]);
#endif
#if MOTOR_COUNT == 3
        log("total_count=[%ld %ld %ld]", total_count[0], total_count[1], total_count[2]);
#endif
#if MOTOR_COUNT == 4
        log("total_count=[%ld %ld %ld %ld]", total_count[0], total_count[1], total_count[2], total_count[3]);
#endif
#endif
        float dis[MOTOR_COUNT] = {0};
        for (int i = 0; i < MOTOR_COUNT; i++)
        {
            // 根据CALC_ODOM_INTERVAL的间隔内的各个电机的编码器变化值，转换各个电机实际的里程
            dis[i] = encoder[i]->get_increment_count_for_odom() * __PI * DataHolder::get()->parameter.params.wheel_diameter * 0.001 / DataHolder::get()->parameter.params.encoder_resolution / DataHolder::get()->parameter.params.motor_ratio;
#if ODOM_DEBUG_OUTPUT
            log(" %ld ", long(dis[i] * 1000000));
#endif
        }
        // 把计算得到的各个电机的里程转为为机器人里程(通过机器人运动模型对象)
        model->get_odom(&odom, dis, CALC_ODOM_INTERVAL);

#if ODOM_DEBUG_OUTPUT
        // 输出机器人的里程
        log(" x=%ld y=%ld yaw=%ld", long(odom.x * 1000), long(odom.y * 1000), long(odom.z * 1000)); // mm
        log("");
#endif
				log("x=%ld cm/s y=%ld cm/s yaw=%ld", long(odom.vel_x * 100), long(odom.vel_y * 100), long(odom.z * 1000)); // mm
				
				
				
        // 转换单位保存到DataHolder
        DataHolder::get()->odom.v_liner_x   = odom.vel_x * 100;         // 转为cm/s
        DataHolder::get()->odom.v_liner_y   = odom.vel_y * 100;         // 转为cm/s
        DataHolder::get()->odom.v_angular_z = odom.vel_z * 100;
        DataHolder::get()->odom.x           = odom.x * 100;             // 转为cm
        DataHolder::get()->odom.y           = odom.y * 100;
        DataHolder::get()->odom.yaw         = long(odom.z * 100) % 628; // 转为0.01rad  628为2pi*100
    }
}

// 获取imu数据
void Robot::GetImuData()
{
#if IMU_ENABLE
    if (imu == NULL){
        return;
    }

    static unsigned long last_millis = 0;
    // 按照设置的时间间隔获取imu数据
    if (Board::get()->getTickCount() - last_millis >= CALC_IMU_INTERVAL){
        last_millis = Board::get()->getTickCount();
        imu->GetData(DataHolder::get()->imu_data);
    }
#endif
}

// 检测joystick
void Robot::CheckJoystick()
{
#if JOYSTICK_ENABLE
    if (joystick == NULL) {
        return;
    }

    static unsigned long last_millis = 0;
    short liner_x = 0, liner_y = 0, angular_z = 0;
    if (Board::get()->getTickCount()-last_millis>=CHECK_JOYSTICK_INTERVAL){
        last_millis = Board::get()->getTickCount();
        // 按照设置的间隔间隔 更新期望速度
        if (joystick->check(liner_x, liner_y, angular_z)) {
            DataHolder::get()->velocity.v_liner_x = liner_x;
            DataHolder::get()->velocity.v_liner_y = liner_y;
            DataHolder::get()->velocity.v_angular_z = angular_z;
            update_velocity();
        }

        joystick->test();
    }
#endif
}