"""
Adaptation layer for the Yard Stick One.
"""
import logging
from struct import unpack, pack
from time import time

from usb.core import find, USBError, USBTimeoutError
from usb.util import get_string

from whad.exceptions import WhadDeviceNotFound, WhadDeviceNotReady, WhadDeviceAccessDenied
from whad.device import VirtualDevice
from whad.device.virtual.yard.constants import YardStickOneId, YardStickOneEndPoints, \
    YardApplications, YardSystemCommands, YardRadioStructure, YardRFStates, \
    YardMemoryRegisters, YardMARCStates, YardCCA, YardFrequencyTransitionPoints, \
    YardNICCommands, YardVCOType, YardRegistersMasks, YardModulations, YardEncodings, \
    POSSIBLE_CHANNEL_BANDWIDTHS, NUM_PREAMBLE_LOOKUP_TABLE, YardUSBProperties, \
    YardInternalStates
from whad.hub.discovery import Domain, Capability
from whad.hub.generic.cmdresult import CommandResult
from whad.hub.phy import Commands, TxPower, Endianness as PhyEndianness, Modulation as PhyModulation
from whad.phy import Endianness
from whad.helpers import swap_bits

logger = logging.getLogger(__name__)

# Helpers functions
def get_yardstickone(index=0,bus=None, address=None):
    '''
    Returns a YardStickOne USB object based on index or bus and address.
    '''
    devices = list(find(idVendor=YardStickOneId.YARD_ID_VENDOR,
                        idProduct=YardStickOneId.YARD_ID_PRODUCT,
                        find_all=True))
    if bus is not None and address is not None:
        for device in devices:
            if device.bus == bus and device.address == address:
                return (devices.index(device), device)
        # No device found with the corresponding bus/address, return None
        return None

    try:
        return (index, devices[index])
    except IndexError:
        return None

