def save_config_file(config_file, config):
    with open(config_file, 'w') as f:
        config_json = ujson.dumps(config)
        f.write("%s" % config_json) def load_config_file(file_name):
    with open(file_name, 'r') as f:
        config_file = ujson.loads(f.readall())
    return config_file class EnableMessage:

    MESSAGE_TYPE_ID = 40

    def __init__(self):
        pass

    @staticmethod
    def decode_payload(payload):
        """
        Used to enable the generation of a specific message type
        :param payload: the payload (bytearray) of the received packed
        :return: the decoded content of the packet; in this case the MESSAGE_TYPE_ID of the message to enable
        """
        return payload[0] class DisableMessage:

    MESSAGE_TYPE_ID = 50

    def __init__(self):
        pass

    @staticmethod
    def decode_payload(payload):
        """
        Used to disable the generation of a specific message type
        :param payload: the payload (bytearray) of the received packed
        :return: the decoded content of the packet; in this case the MESSAGE_TYPE_ID of the message to disable
        """
        return payload[0] class EnableEscCalibration:

    MESSAGE_TYPE_ID = 60

    def __init__(self):
        pass

    @staticmethod
    def enable_esc_calibration():
        '''
        Used to enable esc calibration by changing a flag in the app_config.json. By rebooting the board the change
        will take effect. Rebooting the board again the standard mode will be restored.
        :return:
        '''
        config = load_config_file("app_config.json")
        config['esc_calibration_mode'] = True
        save_config_file("app_config.json", config) class SavePIDSettings:

    MESSAGE_TYPE_ID = 70

    def __init__(self):
        pass

    @staticmethod
    def decode_payload(payload):
        """
        Decode message payload
        :param payload: byte stream representing the message payload
        :return: an array of 8 float [stab_Kp,stab_Kd,stab_Ki,Max Increment,gyro_Kp,gyro_Kd,gyro_Ki,gyro_Max Increment]
        """
        pid_settings = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

        for i in range(0, 8):
            pid_settings[i] = ustruct.unpack('>f', payload[i*4:i*4 + 4])[0]

        return pid_settings class ReadPID:

    MESSAGE_TYPE_ID = 100
    MESSAGE_KEY = 'ReadPID'

    def __init__(self, h_builder, attitude):
        """
        Used to carry the current PID settings as an array of 8 float:
        [stab_Kp,stab_Kd,stab_Ki,Max Increment,gyro_Kp,gyro_Kd,gyro_Ki,gyro_Max Increment]
        :param h_builder: HeaderBuilder object
        :param attitude: AttitudeController object
        """
        self.attitude_controller = attitude
        self.header_builder = h_builder
        self.QCI = 0
        self.floatList = self.attitude_controller.get_pid_settings()
        self.PAYLOAD_PID = struct.pack('%sf' % len(self.floatList), *self.floatList)
        self.PAYLOAD = bytearray(self.PAYLOAD_PID)
        self.PAYLOAD_LENGTH = len(self.PAYLOAD)
        self.EOF = bytearray([self.PAYLOAD[0] & 255])
        self.FAIL_SAFE = (self.attitude_controller.get_rc_controller()).get_link_status()
        self.header = bytearray(h_builder.get_header(self))
        self.message = self.header + self.PAYLOAD + self.EOF

    def get_bytes(self):
        return self.message class SendPIDSettings:

    MESSAGE_TYPE_ID = 90

    def __init__(self):
        """
        Used to trigger the generation of an ap_read_pid_settings message.
        :param h_builder: HeaderBuilder object
        :param attitude: AttitudeController object
        """
        pass

    @staticmethod
    def decode_payload(payload):
        return payload[0] class GyroCalibration:

    MESSAGE_TYPE_ID = 80

    def __init__(self):
        pass

    @staticmethod
    def decode_payload(payload):
        """
        Async message used to start and stop the gyro calibration.
        *****Currently not working*****
        """
        return payload[0] class SaveTxCalibration:

    MESSAGE_TYPE_ID = 110

    def __init__(self):
        pass

    @staticmethod
    def decode_payload(payload):
        """
        Decode message payload
        :param payload: byte stream representing the message payload
        :return: a list of 3 list of floats representing the PWM threshold values for each of the N active channels
         [[min threshold values],[max threshold values], [center threshold values]]
        """

        # 4 byte per float * 3 set of thesholds
        byte_per_thd_set = int(len(payload)/3)
        min_thd_vals = [0.0 for i in range(0, int(byte_per_thd_set/4))]
        max_thd_vals = [0.0 for i in range(0, int(byte_per_thd_set/4))]
        center_thd_vals = [0.0 for i in range(0, int(byte_per_thd_set/4))]

        for i in range(0, int(byte_per_thd_set/4)):
            min_thd_vals[i] = struct.unpack('>f', payload[i*4:i*4 + 4])[0]

        for i in range(0, int(byte_per_thd_set/4)):
            max_thd_vals[i] = struct.unpack('>f', payload[byte_per_thd_set + i*4:i*4 + 4 + byte_per_thd_set])[0]

        for i in range(0, int(byte_per_thd_set/4)):
            center_thd_vals[i] = struct.unpack('>f', payload[2*byte_per_thd_set + i*4:i*4 + 2*byte_per_thd_set + 4])[0]

        return [min_thd_vals, max_thd_vals, center_thd_vals]
