// LegacyESCPackets.c was generated by ProtoGen version 3.2.a

/*
 * This file is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This file is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Author: Oliver Walters
 */

#include "LegacyESCPackets.h"
#include "fielddecode.h"
#include "fieldencode.h"
#include "scaleddecode.h"
#include "scaledencode.h"

/*!
 * \brief Create the ESC_LegacyStatusA packet
 *
 * Legacy (gen-1) definition for the STATUS_A packet
 * \param _pg_pkt points to the packet which will be created by this function
 * \param mode is ESC operating mode. The lower four bits indicate the operational mode of the ESC, in accordance with the ESCOperatingModes enumeration. The upper 3 bits are used for debugging and should be ignored for general use.
 * \param status is ESC status bits
 * \param warnings is ESC warning bits
 * \param errors is ESC *error* bits
 * \param command is ESC operational command - value depends on 'mode' available in this packet. If the ESC is disabled, data reads 0x0000. If the ESC is in open-loop PWM mode, this value is the PWM command in units of 1us, in the range 1000us to 2000us. If the ESC is in closed-loop RPM mode, this value is the RPM command in units of 1RPM
 * \param rpm is Motor speed
 */
void encodeESC_LegacyStatusAPacket(void* _pg_pkt, uint8_t mode, const ESC_LegacyStatusBits_t* status, const ESC_LegacyWarningBits_t* warnings, const ESC_LegacyErrorBits_t* errors, uint16_t command, uint16_t rpm)
{
    uint8_t* _pg_data = getESCVelocityPacketData(_pg_pkt);
    int _pg_byteindex = 0;

    // Set to 0 to indicate a Gen-2 ESC
    _pg_data[_pg_byteindex] = 0;

    // ESC operating mode. The lower four bits indicate the operational mode of the ESC, in accordance with the ESCOperatingModes enumeration. The upper 3 bits are used for debugging and should be ignored for general use.
    // Range of mode is 0 to 127.
    _pg_data[_pg_byteindex] |= (uint8_t)limitMax(mode, 127);
    _pg_byteindex += 1; // close bit field

    // ESC status bits
    encodeESC_LegacyStatusBits_t(_pg_data, &_pg_byteindex, status);

    // ESC warning bits
    encodeESC_LegacyWarningBits_t(_pg_data, &_pg_byteindex, warnings);

    // ESC *error* bits
    encodeESC_LegacyErrorBits_t(_pg_data, &_pg_byteindex, errors);

    // ESC operational command - value depends on 'mode' available in this packet. If the ESC is disabled, data reads 0x0000. If the ESC is in open-loop PWM mode, this value is the PWM command in units of 1us, in the range 1000us to 2000us. If the ESC is in closed-loop RPM mode, this value is the RPM command in units of 1RPM
    // Range of command is 0 to 65535.
    uint16ToBeBytes(command, _pg_data, &_pg_byteindex);

    // Motor speed
    // Range of rpm is 0 to 65535.
    uint16ToBeBytes(rpm, _pg_data, &_pg_byteindex);

    // complete the process of creating the packet
    finishESCVelocityPacket(_pg_pkt, _pg_byteindex, getESC_LegacyStatusAPacketID());

}// encodeESC_LegacyStatusAPacket

/*!
 * \brief Decode the ESC_LegacyStatusA packet
 *
 * Legacy (gen-1) definition for the STATUS_A packet
 * \param _pg_pkt points to the packet being decoded by this function
 * \param mode receives ESC operating mode. The lower four bits indicate the operational mode of the ESC, in accordance with the ESCOperatingModes enumeration. The upper 3 bits are used for debugging and should be ignored for general use.
 * \param status receives ESC status bits
 * \param warnings receives ESC warning bits
 * \param errors receives ESC *error* bits
 * \param command receives ESC operational command - value depends on 'mode' available in this packet. If the ESC is disabled, data reads 0x0000. If the ESC is in open-loop PWM mode, this value is the PWM command in units of 1us, in the range 1000us to 2000us. If the ESC is in closed-loop RPM mode, this value is the RPM command in units of 1RPM
 * \param rpm receives Motor speed
 * \return 0 is returned if the packet ID or size is wrong, else 1
 */
int decodeESC_LegacyStatusAPacket(const void* _pg_pkt, uint8_t* mode, ESC_LegacyStatusBits_t* status, ESC_LegacyWarningBits_t* warnings, ESC_LegacyErrorBits_t* errors, uint16_t* command, uint16_t* rpm)
{
    unsigned int _pg_tempbitfield = 0;
    int _pg_byteindex = 0;
    const uint8_t* _pg_data = getESCVelocityPacketDataConst(_pg_pkt);
    int _pg_numbytes = getESCVelocityPacketSize(_pg_pkt);

    // Verify the packet identifier
    if(getESCVelocityPacketID(_pg_pkt) != getESC_LegacyStatusAPacketID())
        return 0;

    if(_pg_numbytes < getESC_LegacyStatusAMinDataLength())
        return 0;

    // Set to 0 to indicate a Gen-2 ESC
    _pg_tempbitfield = (_pg_data[_pg_byteindex] >> 7);
    // Decoded value must be 0
    if(_pg_tempbitfield != 0)
        return 0;

    // ESC operating mode. The lower four bits indicate the operational mode of the ESC, in accordance with the ESCOperatingModes enumeration. The upper 3 bits are used for debugging and should be ignored for general use.
    // Range of mode is 0 to 127.
    (*mode) = ((_pg_data[_pg_byteindex]) & 0x7F);
    _pg_byteindex += 1; // close bit field

    // ESC status bits
    if(decodeESC_LegacyStatusBits_t(_pg_data, &_pg_byteindex, status) == 0)
        return 0;

    // ESC warning bits
    if(decodeESC_LegacyWarningBits_t(_pg_data, &_pg_byteindex, warnings) == 0)
        return 0;

    // ESC *error* bits
    if(decodeESC_LegacyErrorBits_t(_pg_data, &_pg_byteindex, errors) == 0)
        return 0;

    // ESC operational command - value depends on 'mode' available in this packet. If the ESC is disabled, data reads 0x0000. If the ESC is in open-loop PWM mode, this value is the PWM command in units of 1us, in the range 1000us to 2000us. If the ESC is in closed-loop RPM mode, this value is the RPM command in units of 1RPM
    // Range of command is 0 to 65535.
    (*command) = uint16FromBeBytes(_pg_data, &_pg_byteindex);

    // Motor speed
    // Range of rpm is 0 to 65535.
    (*rpm) = uint16FromBeBytes(_pg_data, &_pg_byteindex);

    return 1;

}// decodeESC_LegacyStatusAPacket
// end of LegacyESCPackets.c