#include "robot.h"

#include "board.h"
#include "usart_transport.h"
#include "simple_dataframe_slave.h"
#include "data_holder.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_debug_init();
#endif

    // 加载参数， 该参数一般存储在flash或者eeprom
    Data_holder::get()->load_parameter();

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

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

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

    pb_printf("init_trans");
    // 通讯相关初始化
    init_trans();
    
    // joystick初始化
    init_joystick();

    pb_printf("pibot startup");
}

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

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

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

void Robot::init_motor()
{
    //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, (Data_holder::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, (Data_holder::get()->parameter.params.encoder_nonexchange_flag & MOTOR_ENCODER_1_FLAG)==0);

    // 定义PID控制对象 传入参数为输入和输出的地址及kp ki kd 这里ko为比例
    static PID pid1(&input[0], &feedback[0], float(Data_holder::get()->parameter.params.kp)/Data_holder::get()->parameter.params.ko, 
                                            float(Data_holder::get()->parameter.params.ki)/Data_holder::get()->parameter.params.ko, 
                                            float(Data_holder::get()->parameter.params.kd)/Data_holder::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, (Data_holder::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, (Data_holder::get()->parameter.params.encoder_nonexchange_flag & MOTOR_ENCODER_2_FLAG)==0);
    static PID pid2(&input[1], &feedback[1], float(Data_holder::get()->parameter.params.kp)/Data_holder::get()->parameter.params.ko, 
                                            float(Data_holder::get()->parameter.params.ki)/Data_holder::get()->parameter.params.ko, 
                                            float(Data_holder::get()->parameter.params.kd)/Data_holder::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, (Data_holder::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, (Data_holder::get()->parameter.params.encoder_nonexchange_flag & MOTOR_ENCODER_3_FLAG)==0);
    static PID pid3(&input[2], &feedback[2], float(Data_holder::get()->parameter.params.kp)/Data_holder::get()->parameter.params.ko, 
                                            float(Data_holder::get()->parameter.params.ki)/Data_holder::get()->parameter.params.ko, 
                                            float(Data_holder::get()->parameter.params.kd)/Data_holder::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(Data_holder::get()->parameter.params.wheel_diameter*0.0005, Data_holder::get()->parameter.params.wheel_track*0.0005);
    model = &diff;
#endif

#if ROBOT_MODEL == MODEL_TYPE_3WD_OMNI     //三轮全向
    static Omni3 omni3(Data_holder::get()->parameter.params.wheel_diameter*0.0005, Data_holder::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::init_trans()
{
    //定义一个串口通讯对象   ！！！注意这里USART_transport继承实现Transport接口， 
    // 实现了一个通讯对象，这么做的原因是，如果我换为其他通讯方式，例如USB，只需要创建一个USB_Transport继承实现实现Transport接口就可以实现
    static USART_transport _trans(MASTER_USART, 115200);

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

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

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

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

void Robot::check_command()
{
    static unsigned long last_millis=0;
    if (Board::get()->get_tick_count()-last_millis>=50){
        last_millis = Board::get()->get_tick_count();
				
		Board::get()->setDOState(_RUN_LED, 2);
    }

    unsigned char ch=0;

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

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

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

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

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

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

#define __PI  3.1415926535897932384626433832795

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

    float vel[3]={vx/100.0, vy/100.0, vz/100.0}; // 保存速度转换单位  传入的速度值为cm/s  0.01rad/s  转为m/s 和rad/s
    float motor_speed[MOTOR_COUNT]={0};
    model->motion_solver(vel, motor_speed);  // 期望的机器人的速度转换为各个电机的速度(通过设置的机器人模型接口)

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

    //pb_printf("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()->get_tick_count();
    
    // 更新变量， 激活pid运算和控制电机
    do_kinmatics_flag = true;
}

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

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

// pid运算， 控制电机
void Robot::do_kinmatics()
{
    if (do_set_pwm_flag) {
        do_set_pwm_flag = false;

        for (int i=0; i<MOTOR_COUNT; i++) {
            motor[i]->control(Data_holder::get()->pwm.value[i]);
        }
    }

    if (!do_kinmatics_flag) {   // 停止后
        for(int i=0;i<MOTOR_COUNT;i++) {
            pid[i]->clear();    // 清除pid
            encoder[i]->get_increment_count_for_dopid();// 实时更新用于pid的encoder值
        }
        return;  //do_kinmatics_flag false表示电机停止 不需要做pid 故返回
    }
    
    static unsigned long last_millis=0;
    // 判断时间间隔做pid运算
    if (Board::get()->get_tick_count()-last_millis>=Data_holder::get()->parameter.params.do_pid_interval) {
        last_millis = Board::get()->get_tick_count();
        
        //得到编码器的反馈差值， 即在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
        pb_printf("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
        pb_printf("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
        pb_printf("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(Data_holder::get()->parameter.params.do_pid_interval*0.001);
            }
        }

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

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

        long elapsed_ms = Board::get()->get_tick_count()-last_velocity_command_time;
        //判断上次下发的时间戳，如果超时，把各个电机期望的输入置零，pid下次会根据改期望计算pid输出转速，慢慢停止点击
        if (elapsed_ms > Data_holder::get()->parameter.params.cmd_last_time) {
            memset(input, 0, sizeof(input));
            if (elapsed_ms > Data_holder::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::calc_odom()
{
    static unsigned long last_millis=0;
    //根据实际间隔计算轮子里程
    if (Board::get()->get_tick_count()-last_millis>=CALC_ODOM_INTERVAL) {
        last_millis = Board::get()->get_tick_count();
#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
        pb_printf("total_count=[%ld %ld]", total_count[0], total_count[1]);
    #endif
    #if MOTOR_COUNT==3
        pb_printf("total_count=[%ld %ld %ld]", total_count[0], total_count[1], total_count[2]);
    #endif
    #if MOTOR_COUNT==4
        pb_printf("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*Data_holder::get()->parameter.params.wheel_diameter*0.001/Data_holder::get()->parameter.params.encoder_resolution/Data_holder::get()->parameter.params.motor_ratio;
#if ODOM_DEBUG_OUTPUT
            pb_printf(" %ld ", long(dis[i]*1000000));
#endif
        }
        
        // 把计算得到的各个电机的里程转为为机器人里程(通过机器人运动模型对象)
        model->get_odom(&odom, dis, CALC_ODOM_INTERVAL);

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

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

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

//检测joystick
void Robot::check_joystick()
{
#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()->get_tick_count()-last_millis>=CHECK_JOYSTICK_INTERVAL){
        last_millis = Board::get()->get_tick_count();
        // 按照设置的间隔间隔 更新期望速度
        if (joystick->check(liner_x, liner_y, angular_z)) {
            Data_holder::get()->velocity.v_liner_x = liner_x;
            Data_holder::get()->velocity.v_liner_y = liner_y;
            Data_holder::get()->velocity.v_angular_z = angular_z;
            update_velocity();
        }

        joystick->test();
    }
#endif
}