import utils.airpy_logger as logger
from utils.airpy_config_utils import save_config_file, load_config_file
from aplink.messages.ap_enable_message import EnableMessage
from aplink.messages.ap_disable_message import DisableMessage
from aplink.messages.ap_enable_esc_calibration import EnableEscCalibration
from aplink.messages.ap_save_pid_settings import SavePIDSettings
from aplink.messages.ap_read_pid_settings import ReadPID
from aplink.messages.ap_send_pid_settings import SendPIDSettings
from aplink.messages.ap_gyro_calibration import GyroCalibration
from aplink.messages.ap_save_tx_calibration import SaveTxCalibration

    def __init__(self, apl_manager, streamer, h_builder):
        """
        This class is used to handle incoming APLINK messages received through the serial interface
        :param apl_manager: AplinkManager object
        :param streamer: airpy_byte_streamer object used to write on the serial interface
        :param h_builder: HeaderBuilder object used to generate the APLINK protocol Header
        """
        self.byte_streamer = streamer
        self.header_builder = h_builder
        self.aplink_manager = apl_manager
        self.tmpByte = None
        self.startByteFound = False
        self.byteIndex = 0
        self.messageId = 0
        self.QCI = 3
        self.lastFragment = 0
        self.messageTypeId = 0
        self.payloadLength = 0
        self.EOF = None
        self.payload = None

        # reporting
        self.valid_msg_count = 0
        self.discarded_msg_count = 0

    def read_byte(self):
        # Read a new byte from the serial connection
        self.tmpByte = self.byte_streamer.read_byte()

        if self.tmpByte is not None:

            if self.startByteFound:
                if self.byteIndex < self.header_builder.HEADER_LEN:
                    self.parse_header()
                else:
                    self.load_payload()
            else:
                if self.tmpByte[0] == 15:  # TODO change into constant
                    self.startByteFound = True
                    self.byteIndex += 1

    def parse_header(self):

        if self.byteIndex == self.header_builder.MESSAGE_ID_BYTE_1:
            self.messageId = self.tmpByte[0] << 8

        elif self.byteIndex == self.header_builder.MESSAGE_ID_BYTE_2:
            self.messageId += self.tmpByte[0]

        elif self.byteIndex == self.header_builder.QCI_AND_LAST_FRAGMENT:
            self.QCI = (self.tmpByte[0] & 0xF8) >> 3
            self.lastFragment = self.tmpByte[0] & 0x07

        elif self.byteIndex == self.header_builder.MESSAGE_TYPE_ID:
            self.messageTypeId = self.tmpByte[0]

        elif self.byteIndex == self.header_builder.PAYLOAD_LENGTH:
            self.payloadLength = self.tmpByte[0]

        self.byteIndex += 1

    def load_payload(self):
        if self.byteIndex == self.header_builder.HEADER_LEN + self.payloadLength:

            if self.tmpByte[0] == self.EOF:
                self.decode_payload(self.messageTypeId, self.payload)
                self.valid_msg_count += 1

            else:
                self.discarded_msg_count += 1

            self.byteIndex = -1
            self.startByteFound = False

        elif self.byteIndex == self.header_builder.HEADER_LEN:  # 1st byte of the payload is replicated at the EOF
            self.EOF = self.tmpByte[0]
            self.payload = bytearray(self.payloadLength)
            self.payload[0] = self.tmpByte[0]

        else:
            self.payload[self.byteIndex-self.header_builder.HEADER_LEN] = self.tmpByte[0]

        self.byteIndex += 1

    def decode_payload(self, message_type_id, payload):

        if message_type_id == EnableMessage.MESSAGE_TYPE_ID:
            self.aplink_manager.set_message_status(EnableMessage.decode_payload(payload), 1)
        elif message_type_id == DisableMessage.MESSAGE_TYPE_ID:
            self.aplink_manager.set_message_status(EnableMessage.decode_payload(payload), 0)
        elif message_type_id == EnableEscCalibration.MESSAGE_TYPE_ID:
            EnableEscCalibration.enable_esc_calibration()
        elif message_type_id == SavePIDSettings.MESSAGE_TYPE_ID:
            pid_settings = SavePIDSettings.decode_payload(payload)
            config = load_config_file("config.json")
            config['attitude']['stab_Kp'] = pid_settings[0]
            config['attitude']['stab_Kd'] = pid_settings[1]
            config['attitude']['stab_Ki'] = pid_settings[2]
            config['attitude']['max_increment'] = pid_settings[3]
            config['attitude']['gyro_Kp'] = pid_settings[4]
            config['attitude']['gyro_Kd'] = pid_settings[5]
            config['attitude']['gyro_Ki'] = pid_settings[6]
            config['attitude']['max_gyro_increment'] = pid_settings[7]
            save_config_file("config.json", config)

            # Release the memory
            config = None

            # update current values
            self.aplink_manager.attitude.set_PID_settings(pid_settings)
        elif message_type_id == GyroCalibration.MESSAGE_TYPE_ID:

            if GyroCalibration.decode_payload(payload) == 10:  # start Calibration
                self.aplink_manager.attitude.gyro_calibration(False)
                logger.info("Gyro Calibration Started")
            elif GyroCalibration.decode_payload(payload) == 20:  # stop Calibration
                self.aplink_manager.attitude.gyro_calibration(True)
                logger.info("Gyro Calibration Completed")


        elif message_type_id == SendPIDSettings.MESSAGE_TYPE_ID: