//
// Created by zx on 2020/4/28.
//

#include "Odom.h"
using namespace agv;

// 无参构造
Odom::Odom()
{
    std::cout<<"odom 构造"<<std::endl;
    mp_cifx_handle = Cifx_communicator::get_instance();
    mb_initialized = false;
    m_odom_callback = nullptr;
    mp_odom_callback_owner = nullptr;
}

// 析构
Odom::~Odom()
{
    std::cout<<"odom 析构"<<std::endl;
    if(mp_cifx_handle != nullptr){
        Cifx_communicator::release_instance();
    }
    if(m_odom_callback!=nullptr)
    {
        m_odom_callback = nullptr;
    }
    if(mp_odom_callback_owner != nullptr)
    {
        mp_odom_callback_owner = nullptr;
    }
}

// 初始化函数
Error_manager Odom::init()
{
    if (mp_cifx_handle == nullptr)
        return POINTER_IS_NULL;
    // 设置cifx读消息回调
    Error_manager ec = mp_cifx_handle->set_read_data_callback(Odom::get_cifx_data_callback, this);
    if(ec != SUCCESS)
        return ec;
    // cifx通信卡初始化
    ec = mp_cifx_handle->cifx_init();
    if(ec != SUCCESS)
        return ec;
    mb_initialized = true;
    return SUCCESS;
}

// 获取初始化状态
bool Odom::is_initialized()
{
    return mb_initialized;
}

// 设置通信卡读数据回调
Error_manager Odom::set_driver_data_callback(ODOM_CALLBACK func, void *pOwner)
{
    m_odom_callback = func;
    mp_odom_callback_owner = pOwner;
    return SUCCESS;
}

// 向通信卡写入电机驱动器数据
Error_manager Odom::write_driver_data(Driver_data &data)
{
    if(mp_cifx_handle == nullptr)
        return POINTER_IS_NULL;
    char t_write_data[WHEEL_OUTPUT_AREA_SIZE];
    memset(t_write_data, 0, WHEEL_OUTPUT_AREA_SIZE);
    for (size_t i = 0; i < WHEEL_OUTPUT_AREA_SIZE; i+=2)
    {
        int16_t t_wheel_data = 0;
        if((i / 2) < MOTOR_NUM){
            t_wheel_data = static_cast<int16_t>(data.front_motor_data[i/2] * 1000.0);
        }else{
            t_wheel_data = static_cast<int16_t>(data.rear_motor_data[i/2-2] * 1000.0);
        }
        // 字节序倒序
        // t_wheel_data = (t_wheel_data << 8 || t_wheel_data >> 8);
        memcpy(t_write_data+i, &t_wheel_data, sizeof(int16_t));
    }
    return mp_cifx_handle->cifx_write(WHEEL_OUTPUT_AREA, 0, WHEEL_OUTPUT_AREA_SIZE, t_write_data);
}



// 向通信卡读数据回调
void Odom::get_cifx_data_callback(Cifx_communicator::AREA_DATA_BLOCK &read_data, void *pOwner)
{
    // std::cout<<"odom data callback"<<std::endl;
    if(pOwner == nullptr)
        return;
    Odom* odom = reinterpret_cast<Odom*>(pOwner);
    if(odom->m_odom_callback != nullptr && odom->mp_odom_callback_owner != nullptr){
        Driver_data t_driver_data;
        for (size_t i = 0; i < WHEEL_OUTPUT_AREA_SIZE; i+=2)
        {
            int16_t t_wheel_data = *(reinterpret_cast<uint16_t*>(read_data.data+i));
            if((i / 2) < MOTOR_NUM){
                t_driver_data.front_motor_data[i/2] = t_wheel_data / 1000.0;
            }else{
                t_driver_data.rear_motor_data[i/2 - 2] = t_wheel_data / 1000.0;
            }
        }
        odom->m_odom_callback(t_driver_data, odom->mp_odom_callback_owner);
    }
}