#include "data_holder.h"
#include "board.h"
#include <stdio.h>

Data_holder::Data_holder()
{
    strncpy((char*)&firmware_info.version, FW_VERSION, strlen(FW_VERSION)>sizeof(firmware_info.version)?sizeof(firmware_info.version):strlen(FW_VERSION));
    sprintf(firmware_info.time, "%s", "20220301");
    memset(&parameter, 0, sizeof(struct Robot_parameter));
    memset(&velocity, 0, sizeof(struct Robot_velocity));
    memset(&odom, 0, sizeof(struct Robot_odom));
    memset(&pid_data, 0, sizeof(struct Robot_pid_data));
    memset(imu_data, 0, sizeof(imu_data));
}

void Data_holder::load_parameter()
{
    Board::get()->get_config((unsigned char*)&parameter, sizeof(parameter));

    if (parameter.params.ko == 0)
            parameter.params.ko = 1;
    
    if (parameter.params.do_pid_interval == 0)
            parameter.params.do_pid_interval = 10;

    if (parameter.params.motor_ratio == (unsigned short)-1 || parameter.params.motor_ratio == 0) {
        parameter.params.motor_ratio = 1;
    }

    parameter.params.model_type = MODEL_TYPE_2WD_DIFF; // in stm32f1 fix 2wd
}

void Data_holder::save_parameter()
{
    Board::get()->set_config((unsigned char*)&parameter, sizeof(parameter));
    parameter.params.model_type = MODEL_TYPE_2WD_DIFF; // in stm32f1 fix 2wd
}