class YardStickOneDevice(VirtualDevice):
    """Yard Stick One virtual device.
    """

    INTERFACE_NAME = "yardstickone"

    @classmethod
    def list(cls):
        '''
        Returns a list of available RZUSBStick devices.
        '''
        available_devices = []
        try:
            for yard in find(idVendor=YardStickOneId.YARD_ID_VENDOR,
                             idProduct=YardStickOneId.YARD_ID_PRODUCT,
                             find_all=True):
                available_devices.append(YardStickOneDevice(bus=yard.bus, address=yard.address))
        except ValueError:
            logger.warning("Cannot access YardStickOne, root privileges may be required.")

        return available_devices

    @property
    def identifier(self):
        '''
        Returns the identifier of the current device (e.g., bus + address in
        format "<bus>-<address>").
        '''
        return str(self.__yard.bus)+"-"+str(self.__yard.address)


    def __init__(self, index=0, bus=None, address=None):
        """
        Create device connection
        """
        device = get_yardstickone(index,bus=bus,address=address)
        if device is None:
            raise WhadDeviceNotFound
        self.__opened_stream = False

        self.__supported_frequency_range = [
            (281000000, 361000000),
            (378000000, 481000000),
            (749000000, 962000000),
        ]

        self.radio_structure = None
        self._frequency_offset_accumulator = None
        self.__internal_state = YardInternalStates.YARD_MODE_IDLE
        self.__frequency = None
        self.__endianness = Endianness.BIG
        self._rf_mode = 0

        self.__start_time = time()*1000000

        self.__opened = False
        _, self.__yard = device
        super().__init__()

    @property
    def frequency(self) -> int:
        """Get current frequency.

        :return: Current frequency.
        :rtype: int
        """
        return self.__frequency

    def _enter_configuration_mode(self):
        self._set_idle_mode()
        self._strobe_idle_mode()

    def _restore_previous_mode(self):
        if self.__internal_state == YardInternalStates.YARD_MODE_RX:
            self._set_rx_mode()
            self._strobe_rx_mode()

        elif self.__internal_state == YardInternalStates.YARD_MODE_TX:
            self._set_tx_mode()
            self._strobe_tx_mode()

    def _on_whad_phy_get_supported_freq(self, message):
        # Create a SupportedFreqRanges
        msg = self.hub.phy.create_supported_freq_ranges(
            self.__supported_frequency_range
        )

        # Send message
        self._send_whad_message(msg)

    def _on_whad_phy_send(self, message):
        self._send_packet(message.packet)
        self._send_whad_command_result(CommandResult.SUCCESS)


    def _on_whad_phy_set_freq(self, message):
        found = False
        for supported_range in self.__supported_frequency_range:
            range_start, range_end = supported_range[0], supported_range[1]
            if message.frequency >= range_start and message.frequency <= range_end:
                found = True
                break

        if found:
            self._enter_configuration_mode()
            self._set_frequency(message.frequency)
            self.__frequency = message.frequency
            self._restore_previous_mode()
            self._send_whad_command_result(CommandResult.SUCCESS)
        else:
            self._send_whad_command_result(CommandResult.PARAMETER_ERROR)

    def _on_whad_phy_datarate(self, message):
        self._enter_configuration_mode()
        self._set_data_rate(message.rate)
        self._restore_previous_mode()
        self._send_whad_command_result(CommandResult.SUCCESS)

    def _on_whad_phy_packet_size(self, message):
        if message.packet_size < 255:
            self._enter_configuration_mode()
            self._set_packet_length(message.packet_size)
            self._restore_previous_mode()
            self._send_whad_command_result(CommandResult.SUCCESS)
        else:
            self._send_whad_command_result(CommandResult.PARAMETER_ERROR)

    def _on_whad_phy_endianness(self, message):
        if message.endianness in (Endianness.BIG, Endianness.LITTLE):
            self.__endianness = message.endianness
            self._send_whad_command_result(CommandResult.SUCCESS)
        else:
            self._send_whad_command_result(CommandResult.PARAMETER_ERROR)

    def _on_whad_phy_tx_power(self, message):
        tx_powers = {
            TxPower.LOW : 0x20,
            TxPower.MEDIUM : 0x80,
            TxPower.HIGH : 0xC0
        }

        if message.power in tx_powers:
            self._enter_configuration_mode()
            self._set_power(
                tx_powers[message.power]
            )
            self._restore_previous_mode()
            self._send_whad_command_result(CommandResult.SUCCESS)
        else:
            self._send_whad_command_result(CommandResult.PARAMETER_ERROR)

    def _on_whad_phy_mod_ask(self, message):
        self._enter_configuration_mode()
        self._set_modulation(YardModulations.MODULATION_ASK)
        self._set_encoding(YardEncodings.NON_RETURN_TO_ZERO)
        self._restore_previous_mode()
        self._send_whad_command_result(CommandResult.SUCCESS)

    def _on_whad_phy_mod_4fsk(self, message):
        self._enter_configuration_mode()
        self._set_modulation(YardModulations.MODULATION_4FSK)
        self._set_encoding(YardEncodings.NON_RETURN_TO_ZERO)
        self._set_deviation(message.deviation)
        self._restore_previous_mode()
        self._send_whad_command_result(CommandResult.SUCCESS)


    def _on_whad_phy_mod_fsk(self, message):
        self._enter_configuration_mode()
        self._set_modulation(YardModulations.MODULATION_2FSK)
        self._set_encoding(YardEncodings.NON_RETURN_TO_ZERO)
        self._set_deviation(message.deviation)
        self._restore_previous_mode()
        self._send_whad_command_result(CommandResult.SUCCESS)

    def _on_whad_phy_mod_gfsk(self, message):
        self._enter_configuration_mode()
        self._set_modulation(YardModulations.MODULATION_GFSK)
        self._set_encoding(YardEncodings.NON_RETURN_TO_ZERO)
        self._set_deviation(message.deviation)
        # message.gaussian_filter can't be processed
        self._restore_previous_mode()
        self._send_whad_command_result(CommandResult.SUCCESS)

    def _on_whad_phy_sync_word(self, message):
        if len(message.sync_word) == 0:
            self._enter_configuration_mode()
            self._set_crc(enable=False)
            self._set_whitening(enable=False)
            self._set_packet_format(0)
            self._set_forward_error_correction(enable=False)
            self._set_clear_channel_assessment(mode=YardCCA.NO_CCA)
            self._set_sync_word(b"")
            self._set_preamble_quality_threshold(0)
            self._restore_previous_mode()
            self._send_whad_command_result(CommandResult.SUCCESS)
        elif len(message.sync_word) <= 2:
            self._enter_configuration_mode()
            self._set_crc(enable=False)
            self._set_whitening(enable=False)
            self._set_packet_format(0)
            self._set_forward_error_correction(enable=False)
            self._set_clear_channel_assessment(mode=YardCCA.NO_CCA)
            if self.__endianness == Endianness.LITTLE:
                sync = bytes([swap_bits(i) for i in message.sync_word])[::-1]
            else:
                sync = message.sync_word
            self._set_sync_word(sync)
            self._set_preamble_quality_threshold(0)
            self._restore_previous_mode()
            self._send_whad_command_result(CommandResult.SUCCESS)
        elif len(message.sync_word) == 4 and message.sync_word[:2] == message.sync_word[2:]:
            self._enter_configuration_mode()
            self._set_crc(enable=False)
            self._set_whitening(enable=False)
            self._set_packet_format(0)
            self._set_forward_error_correction(enable=False)
            self._set_clear_channel_assessment(mode=YardCCA.NO_CCA)
            if self.__endianness == Endianness.LITTLE:
                sync = bytes([swap_bits(i) for i in message.sync_word])[::-1]
            else:
                sync = message.sync_word
            self._set_sync_word(sync)
            self._set_preamble_quality_threshold(0)
            self._restore_previous_mode()
            self._send_whad_command_result(CommandResult.SUCCESS)

        else:
            self._send_whad_command_result(CommandResult.PARAMETER_ERROR)


    def _on_whad_phy_sniff(self, message):
        self.__internal_state = YardInternalStates.YARD_MODE_RX
        self._send_whad_command_result(CommandResult.SUCCESS)

    def _on_whad_phy_start(self, message):
        if self.__internal_state == YardInternalStates.YARD_MODE_RX:
            self._set_rx_mode()
            self._strobe_rx_mode()
            self.__opened_stream = True
        self._send_whad_command_result(CommandResult.SUCCESS)

    def _on_whad_phy_stop(self, message):
        #self._set_idle_mode()
        #self._strobe_idle_mode()
        self.__opened_stream = False
        self._send_whad_command_result(CommandResult.SUCCESS)


    def _send_whad_phy_pdu(self, packet, timestamp=None):
        # Create a PacketReceived message
        msg = self.hub.phy.create_packet_received(
            self._get_frequency(),
            packet,
            syncword=self._get_sync_word(),
            endianness=PhyEndianness.LITTLE if self.__endianness == Endianness.LITTLE \
                else PhyEndianness.BIG,
            deviation=int(self._get_deviation()),
            datarate=int(self._get_data_rate())
        )

        # Set packet timestamp if available
        if timestamp is not None:
            msg.timestamp = timestamp

        self._send_whad_message(msg)


    def open(self):
        try:
            self.__yard.set_configuration()
        except USBError as err:
            if err.errno == 13:
                raise WhadDeviceAccessDenied("yardstickone") from err
            raise WhadDeviceNotReady() from err

        self.reset()
        self._dev_id = self._get_serial_number()
        self._fw_author = self._get_manufacturer()
        self._fw_url = self._get_url()
        self._fw_version = self._get_firmware_version()
        self._dev_capabilities = self._get_capabilities()

        self.__opened = True

        self.radio_structure = YardRadioStructure(self._poke, self._peek)

        self._frequency_offset_accumulator = 0
        #print(self.radio_structure)

        self._set_idle_mode()
        self._set_rf_register("MDMCFG4",0x68)
        self._set_rf_register("MDMCFG3",0xb5)
        self._set_rf_register("MDMCFG2",0x80)
        self._set_rf_register("MDMCFG1",0x23)
        self._set_rf_register("MDMCFG0",0x11)
        self._set_modulation(YardModulations.MODULATION_ASK)
        self._set_encoding(YardEncodings.NON_RETURN_TO_ZERO)
        self._set_crc(enable=False)
        self._set_whitening(enable=False)
        self._set_packet_format(0)
        self._set_forward_error_correction(enable=False)
        self._set_clear_channel_assessment(mode=YardCCA.NO_CCA)
        self._set_frequency(433870000)
        self._set_data_rate(10000)
        #self._set_channel_spacing(self.compute_best_channel_bandwidth())
        self._set_channel(0)
        self._set_intermediate_frequency(44444)
        self._set_idle_mode()

        # Ask parent class to run a background I/O thread
        super().open()
        self.__opened_stream = False

    def write(self, data):
        if not self.__opened:
            raise WhadDeviceNotReady()

    def read(self):
        """Read data from Yard Stick One
        """
        if not self.__opened:
            raise WhadDeviceNotReady()

        if self.__opened_stream:
            try:
                self._yard_send_command(YardApplications.NIC, YardNICCommands.SET_RECV_LARGE,
                                        pack("<H", 200))
                data = self._yard_send_command(YardApplications.NIC,YardNICCommands.RECV)
                while self.__opened_stream and self.__internal_state == YardInternalStates.YARD_MODE_RX:

                    data = self._yard_read_response()

                    if data[0] is not None and data[0] != 0xFF and not (len(data[2]) == 1 \
                            and data[2] == b"\xC8"):
                        if self.__endianness == Endianness.LITTLE:
                            formatted_data = bytes([swap_bits(i) for i in data[2]])
                        else:
                            formatted_data = data[2]

                        self._send_whad_phy_pdu(formatted_data, int(
                            time()*1000000 - self.__start_time
                            ))

            except USBTimeoutError:
                pass

    def reset(self):
        self._yard_send_command(
            YardApplications.SYSTEM,
            YardSystemCommands.RESET,
            b"RESET_NOW\x00"
        )

    def close(self):
        if self.__opened:
            self.__opened_stream = False
            self._set_idle_mode()
        super().close()

    # Yard Stick One low level communication primitives

    def _yard_read_response(self, timeout=1000):
        """Read response sent by the yard stick one.
        """
        try:
            response = bytes(self.__yard.read(YardStickOneEndPoints.IN_ENDPOINT,
                                              512, timeout=timeout))
            if len(response) >= 3:
                size = unpack("<H", response[3:5])[0]
                if len(response) >= 5 + size:
                    app = response[1]
                    verb = response[2]
                    data = response[5:5+size]
                    return app, verb, data
        except USBTimeoutError:
            response = (None, None, None)
        return response

    def _yard_send_command(self, app, command, data=b"", timeout=1000, no_response=False):
        message = bytes([app, command]) + pack("<H", len(data)) + data
        if no_response:
            return None
        recv_app, recv_verb, recv_data = None, None, None
        while (recv_app != app and recv_verb != command):
            self.__yard.write(YardStickOneEndPoints.OUT_ENDPOINT, message, timeout=timeout)
            recv_app, recv_verb, recv_data = self._yard_read_response()
        return recv_data

    # Discovery related functions
    def _get_capabilities(self):
        capabilities = {
            Domain.Phy : (
                (Capability.Sniff | Capability.NoRawData),
                [
                    Commands.GetSupportedFrequencies,
                    Commands.SetASKModulation,
                    Commands.SetFSKModulation,
                    Commands.Set4FSKModulation,
                    Commands.GetSupportedFrequencies,
                    Commands.SetFrequency,
                    Commands.SetDataRate,
                    Commands.SetEndianness,
                    Commands.SetTXPower,
                    Commands.SetPacketSize,
                    Commands.SetSyncWord,
                    Commands.Sniff,
                    Commands.Send,
                    Commands.Start,
                    Commands.Stop,
                    Commands.Set4FSKModulation
                ]
            )
        }

        return capabilities

    def _get_manufacturer(self):
        return get_string(self.__yard, self.__yard.iManufacturer).encode('utf-8')

    def _get_serial_number(self):
        """Get serial number.
        """
        return bytes.fromhex(
            self.__yard.serial_number +
            f"{self.__yard.bus:04x}{self.__yard.address:04x}"
        )

    def _get_firmware_version(self):
        response = self._yard_send_command(
            YardApplications.SYSTEM,
            YardSystemCommands.BUILDTYPE
        )
        revision = int(response.split(b" r")[1][:-1])
        return (revision, 0, 0)

    def _get_url(self):
        return "https://github.com/atlas0fd00m/rfcat".encode('utf-8')

    def _send_packet(self, packet, repeat=1, offset=0):
        data = bytes(packet)

        # It is the only solution we found to allow transmitting data without
        # breaking the sniffer mode.
        old_state = self.__internal_state
        self.__internal_state = YardInternalStates.YARD_MODE_TX
        opened_stream = self.__opened_stream
        self.__opened_stream = False

        self._set_idle_mode()
        self._strobe_idle_mode()


        self._set_tx_mode()
        self._strobe_tx_mode()

        self._yard_send_command(YardApplications.NIC, YardNICCommands.XMIT,
                                pack("<HHH", len(data), repeat, offset) + data,
                                timeout=500)

        self.__internal_state = old_state
        self._restore_previous_mode()

        self.__opened_stream = opened_stream


    def _peek(self, address, size):
        return self._yard_send_command(
            YardApplications.SYSTEM,
            YardSystemCommands.PEEK,
            pack("<HH", size, address)
        )

    def _poke(self, address, data=b""):
        return self._yard_send_command(
            YardApplications.SYSTEM,
            YardSystemCommands.POKE,
            pack("<H", address) + data
        )

    def _set_rf_mode(self, mode):
        self._rf_mode = mode
        return self._yard_send_command(
            YardApplications.SYSTEM,
            YardSystemCommands.RFMODE,
            bytes([mode])
        )

    def _set_tx_mode(self):
        self._set_rf_mode(YardRFStates.STX)

    def _set_rx_mode(self):
        self._set_rf_mode(YardRFStates.SRX)

    def _set_idle_mode(self):
        self._set_rf_mode(YardRFStates.SIDLE)

    def _strobe_tx_mode(self):
        self._poke(YardMemoryRegisters.RFST, bytes([YardRFStates.STX]))

    def _strobe_rx_mode(self):
        self._poke(YardMemoryRegisters.RFST, bytes([YardRFStates.SRX]))

    def _strobe_idle_mode(self):
        self._poke(YardMemoryRegisters.RFST, bytes([YardRFStates.SIDLE]))

    def _strobe_cal_mode(self):
        self._poke(YardMemoryRegisters.RFST, bytes([YardRFStates.SCAL]))

    def _strobe_fstxon_mode(self):
        self._poke(YardMemoryRegisters.RFST, bytes([YardRFStates.SFSTXON]))

    def _strobe_return_mode(self):
        self._poke(YardMemoryRegisters.RFST, bytes([self._rf_mode]))

    def _set_rf_register(self, register, value):
        # Update the radio structure
        self.radio_structure.update()
        # Check Main Radio Control State
        old_marc_state = self.radio_structure.get("MARCSTATE")

        # Go back to idle state
        if old_marc_state != YardMARCStates.MARC_STATE_IDLE:
            self._strobe_idle_mode()

        self.radio_structure.set(register, value)

        # Go back to configured mode
        self._strobe_return_mode()


    def _set_clear_channel_assessment(self, mode=YardCCA.CCA_PACKET,
                                      absolute_threshold=0, relative_threshold=1, magnitude=3):

        masks_mcsm1 = YardRegistersMasks.MCSM1
        mcsm1 = self.radio_structure.get("MCSM1") & ~(
            masks_mcsm1.CCA_MODE.mask << masks_mcsm1.CCA_MODE.offset
        )
        mcsm1 |= (mode << masks_mcsm1.CCA_MODE.offset)

        mask_agcctrl2 = YardRegistersMasks.AGCCTRL2
        agcctrl2 = self.radio_structure.get("AGCCTRL2") & ~(
            mask_agcctrl2.MAGN_TARGET.mask << mask_agcctrl2.MAGN_TARGET.offset
        )
        agcctrl2 |= (magnitude << mask_agcctrl2.MAGN_TARGET.offset)

        mask_agcctrl1 = YardRegistersMasks.AGCCTRL1
        agcctrl1 = self.radio_structure.get("AGCCTRL1") & ~(
            (mask_agcctrl1.CARRIER_SENSE_REL_THR.mask << mask_agcctrl1.CARRIER_SENSE_REL_THR.offset) |
            (mask_agcctrl1.CARRIER_SENSE_ABS_THR.mask << mask_agcctrl1.CARRIER_SENSE_ABS_THR.offset)
        )
        agcctrl1 |= (
            ((relative_threshold & mask_agcctrl1.CARRIER_SENSE_REL_THR.mask) << mask_agcctrl1.CARRIER_SENSE_REL_THR.offset) |
            ((absolute_threshold & mask_agcctrl1.CARRIER_SENSE_ABS_THR.mask) << mask_agcctrl1.CARRIER_SENSE_ABS_THR.offset)
        )

        self._set_rf_register("MCSM1", mcsm1)
        self._set_rf_register("AGCCTRL1", agcctrl1)
        self._set_rf_register("AGCCTRL2", agcctrl2)

    def _set_frequency(self, frequency=433920000):
        freq_multiplier = (0x10000 / 1000000.0) / 24
        computed_value = int(frequency * freq_multiplier)

        self._set_rf_register("FREQ2", (computed_value >> 16))
        self._set_rf_register("FREQ1", (computed_value >> 8) & 0xFF)
        self._set_rf_register("FREQ0", (computed_value & 0xFF))

        if (
            (frequency > YardFrequencyTransitionPoints.FREQ_EDGE_900 and
            frequency < YardFrequencyTransitionPoints.FREQ_MID_900) or
            (frequency > YardFrequencyTransitionPoints.FREQ_EDGE_400 and
            frequency < YardFrequencyTransitionPoints.FREQ_MID_400) or
            (frequency < YardFrequencyTransitionPoints.FREQ_MID_300)
        ):
            self._set_rf_register("FSCAL2", YardVCOType.LOW_VCO)
        elif (
            frequency < 1e9 and (
                (frequency > YardFrequencyTransitionPoints.FREQ_MID_900) or
                (frequency > YardFrequencyTransitionPoints.FREQ_MID_400) or
                (frequency > YardFrequencyTransitionPoints.FREQ_MID_300))
            ):
            self._set_rf_register("FSCAL2", YardVCOType.HIGH_VCO)

    def _get_frequency(self):
        freq_multiplier = (0x10000 / 1000000.0) / 24

        self.radio_structure.update()
        num = (
                (self.radio_structure.get("FREQ2") << 16) +
                (self.radio_structure.get("FREQ1") << 8) +
                self.radio_structure.get("FREQ0")
        )
        return int(num / freq_multiplier)

    def _get_frequency_offset_estimate(self):
        self.radio_structure.update()
        frequency_offset_estimate = self.radio_structure.get("FREQEST")
        return int(frequency_offset_estimate)

    def _set_frequency_offset(self, intermediate_frequency_offset):
        self._set_rf_register("FSCTRL0", intermediate_frequency_offset)

    def _get_frequency_offset(self):
        self.radio_structure.update()
        return self.radio_structure.get("FSCTRL0")

    def _adjust_frequency_offset(self):
        self._get_frequency_offset_estimate()
        self._frequency_offset_accumulator += self.radio_structure.get("FREQEST")
        self._frequency_offset_accumulator &= 0xFF
        self._set_frequency_offset(self._frequency_offset_accumulator)

    def _set_modulation(self, modulation, invert=False):
        self.radio_structure.update()
        mask_mdmcfg2 = YardRegistersMasks.MDMCFG2
        mdmcfg2 = self.radio_structure.get("MDMCFG2") & ~(
            mask_mdmcfg2.MOD_FORMAT.mask << mask_mdmcfg2.MOD_FORMAT.offset
        )
        mdmcfg2 |= ((modulation & mask_mdmcfg2.MOD_FORMAT.mask) << mask_mdmcfg2.MOD_FORMAT.offset)

        if modulation == YardModulations.MODULATION_ASK and not invert:
            if self.radio_structure.get("PA_TABLE0") != 0 and self.radio_structure.get("PA_TABLE1") == 0:
                power = self.radio_structure.get("PA_TABLE0")
                self._set_power(power, invert=invert)
        else:
            if self.radio_structure.get("PA_TABLE0") == 0 and self.radio_structure.get("PA_TABLE1") != 0:
                power = self.radio_structure.get("PA_TABLE1")
                self._set_power(power, invert=invert)
        #print(mdmcfg2)
        self._set_rf_register("MDMCFG2", mdmcfg2)

    def _get_modulation(self):
        self.radio_structure.update()
        mask_mdmcfg2 = YardRegistersMasks.MDMCFG2
        format_mod = (
                    self.radio_structure.get("MDMCFG2") &
                    (mask_mdmcfg2.MOD_FORMAT.mask << mask_mdmcfg2.MOD_FORMAT.offset)
        ) >> mask_mdmcfg2.MOD_FORMAT.offset
        return format_mod

    def _set_power(self, power, invert=False):
        modulation = self._get_modulation()
        if modulation == YardModulations.MODULATION_ASK and not invert:
            self.radio_structure.set("PA_TABLE0", 0)
            self.radio_structure.set("PA_TABLE1", power)
            self.radio_structure.set("PA_TABLE2", 0)
            self.radio_structure.set("PA_TABLE3", 0)
            self.radio_structure.set("PA_TABLE4", 0)
            self.radio_structure.set("PA_TABLE5", 0)
            self.radio_structure.set("PA_TABLE6", 0)
            self.radio_structure.set("PA_TABLE7", 0)


        else:
            self.radio_structure.set("PA_TABLE0", power)
            self.radio_structure.set("PA_TABLE1", 0)
            self.radio_structure.set("PA_TABLE2", 0)
            self.radio_structure.set("PA_TABLE3", 0)
            self.radio_structure.set("PA_TABLE4", 0)
            self.radio_structure.set("PA_TABLE5", 0)
            self.radio_structure.set("PA_TABLE6", 0)
            self.radio_structure.set("PA_TABLE7", 0)

        mask_frend0 = YardRegistersMasks.FREND0
        frend0 = self.radio_structure.get("FREND0") & ~(mask_frend0.PA_POWER.mask << mask_frend0.PA_POWER.offset)
        if modulation == YardModulations.MODULATION_ASK:
            frend0 |= (1 << mask_frend0.PA_POWER.offset)
        self._set_rf_register("FREND0", frend0)

    def _get_channel_spacing(self):
        self.radio_structure.update()
        mask_mdmcfg1 = YardRegistersMasks.MDMCFG1
        channel_spacing_mantissa = self.radio_structure.get("MDMCFG0")
        channel_spacing_exponent = (
            (self.radio_structure.get("MDMCFG1") &
            (mask_mdmcfg1.CHANSPC_E.mask << mask_mdmcfg1.CHANSPC_E.offset)
            ) >> mask_mdmcfg1.CHANSPC_E.offset
        )
        channel_spacing = 1000000.0 * (24/(2**18)) * (
            (256 + channel_spacing_mantissa) * (2**channel_spacing_exponent)
        )
        return channel_spacing


    def _set_channel_spacing(self, channel_spacing_khz):
        self.radio_structure.update()
        mask_mdmcfg1 = YardRegistersMasks.MDMCFG1

        for exponent in range(4):
            mantissa = int((channel_spacing_khz * (2**18)) /
                           (((((1000000.0 * 24 * (2**exponent))))-256) +.5))
            if mantissa < 256:
                mdmcfg1 = (
                    self.radio_structure.get("MDMCFG1") &
                    ~(mask_mdmcfg1.CHANSPC_E.mask << mask_mdmcfg1.CHANSPC_E.offset)
                )
                mdmcfg1 |= ((exponent & mask_mdmcfg1.CHANSPC_E.mask) << mask_mdmcfg1.CHANSPC_E.offset)
                self._set_rf_register("MDMCFG1", mdmcfg1)
                self._set_rf_register("MDMCFG0", mantissa)
                return True
        return False

    def _set_packet_length(self, length, variable=False):
        self.radio_structure.update()
        mask_pktctrl0 = YardRegistersMasks.PKTCTRL0
        pktctrl0 = self.radio_structure.get("PKTCTRL0") & ~(
            mask_pktctrl0.LENGTH_CONFIG.mask << mask_pktctrl0.LENGTH_CONFIG.offset
        )
        pktctrl0 |= ((int(variable) & mask_pktctrl0.LENGTH_CONFIG.mask) << mask_pktctrl0.LENGTH_CONFIG.offset)

        self._set_rf_register("PKTLEN", length if length <= 255 else 0)

    def _is_packet_length_variable(self):
        self.radio_structure.update()
        mask_pktctrl0 = YardRegistersMasks.PKTCTRL0
        length_config = (
                        (
                            self.radio_structure.get("PKTCTRL0") &
                            (mask_pktctrl0.LENGTH_CONFIG.mask << mask_pktctrl0.LENGTH_CONFIG.offset)
                        ) >> mask_pktctrl0.LENGTH_CONFIG.offset
        )
        return bool(length_config)

    def _get_packet_length(self):
        self.radio_structure.update()
        return self.radio_structure.get("PKTLEN")

    def _set_crc(self, enable=False):
        self.radio_structure.update()
        mask_pktctrl0 = YardRegistersMasks.PKTCTRL0
        pktctrl0 = self.radio_structure.get("PKTCTRL0") & ~(
            mask_pktctrl0.CRC_EN.mask << mask_pktctrl0.CRC_EN.offset
        )
        pktctrl0 |= int(enable) << mask_pktctrl0.CRC_EN.offset
        self._set_rf_register("PKTCTRL0", pktctrl0)


    def _is_crc_enabled(self):
        self.radio_structure.update()
        mask_pktctrl0 = YardRegistersMasks.PKTCTRL0
        crc_enabled = bool(
            (
                self.radio_structure.get("PKTCTRL0") &
                (mask_pktctrl0.CRC_EN.mask << mask_pktctrl0.CRC_EN.offset)
            ) >> mask_pktctrl0.CRC_EN.offset
        )
        return crc_enabled

    def _set_whitening(self, enable=False):
        self.radio_structure.update()
        mask_pktctrl0 = YardRegistersMasks.PKTCTRL0
        pktctrl0 = self.radio_structure.get("PKTCTRL0") & ~(
            mask_pktctrl0.WHITE_DATA.mask << mask_pktctrl0.WHITE_DATA.offset
        )
        pktctrl0 |= int(enable) << mask_pktctrl0.WHITE_DATA.offset
        self._set_rf_register("PKTCTRL0", pktctrl0)


    def _is_whitening_enabled(self):
        self.radio_structure.update()
        mask_pktctrl0 = YardRegistersMasks.PKTCTRL0
        whitening_enabled = bool(
            (
                self.radio_structure.get("PKTCTRL0") &
                (mask_pktctrl0.WHITE_DATA.mask << mask_pktctrl0.WHITE_DATA.offset)
            ) >> mask_pktctrl0.WHITE_DATA.offset
        )
        return whitening_enabled


    def _set_preamble_quality_threshold(self, threshold=3):
        self.radio_structure.update()
        mask_pktctrl1 = YardRegistersMasks.PKTCTRL1
        pktctrl1 = self.radio_structure.get("PKTCTRL1") & ~(
            mask_pktctrl1.PQT.mask << mask_pktctrl1.PQT.offset
        )
        pktctrl1 |= (threshold & mask_pktctrl1.PQT.mask) << mask_pktctrl1.PQT.offset
        self._set_rf_register("PKTCTRL1", pktctrl1)


    def _get_preamble_quality_threshold(self):
        self.radio_structure.update()
        mask_pktctrl1 = YardRegistersMasks.PKTCTRL1
        pqt = int(
            (
                self.radio_structure.get("PKTCTRL1") &
                (mask_pktctrl1.PQT.mask << mask_pktctrl1.PQT.offset)
            ) >> mask_pktctrl1.PQT.offset
        )
        return pqt


    def _is_append_packet_status_enabled(self):
        self.radio_structure.update()
        mask_pktctrl1 = YardRegistersMasks.PKTCTRL1
        append_packet_status_enabled = bool(
            (
                self.radio_structure.get("PKTCTRL1") &
                (mask_pktctrl1.APPEND_STATUS.mask << mask_pktctrl1.APPEND_STATUS.offset)
            ) >> mask_pktctrl1.APPEND_STATUS.offset
        )
        return append_packet_status_enabled

    def _set_append_packet_status(self, enable=False):
        self.radio_structure.update()
        mask_pktctrl1 = YardRegistersMasks.PKTCTRL1
        pktctrl1 = self.radio_structure.get("PKTCTRL1") & ~(
            mask_pktctrl1.APPEND_STATUS.mask << mask_pktctrl1.APPEND_STATUS.offset
        )
        pktctrl1 |= int(enable) << mask_pktctrl1.APPEND_STATUS.offset
        self._set_rf_register("PKTCTRL1", pktctrl1)


    def _set_encoding(self, encoding=YardEncodings.NON_RETURN_TO_ZERO):
        self.radio_structure.update()
        mask_mdmcfg2 = YardRegistersMasks.MDMCFG2
        mdmcfg2 = self.radio_structure.get("MDMCFG2") & ~(
            mask_mdmcfg2.MANCHESTER_EN.mask << mask_mdmcfg2.MANCHESTER_EN.offset
        )
        mdmcfg2 |= (int(encoding) << mask_mdmcfg2.MANCHESTER_EN.offset)
        self._set_rf_register("MDMCFG2", mdmcfg2)


    def _get_encoding(self):
        self.radio_structure.update()
        mask_mdmcfg2 = YardRegistersMasks.MDMCFG2
        encoding = int(
            (
                self.radio_structure.get("MDMCFG2") &
                (mask_mdmcfg2.MANCHESTER_EN.mask << mask_mdmcfg2.MANCHESTER_EN.offset)
            ) >> mask_mdmcfg2.MANCHESTER_EN.offset
        )
        return YardEncodings.MANCHESTER if encoding else YardEncodings.NON_RETURN_TO_ZERO


    def _set_forward_error_correction(self, enable=False):
        self.radio_structure.update()
        mask_mdmcfg1 = YardRegistersMasks.MDMCFG1
        mdmcfg1 = self.radio_structure.get("MDMCFG1") & ~(
            mask_mdmcfg1.FEC_EN.mask << mask_mdmcfg1.FEC_EN.offset
        )
        mdmcfg1 |= int(enable) << mask_mdmcfg1.FEC_EN.offset
        self._set_rf_register("MDMCFG1", mdmcfg1)


    def _get_forward_error_correction(self):
        self.radio_structure.update()
        mask_mdmcfg1 = YardRegistersMasks.MDMCFG1
        forward_error_correction = bool(
            (
                self.radio_structure.get("MDMCFG1") &
                (mask_mdmcfg1.FEC_EN.mask << mask_mdmcfg1.FEC_EN.offset)
            ) >> mask_mdmcfg1.FEC_EN.offset
        )
        return forward_error_correction


    def _set_intermediate_frequency(self, intermediate_frequency):
        self.radio_structure.update()
        mask_fsctrl1 = YardRegistersMasks.FSCTRL1
        computed_value = int(0.5 + (intermediate_frequency * (2**10)) / (1000000.0 * 24))
        fsctrl1 = self.radio_structure.get("FSCTRL1") & ~(
            mask_fsctrl1.FREQ_IF.mask << mask_fsctrl1.FREQ_IF.offset
        )
        fsctrl1 |= int(computed_value) << mask_fsctrl1.FREQ_IF.offset
        self._set_rf_register("FSCTRL1", fsctrl1)


    def _get_intermediate_frequency(self):
        self.radio_structure.update()
        mask_fsctrl1 = YardRegistersMasks.FSCTRL1
        if_value = int(
            (
                self.radio_structure.get("MDMCFG1") &
                (mask_fsctrl1.FREQ_IF.mask << mask_fsctrl1.FREQ_IF.offset)
            ) >> mask_fsctrl1.FREQ_IF.offset
        )
        intermediate_frequency = (if_value * ((1000000.0 * 24) / (2**10)))
        return intermediate_frequency

    def _set_channel(self, channel):
        self.radio_structure.set("CHANNR", channel)

    def _get_channel(self, channel):
        self.radio_structure.update()
        return self.radio_structure.get("CHANNR")


    def _set_channel_bandwidth(self, bandwidth):
        self.radio_structure.update()

        for exponent in range(4):
            mantissa = int((((24*1000000.0)/(bandwidth*(2**exponent)*8.0)) - 4) + 0.5)
            if mantissa < 4:
                mdmcfg4_mask = YardRegistersMasks.MDMCFG4
                mdmcfg4 = self.radio_structure.get("MDMCFG4") & ~(
                    (mdmcfg4_mask.CHANBW_E.mask << mdmcfg4_mask.CHANBW_E.offset ) |
                    (mdmcfg4_mask.CHANBW_M.mask << mdmcfg4_mask.CHANBW_M.offset )
                )
                mdmcfg4 |= (
                    ((exponent & mdmcfg4_mask.CHANBW_E.mask) << mdmcfg4_mask.CHANBW_E.offset) |
                    ((mantissa & mdmcfg4_mask.CHANBW_M.mask) << mdmcfg4_mask.CHANBW_M.offset)
                )
                self._set_rf_register("MDMCFG4", mdmcfg4)
                bw = 1000.0*24 / (8.0*(4+mantissa) * (2**exponent))
                if bw > 102e3:
                    self._set_rf_register("FREND1", 0xb6)
                else:
                    self._set_rf_register("FREND1", 0x56)

                if bw > 325e3:
                    self._set_rf_register("TEST2", 0x88)
                    self._set_rf_register("TEST1", 0x31)
                else:
                    self._set_rf_register("TEST2", 0x81)
                    self._set_rf_register("TEST1", 0x35)
                return True
        return False

    def _get_channel_bandwidth(self):
        self.radio_structure.update()

        mdmcfg4_mask = YardRegistersMasks.MDMCFG4
        mdmcfg4 = self.radio_structure.get("MDMCFG4")
        exponent = (
            mdmcfg4 & (mdmcfg4_mask.CHANBW_E.mask << mdmcfg4_mask.CHANBW_E.offset )
            ) >> mdmcfg4.CHANBW_E.offset
        mantissa = (
            mdmcfg4 & (mdmcfg4_mask.CHANBW_M.mask << mdmcfg4_mask.CHANBW_M.offset )
            ) >> mdmcfg4.CHANBW_M.offset

        return (1000000.0*24) / (8.0*(4+mantissa) * (2**exponent))

    def _set_data_rate(self, data_rate):
        self.radio_structure.update()
        mdmcfg4 = self.radio_structure.get("MDMCFG4")
        mdmcfg4_mask = YardRegistersMasks.MDMCFG4

        for exponent in range(16):
            mantissa = int(((data_rate * (2**28)) / ((2**exponent) * (24*1000000.0)) - 256) + 0.5)
            if mantissa < 256:
                self._set_rf_register("MDMCFG3", mantissa)
                mdmcfg4 = mdmcfg4 & ~(mdmcfg4_mask.DRATE_E.mask << mdmcfg4_mask.DRATE_E.offset)
                mdmcfg4 |= ((exponent & mdmcfg4_mask.DRATE_E.mask) << mdmcfg4_mask.DRATE_E.offset)
                self._set_rf_register("MDMCFG4", mdmcfg4)
                return True
        return False

    def _get_data_rate(self):
        self.radio_structure.update()
        mdmcfg4_mask = YardRegistersMasks.MDMCFG4
        exponent = int(
            (
                self.radio_structure.get("MDMCFG4") &
                (mdmcfg4_mask.DRATE_E.mask << mdmcfg4_mask.DRATE_E.offset)
            ) >> mdmcfg4_mask.DRATE_E.offset
        )
        mantissa = self.radio_structure.get("MDMCFG3")
        data_rate = 1000000.0 * 24 * (256+mantissa) * (2**exponent) / (2**28)
        return data_rate


    def _set_deviation(self, deviation):
        deviatn_mask = YardRegistersMasks.DEVIATN

        for exponent in range(8):
            mantissa = int(((deviation * (2**17)) / ((2**exponent) * (24*1000000.0)) - 8) + 0.5)
            if mantissa < 8:
                deviatn = (
                    ((mantissa & deviatn_mask.DEVIATION_M.mask) << deviatn_mask.DEVIATION_M.offset) |
                    ((exponent & deviatn_mask.DEVIATION_E.mask) << deviatn_mask.DEVIATION_E.offset)
                )
                self._set_rf_register("DEVIATN", deviatn)
                return True
        return False

    def _get_deviation(self):
        self.radio_structure.update()
        deviatn_mask = YardRegistersMasks.DEVIATN
        deviatn = self.radio_structure.get("DEVIATN")
        exponent = int(
            (
                deviatn &
                (deviatn_mask.DEVIATION_E.mask << deviatn_mask.DEVIATION_E.offset)
            ) >> deviatn_mask.DEVIATION_E.offset
        )
        mantissa = int(
            (
                deviatn &
                (deviatn_mask.DEVIATION_E.mask << deviatn_mask.DEVIATION_E.offset)
            ) >> deviatn_mask.DEVIATION_E.offset
        )

        deviation = 1000000.0 * 24 * (8+mantissa) * (2**exponent) / (2**17)
        return deviation

    def _set_sync_word(self, sync_word, carrier_sense=False, bitflip_tolerance=False):
        sync_mode = 0
        if len(sync_word) == 0:
            sync_mode = int(carrier_sense) << 2 | 0
        elif len(sync_word) == 2:
            sync_mode = (int(carrier_sense) << 2) | (1 << int(bitflip_tolerance))
        elif len(sync_word) == 4:
            if sync_word[:2] == sync_word[2:]:
                sync_mode = (int(carrier_sense) << 2) | 0b11
            else:
                return False
        else:
            return False

        self.radio_structure.update()
        mdmcfg2 = self.radio_structure.get("MDMCFG2")
        mdmcfg2_mask = YardRegistersMasks.MDMCFG2

        mdmcfg2 &= ~(mdmcfg2_mask.SYNC_MODE.mask << mdmcfg2_mask.SYNC_MODE.offset)
        mdmcfg2 |= ((sync_mode & mdmcfg2_mask.SYNC_MODE.mask) <<  mdmcfg2_mask.SYNC_MODE.offset)
        if len(sync_word) > 0:
            self._set_rf_register("SYNC1",sync_word[1])
            self._set_rf_register("SYNC0",sync_word[0])
        self._set_rf_register("MDMCFG2", mdmcfg2)

        return True

    def _get_sync_word(self):
        self.radio_structure.update()
        mdmcfg2 = self.radio_structure.get("MDMCFG2")
        mdmcfg2_mask = YardRegistersMasks.MDMCFG2
        sync_mode = int(
            (mdmcfg2 & (mdmcfg2_mask.SYNC_MODE.mask << mdmcfg2_mask.SYNC_MODE.offset))
            >>  mdmcfg2_mask.SYNC_MODE.offset
        )
        if sync_mode & 0b11 == 0b11:
            multiplier = 2
        elif sync_mode & 0b11 == 0:
            multiplier = 0
        else:
            multiplier = 1
        return multiplier * bytes(
            [self.radio_structure.get("SYNC0"), self.radio_structure.get("SYNC1")]
        )

    def _get_number_of_preamble_bytes(self):
        self.radio_structure.update()
        mdmcfg1 = self.radio_structure.get("MDMCFG1")
        mdmcfg1_mask = YardRegistersMasks.MDMCFG1
        num_preamble_flag = int((
            mdmcfg1 &
            (mdmcfg1.NUM_PREAMBLE.mask << mdmcfg1.NUM_PREAMBLE.offset)
        ) >> mdmcfg1.NUM_PREAMBLE.offset)
        return NUM_PREAMBLE_LOOKUP_TABLE[num_preamble_flag]

    def _set_number_of_preamble_bytes(self, number):
        try:
            num_preamble_flag = NUM_PREAMBLE_LOOKUP_TABLE[number]
        except IndexError:
            return False

        self.radio_structure.update()
        mdmcfg1 = self.radio_structure.get("MDMCFG1")
        mdmcfg1_mask = YardRegistersMasks.MDMCFG1
        mdmcfg1 &= ~(mdmcfg1.NUM_PREAMBLE.mask << mdmcfg1.NUM_PREAMBLE.offset)

        mdmcfg1 |= ((num_preamble_flag & mdmcfg1.NUM_PREAMBLE.mask) << mdmcfg1.NUM_PREAMBLE.offset)
        self._set_rf_register("MDMCFG1", mdmcfg1)
        return True

    def compute_best_deviation(self):
        """Compute best deviation value.
        """
        data_rate = self._get_data_rate()
        if data_rate <= 2400:
            deviation = 5100
        elif data_rate <= 38400:
            deviation = 20000 * ((data_rate - 2400) / 36000)
        else:
            deviation = 129000 * ((data_rate - 38400) / 211600)
        return deviation

    def compute_best_channel_bandwidth(self):
        """Compute best channel bandwidth value.
        """
        frequency = self._get_frequency()
        data_rate = self._get_data_rate()
        #center_frequency = frequency + 14000000
        frequency_uncertainty = 2 * (20e-6 * frequency)
        min_bandwidth = frequency_uncertainty + data_rate
        best_bandwidth = None
        for possible in POSSIBLE_CHANNEL_BANDWIDTHS:
            if min_bandwidth < possible:
                best_bandwidth = possible
                break
        return best_bandwidth

    def _set_power_amplifier_mode(self, mode=0):
        return self._yard_send_command(
            YardApplications.NIC,
            YardNICCommands.SET_AMP_MODE,
            pack("B", mode)
        )
    def _get_power_amplifier_mode(self):
        return self._yard_send_command(
            YardApplications.NIC,
            YardNICCommands.GET_AMP_MODE
        )[0]

    def _set_packet_format(self, format=0):
        self.radio_structure.update()
        mask_pktctrl0 = YardRegistersMasks.PKTCTRL0

        pktctrl0 = self.radio_structure.get("PKTCTRL0") & ~(
            mask_pktctrl0.PKT_FORMAT.mask << mask_pktctrl0.PKT_FORMAT.offset
        )
        pktctrl0 |= ((int(format) & mask_pktctrl0.PKT_FORMAT.mask) << mask_pktctrl0.PKT_FORMAT.offset)

        self._set_rf_register("PKTCTRL0", pktctrl0)

    def set_test_config(self):
        """Put the YardStickOne in test configuration mode.
        """
        self._set_rf_register("IOCFG0",0x06)
        self._set_rf_register("SYNC1",0xaa)
        self._set_rf_register("SYNC0",0xaa)
        self._set_rf_register("PKTLEN",255)
        self._set_rf_register("PKTCTRL1",0x00)
        self._set_rf_register("PKTCTRL0",0x08)
        self._set_rf_register("FSCTRL1",0x0b)
        self._set_rf_register("FSCTRL0",0x00)
        self._set_rf_register("ADDR",0x00)
        self._set_rf_register("CHANNR",0x00)
        self._set_rf_register("MDMCFG4",0x68)
        self._set_rf_register("MDMCFG3",0xb5)
        self._set_rf_register("MDMCFG2",0x80)
        self._set_rf_register("MDMCFG1",0x23)
        self._set_rf_register("MDMCFG0",0x11)
        self._set_rf_register("MCSM2",0x07)
        self._set_rf_register("MCSM1",0x3f)
        self._set_rf_register("MCSM0",0x14)
        self._set_rf_register("DEVIATN",0x45)
        self._set_rf_register("FOCCFG",0x16)
        self._set_rf_register("BSCFG",0x6c)
        self._set_rf_register("AGCCTRL2",0x43)
        self._set_rf_register("AGCCTRL1",0x40)
        self._set_rf_register("AGCCTRL0",0x91)
        self._set_rf_register("FREND1",0x56)
        self._set_rf_register("FREND0",0x10)
        self._set_rf_register("FSCAL3",0xad)
        self._set_rf_register("FSCAL2",0x0A)
        self._set_rf_register("FSCAL1",0x00)
        self._set_rf_register("FSCAL0",0x11)
        self._set_rf_register("TEST2",0x88)
        self._set_rf_register("TEST1",0x31)
        self._set_rf_register("TEST0",0x09)
        self._set_rf_register("PA_TABLE0",0x05)
        self._set_rf_register("PA_TABLE1",0x00)
