/*
 * HipPosition.cpp
 *
 *  Created on: 20 февр. 2020 г.
 *      Author: user
 */

#include "HipPosition.hh"

//float communication::hiperface::HipPositionClient::get() const {
//
//    if( not request) position_request.message_control.ans_ready_flag = false, request = hip_driver.send( &position_request.message_control );
//    else {
//        if( position_request.message_control.ans_ready_flag ) {
//            request = position_request.message_control.ans_ready_flag = false;
//
//            reinterpret_cast<unsigned char*>( &position_code )[0] = (position_request.rec_data[2] << 8) | position_request.rec_data[3];
//            reinterpret_cast<unsigned char*>( &position_code )[1] = (position_request.rec_data[0] << 8) | position_request.rec_data[1];
//
//            return position_code * _1_to_rad;
//        }
//    }
//
//    return NAN;
//
//}

communication::hiperface::HipPositionClient::HipPositionClient(
        driver::HiperfaceNetworkDriver & hip_driver, unsigned short address )
: hip_driver(hip_driver) {

    position_request.message_control.address = address;
    set_position_request.message_control.address = address;

}

void communication::hiperface::HipPositionClient::set( unsigned long position ) {

    set_position_request.send_data[0] = reinterpret_cast<unsigned char*>( &position )[1] >> 8;
    set_position_request.send_data[1] = reinterpret_cast<unsigned char*>( &position )[1];
    set_position_request.send_data[2] = reinterpret_cast<unsigned char*>( &position )[0] >> 8;
    set_position_request.send_data[3] = reinterpret_cast<unsigned char*>( &position )[0];

    if( not set_request) set_position_request.message_control.ans_ready_flag = false, set_request = hip_driver.send( &set_position_request.message_control );
    else {
        if( set_position_request.message_control.ans_ready_flag ) {
            set_request = position_request.message_control.ans_ready_flag = false;

            //новая позиция установлена успешно!
        } else /*произошла ошибка*/;
    }

}

unsigned long communication::hiperface::HipPositionClient::getPositionCode() const {

    unsigned long position_code;

    reinterpret_cast<unsigned char*>( &position_code )[0] = (position_request.rec_data[2] << 8) | position_request.rec_data[3];
    reinterpret_cast<unsigned char*>( &position_code )[1] = (position_request.rec_data[0] << 8) | position_request.rec_data[1];

    return position_code;

}