# SOME NOTES FOR RILEY FOR LATER
# EVERY MESSAGE TYPE WILL HAVE ITS OWN PACK OR SEND FUNCTION
# populate the constants into the struct, fill the struct variables, return the bytes, pass that pointer to send function
# we can do this everywhere and not worry about all the weirdass pythonisms. There's a LOT of excess data here we don't need.

#!/usr/bin/env python
"""
parse a MAVLink protocol XML file and generate a SPIN2 implementation

Copyright Riley August 2025
Released under GNU GPL version 3 or later
"""


import os
import sys
import textwrap
from . import mavtemplate

t = mavtemplate.MAVTemplate()

def generate_preamble(outf, msgs, basename, args, xml):
    print("Generating preamble")
    print("cache check wtf")
    params = dict(xml)
    params["FILELIST"] = (",".join(args),)
    params["DIALECT"] = os.path.splitext(os.path.basename(basename))[0]

    t.write(
        outf,
        '''
{Spin2_v51}
{{
MAVLink protocol implementation (auto-generated by mavgen.py)

Generated from: ${FILELIST}

Note: this file has been auto-generated. DO NOT EDIT
}}

DAT
    WIRE_PROTOCOL_VERSION    byte    "2.0"
    DIALECT            byte    "spin2"

CON
    PROTOCOL_MARKER_V1 = $FE
    PROTOCOL_MARKER_V2 = $FD
    HEADER_LEN_V1 = 6
    HEADER_LEN_V2 = 10

    MAVLINK_SIGNATURE_BLOCK_LEN = 13

    MAVLINK_IFLAG_SIGNED = $01
    LITTLE_ENDIAN = ${little_endian}
    PROTOCOL_MARKER = ${protocol_marker}
    MAVLINK_PAYLOAD_SIZE = 255
    MAVLINK_BUF_LEN = 280
    MAVLINK_IGNORE_CRC = True ' fix this later
    STRUCT int64(msb, lsb)
    STRUCT double(msb, lsb)
VAR    
    BYTE payload_buf [MAVLINK_PAYLOAD_SIZE]
    BYTE buf [MAVLINK_BUF_LEN]    
    BYTE sequence ' sequence number for the CURRENT SESSION
    LONG total_bytes_sent
    LONG total_packets_sent
    BYTE srcSystemId
    BYTE srcComponentId
    BYTE expected_length
    ' long startup_time
    
' OBJ
' logger : "logger" tODO: implement normie logging
' allow MAV_IGNORE_CRC=1 to ignore CRC, allowing some
' corrupted msgs to be seen    
    
CON 
' some base types from mavlink_types.h
    MAVLINK_TYPE_CHAR = 0
    MAVLINK_TYPE_UINT8_T = 1
    MAVLINK_TYPE_INT8_T = 2
    MAVLINK_TYPE_UINT16_T = 3
    MAVLINK_TYPE_INT16_T = 4
    MAVLINK_TYPE_UINT32_T = 5
    MAVLINK_TYPE_INT32_T = 6
    MAVLINK_TYPE_UINT64_T = 7
    MAVLINK_TYPE_INT64_T = 8
    MAVLINK_TYPE_FLOAT = 9
    MAVLINK_TYPE_DOUBLE = 10

{{CRC-16/MCRF4XX - based on checksum.h from mavlink library}}

VAR 
    WORD crc
    BYTE tmp1, tmp2
    
pub get_crc():result
    result := crc
    return
    
pub make_crc(msgadr,len,CRC_EXTRA)
    crc:=$FFFF
    byte[msgadr+len-2]:= CRC_EXTRA & $FF ' stick the crcextra in the first byte
    msgadr++ ' skip first byte (protocol marker)
    repeat len-2
        crc:=metabolize(crc,byte[msgadr++])
    
pri metabolize (ccrc,databyte) : temp
    temp:=databyte
    repeat 8
       if (ccrc & 1) == (temp & 1)
         ccrc:=(ccrc & $FFFF) >> 1
         temp:=(temp & $FF) >> 1
       else
         ccrc:=(ccrc & $FFFF) >> 1
         temp:=(temp & $FF) >> 1
         ccrc:=((ccrc & $FFFF) ^ $8408) & $FFFF
    temp:=ccrc

''',
        params,
    )


def generate_enums(outf, enums):
    print("Generating enums")
    outf.write("CON")
    for e in enums:
        outf.write("\n' %s\n" % e.name) # enum category
        for entry in e.entry:
            outf.write("%s = %u\n" % (entry.name, entry.value))
            description = entry.description.replace("\t", "    ")
            outf.write("%s = %u\n" % (entry.name+"_description", description))
    return
                 
def generate_message_ids(outf, msgs):
    print("Generating message IDs")
    outf.write("\n' message IDs\n")
    outf.write("CON\n")
    outf.write("\tMSG_ID_BAD_DATA = -1\n")
    outf.write("\tMSG_ID_UNKNOWN = -2\n")
    for m in msgs:
        nm = "MSG_ID_%s" % (m.name.upper())
        if (len(nm) > 30):
            nm = "M_ID_%s" % (m.name.upper())
        if (len(nm) > 30):            
            outstr = m.name.upper().replace("_A", "_a").replace("_E","_e").replace("_I","_i").replace("_O","_o").replace("_U","_u")            
            outstr = outstr.replace("A","").replace("E","").replace("I","").replace("O","").replace("U","")            
            nm = "M_ID_%s" % (outstr.upper())
        if (len(nm) > 30):
            nm = nm[0:30]
        outf.write("\t%s = %u\n" % (nm, m.id))


def byname_hash_from_field_attribute(m, attribute):
    strings = []
    for field in m.fields:
        value = getattr(field, attribute, None)
        if value is None or value == "":
            continue
        if attribute == "units":
            # hack; remove the square brackets further up
            if value[0] == "[":
                value = value[1:-1]
        strings.append('"%s": "%s"' % (field.name, value))
    return ", ".join(strings)


def generate_message_defs(outf, msgs, enums):
    wrapper = textwrap.TextWrapper(initial_indent="", subsequent_indent="    ")
    for m in msgs:
        m.classname = "MAVLink_%s_message" % m.name.lower()        
        m.spin_fields = []
        m.spin_names = []
        m.init_fields = []
        m.cleanname = m.name.lower()
        if len(m.cleanname) > 25:
            # strip then truncate
            m.cleanname.replace("system", "sys")
            m.cleanname.replace("SYSTEM", "SYS")
            m.cleanname = m.name.upper().replace("_A", "_a").replace("_E","_e").replace("_I","_i").replace("_O","_o").replace("_U","_u")            
            m.cleanname = m.cleanname.replace("A","").replace("E","").replace("I","").replace("O","").replace("U","")
            m.cleanname = m.cleanname.lower()
        if len(m.cleanname) > 25:
            m.cleanname = m.cleanname[0:25]
        if len(m.classname) > 30:
            m.classname = "M_%s_mg" % m.cleanname
        # SPIN2 STRUCT FORMAT IS CON STRUCT struct_ name (TYPE name[array],...)    
        # var limits are 30
        # ESCAPE SPIN KEYWORDS
        for field in m.ordered_fields:                
            # TODO: CHANGE THIS TO A RESERVED DICT
            if(field.name == "sequence"):
                field.name = "_sequence"
            elif(field.name == "end"):
                field.name = "_end"
            elif(field.name == "field"):
                field.name = "_field"
            if len(field.name) > 30:
                field.name.upper().replace("SYSTEM", "SYS")
                # strip then truncate
                field.name = field.name.upper().replace("_A", "_a").replace("_E","_e").replace("_I","_i").replace("_O","_o").replace("_U","_u")            
                field.name = field.name.replace("A","").replace("E","").replace("I","").replace("O","").replace("U","")
                field.name = field.name.lower()
            if len(field.name) > 30:
                field.name = field.name[0:30]            
            if field.type == "uint8_t" or field.type == "int8_t" or field.type.startswith("char"):
                if(field.array_length > 0):
                    m.spin_names.append(field.name) # array pointer takes precedence over struct pointer, we just receive a long
                    m.spin_fields.append("BYTE " + field.name + "[" + str(field.array_length) + "]")
                else:
                    m.spin_names.append(field.name)
                    m.spin_fields.append("BYTE " + field.name)
            elif field.type == "uint16_t" or field.type == "int16_t":
                if(field.array_length > 0):
                    m.spin_names.append(field.name) # array pointer takes precedence over struct pointer, we just receive a long
                    m.spin_fields.append("WORD " + field.name + "[" + str(field.array_length) + "]")
                else:
                    m.spin_names.append(field.name)
                    m.spin_fields.append("WORD " + field.name)
            elif field.type == "uint32_t" or field.type == "int32_t" or field.type == "float":
                if(field.array_length > 0):
                    m.spin_names.append(field.name) # array pointer takes precedence over struct pointer, we just receive a long
                    m.spin_fields.append(field.name + "[" + str(field.array_length) + "]")
                else:
                    m.spin_names.append(field.name)
                    m.spin_fields.append(field.name)
            elif field.type == "uint64_t" or field.type == "int64_t":
                if(field.array_length > 0):
                    m.spin_names.append(field.name) # array pointer takes precedence over struct pointer, we just receive a long
                    m.spin_fields.append("int64 " + field.name + "[" + str(field.array_length) + "]")
                else:
                    m.spin_names.append("int64 " + field.name)
                    m.spin_fields.append("int64 " + field.name)
            elif field.type == "double":
                if(field.array_length > 0):
                    m.spin_names.append(field.name) # array pointer takes precedence over struct pointer, we just receive a long
                    m.spin_fields.append("double " + field.name + "[" + str(field.array_length) + "]")
                else:
                    m.spin_names.append("double " + field.name)
                    m.spin_fields.append("double " + field.name)
                
            else:
                if field.array_length > 0:                    
                    m.spin_names.append(field.name) # I'm an array of unknown structs
                    m.spin_fields.append(field.type + field.name + "[" + str(field.array_length) + "]")
                else:
                    m.spin_fields.append(field.type + " " + field.name) # I'm an unknown struct
                    m.spin_names.append(field.type + " " + field.name)        
            if(field.array_length > 0):
                if(field.type == "double"):
                    m.init_fields.append("BYTEMOVE(@msg.%s, %s, %s*sizeof(double))" % (field.name, field.name, field.array_length))
                elif(field.type == "uint64_t" or field.type == "int64_t"):
                    m.init_fields.append("BYTEMOVE(@msg.%s, %s, %s*sizeof(int64)" % (field.name, field.name, field.array_length))
                else:
                    m.init_fields.append("BYTEMOVE(@msg.%s, %s, %s)" % (field.name, field.name, field.array_length))
            else:
                m.init_fields.append("msg.%s := %s" % (field.name, field.name))
        t.write(
            outf,
            '''

{{
${docstring}
}}
CON
    ${cleanname}_id = ${msgid}
    ${cleanname}_crcx = ${crc_extra}
    STRUCT ${classname} (${spin_fields})

PUB ${cleanname}_pack(${field_names}, ^${classname} msg): siz
    BYTEFILL(@payload_buf, 0, MAVLINK_PAYLOAD_SIZE)
    ${init_fields}
    ' build payload
    siz := sizeof(msg)
    BYTEMOVE(@payload_buf, @msg, siz)
    

PUB ${cleanname}_send(${field_names}): siz | ${classname} msg
    BYTEFILL(@payload_buf, 0, MAVLINK_PAYLOAD_SIZE)
    ${init_fields}
    ' build payload
    siz := sizeof(msg)
    BYTEMOVE(@payload_buf, @msg, siz)
    send_mavlink(${cleanname}_id, srcSystemId, srcComponentId, ${cleanname}_crcx, siz, False)

''',
                {
                    "classname": m.classname,
                    "field_names": ", ".join(m.spin_names),
                    "docstring": wrapper.fill(m.description.strip()),
                    "cleanname": m.cleanname,
                    "crc_extra": m.crc_extra,
                    "msgid": m.id,
                    "spin_fields": ", ".join(m.spin_fields),
                    "init_fields": "\n\t".join(m.init_fields),
                },
        )


def native_mavfmt(field):
    """Work out the struct format for a type."""
    map = {
        "float": "f",
        "double": "d",
        "char": "c",
        "int8_t": "b",
        "uint8_t": "B",
        "uint8_t_mavlink_version": "v",
        "int16_t": "h",
        "uint16_t": "H",
        "int32_t": "i",
        "uint32_t": "I",
        "int64_t": "q",
        "uint64_t": "Q",
    }
    return map[field.type]


def mavfmt(field):
    """work out the struct format for a type"""
    map = {
        "float": "f",
        "double": "d",
        "char": "c",
        "int8_t": "b",
        "uint8_t": "B",
        "uint8_t_mavlink_version": "B",
        "int16_t": "h",
        "uint16_t": "H",
        "int32_t": "i",
        "uint32_t": "I",
        "int64_t": "q",
        "uint64_t": "Q",
    }

    if field.array_length:
        if field.type == "char":
            return str(field.array_length) + "s"
        return str(field.array_length) + map[field.type]
    return map[field.type]


def mavpytype(field):
    c_type_to_py = {
        "float": "float",
        "double": "float",
        "char": "bytes",
        "int8_t": "int",
        "uint8_t": "int",
        "uint8_t_mavlink_version": "int",
        "int16_t": "int",
        "uint16_t": "int",
        "int32_t": "int",
        "uint32_t": "int",
        "int64_t": "int",
        "uint64_t": "int",
    }

    if field.array_length:
        if field.type == "char":
            return "bytes"
        return "Sequence[{}]".format(c_type_to_py[field.type])
    return c_type_to_py[field.type]


def mavdefault(field):
    """returns default value for field (as string) for mavlink2 extensions"""
    if field.type == "char":
        return 'b""'
    else:
        if field.array_length == 0:
            return "0"
        else:
            return "(" + ", ".join(["0"] * field.array_length) + ")"


def generate_mavlink_parser(outf, msgs, xml):
    print("Generating MAVLink parser")
# TODO: implement a lookup dict here if we still want one.
#    t.write(
#        outf,
#        """


#mavlink_map: Dict[int, Type[MAVLink_message]] = {
#""",
#        xml,
#    )
#    for m in msgs:
#        outf.write(
#            "    MAVLINK_MSG_ID_%s: MAVLink_%s_message,\n" % (m.name.upper(), m.name.lower())
#        )
#    outf.write("}\n\n")

    t.write(
        outf,
        '''
        
        
CON struct MAVLink(BYTE magic, BYTE len, BYTE incompat_flags, BYTE compat_flags, BYTE seq, BYTE sysid, BYTE compid, BYTE msgid, BYTE msgid_m, BYTE msgid_h, BYTE payload[MAVLINK_PAYLOAD_SIZE], WORD checksum)

CON struct MAVLink_Signed(BYTE magic, BYTE len, BYTE incompat_flags, BYTE compat_flags, BYTE seq, BYTE sysid, BYTE compid, BYTE msgid, BYTE msgid_m, BYTE msgid_h, BYTE payload[MAVLINK_PAYLOAD_SIZE], WORD checksum, BYTE signature[13])

CON struct MAVLink_1(BYTE magic, BYTE len, BYTE seq, BYTE sysid, BYTE compid, BYTE msgid, BYTE payload[MAVLINK_PAYLOAD_SIZE], WORD checksum)

' TODO: audit compatibility flags on send
PUB send_mavlink(msg_id, sys_id, comp_id, crc_extra, length, force_mavlink1) | MAVLink msg, b ' we could do full message instead of storing BUF and do one less BYTEMOVE at the cost of possibly confusing
    if(force_mavlink1)
        send_mavlink1(msg_id, sys_id, comp_id, length, crc_extra)
        return
    sequence := (sequence + 1) // 256
    msg.magic := PROTOCOL_MARKER_V2
    msg.seq := sequence
    msg.sysid := sys_id
    msg.compid := comp_id
    msg.len := length
    ' convert long to 3 bytes
    msg.msgid := 0 | msg_id
    msg.msgid_m := 0 | msg_id >> 8
    msg.msgid_h :=  0 | msg_id >> 16
        
    BYTEFILL(@msg.payload, 0, MAVLINK_PAYLOAD_SIZE)
    BYTEMOVE(@msg.payload, @payload_buf, msg.len) ' put the payload into the message struct, we don't need 2 send buffers    
    
    make_crc(@msg, msg.len+12, crc_extra)
    msg.checksum := crc
    total_packets_sent += 1
    total_bytes_sent += sizeof(msg) - MAVLINK_PAYLOAD_SIZE + msg.len

    REPEAT b FROM 0 to 9
        sendSerial(byte[@msg+b])
    REPEAT b FROM 10 to (msg.len+9)
        sendSerial(byte[@msg+b])    
    REPEAT b FROM 265 to (sizeof(msg)-1)
        sendSerial(byte[@msg+b])

PUB send_mavlink1(msg_id, sys_id, comp_id, length, crc_extra) | MavLink_1 msg, b
    sequence := (sequence + 1) // 256
    msg.magic := PROTOCOL_MARKER_V1
    msg.seq := sequence
    msg.sysid := sys_id
    msg.compid := comp_id
    msg.len := length
    BYTEFILL(@msg.payload, 0, MAVLINK_PAYLOAD_SIZE)
    BYTEMOVE(@msg.payload, @payload_buf, msg.len) ' put the payload into the message struct, we don't need 2 send buffers
    total_packets_sent += 1
    total_bytes_sent += sizeof(msg) - MAVLINK_PAYLOAD_SIZE + msg.len
    REPEAT b FROM 0 to 5
        sendSerial(byte[@msg+b])
    REPEAT b FROM 6 to (msg.len+5)
        sendSerial(byte[@msg+b])    
    REPEAT b FROM 260 to (sizeof(msg)-1)
        sendSerial(byte[@msg+b])
    
VAR
    LONG isMavlink2
    LONG sendSerial
    LONG recvSerial
    
PUB init_session(srcSystem, srcComponent, baud, serialTxPtr, serialRxPtr)
    isMavlink2 := TRUE
    srcSystemId := srcSystem
    srcComponentId := srcComponent
    expected_length := HEADER_LEN_V1 + 2 ' not sure that's right
    sendSerial := serialTxPtr
    recvSerial := serialRxPtr
    reset_session()    
    
PUB reset_session() ' reset session stats and sequence, keep IDs
    sequence~
    total_packets_sent~
    total_bytes_sent~
    isMavlink2 := TRUE

{{parser enums}}
CON 
    PARSE_STATE_UNINIT = 0
    PARSE_STATE_IDLE = 1
    PARSE_STATE_GOT_STX = 2
    PARSE_STATE_GOT_LENGTH = 3
    PARSE_STATE_GOT_INCOMPAT_FLAGS = 4
    PARSE_STATE_GOT_COMPAT_FLAGS = 5    
    PARSE_STATE_GOT_SEQ = 6
    PARSE_STATE_GOT_SYSID = 7
    PARSE_STATE_GOT_COMPID = 8
    PARSE_STATE_GOT_MSGID1 = 9
    PARSE_STATE_GOT_MSGID2 = 10
    PARSE_STATE_GOT_MSGID3 = 11    
    PARSE_STATE_GOT_CRC1 = 12
    PARSE_STATE_GOT_CRC2 = 13
    PARSE_STATE_GOT_PAYLOAD = 14
    PARSE_STATE_GOT_SIGNATURE = 15    

VAR 
    BYTE parse_state
    MAVLink inPacket
    BYTE payloadIndex    
    
PRI crcError()
    ' Do nothing for now. TODO: implement error handling
    return

PUB newPacket()
    parse_state := PARSE_STATE_IDLE
    inPacket~
    payloadIndex~
	
PUB getPacket(): packetAddr
	packetAddr := @inPacket
	return packetAddr

PUB parse_char(c)
    CASE_FAST parse_state
        PARSE_STATE_UNINIT..PARSE_STATE_IDLE:
            IF (c == PROTOCOL_MARKER_V2)
                inPacket.magic := c
                if (isMavlink2 <> TRUE)                    
                    isMavlink2 := TRUE
                parse_state := PARSE_STATE_GOT_STX
            ELSEIF (c == PROTOCOL_MARKER_V1)
                inPacket.magic := c
                if (isMavlink2)
                    isMavlink2 := False
                parse_state := PARSE_STATE_GOT_STX
        PARSE_STATE_GOT_STX: ' mavlink 1 and 2
            inPacket.len := c
            if (isMavlink2 == TRUE)
                parse_state := PARSE_STATE_GOT_LENGTH                
            else
                parse_state := PARSE_STATE_GOT_COMPAT_FLAGS ' skip to seq, mavlink 1
        PARSE_STATE_GOT_LENGTH:
            inPacket.incompat_flags := c
            if (c <> 0 && c <> 1) ' I am incompatible
                parse_state := PARSE_STATE_IDLE
            else
                parse_state := PARSE_STATE_GOT_INCOMPAT_FLAGS
        PARSE_STATE_GOT_INCOMPAT_FLAGS: inPacket.compat_flags := c
            parse_state := PARSE_STATE_GOT_COMPAT_FLAGS
        PARSE_STATE_GOT_COMPAT_FLAGS: inPacket.seq := c
            parse_state := PARSE_STATE_GOT_SEQ
        PARSE_STATE_GOT_SEQ: inPacket.sysid := c                
            parse_state := PARSE_STATE_GOT_SYSID
        PARSE_STATE_GOT_SYSID: inPacket.compid := c
            parse_state := PARSE_STATE_GOT_COMPID
        PARSE_STATE_GOT_COMPID: inPacket.msgid := c
            IF (isMavlink2 == TRUE)
                parse_state := PARSE_STATE_GOT_MSGID1 ' three byte id
            ELSE
                parse_state := PARSE_STATE_GOT_MSGID3 ' one byte id
        PARSE_STATE_GOT_MSGID1: inPacket.msgid_m := c
            parse_state := PARSE_STATE_GOT_MSGID2
        PARSE_STATE_GOT_MSGID2: inPacket.msgid_h := c
            if inPacket.len > 0
                parse_state := PARSE_STATE_GOT_MSGID3
            else
                parse_state := PARSE_STATE_GOT_PAYLOAD ' no payload
        PARSE_STATE_GOT_MSGID3: inPacket.payload[payloadIndex++] := c
            if (payloadIndex == inPacket.len)
                parse_state := PARSE_STATE_GOT_PAYLOAD
        PARSE_STATE_GOT_PAYLOAD: 
            ' don't handle the CRC just read it, verify it in handling steps
            byte[@inPacket.checksum] := c
            parse_state := PARSE_STATE_GOT_CRC1
        PARSE_STATE_GOT_CRC1:
            byte[@inPacket.checksum + 1] := c
            parse_state := PARSE_STATE_GOT_CRC2 ' done; this is our terminator state
            total_packets_sent++            
        PARSE_STATE_GOT_CRC2:
            parse_state := PARSE_STATE_IDLE
            crcError()        

PUB receive() : hasPacket | b, intended_packet_length
        b := recvSerial(0):1
        'we should be able to eat a whole packet but are we being flooded?
            if(b == $FD) ' start packet; we may need to figure out why the GCS has started sending mavlink1 packets                                        
                newPacket()
                hasPacket := TRUE
                intended_packet_length := recvSerial(10):1 ' second byte is payload length, packet length is payload length +12 (10 before, 2 after)
                IF (intended_packet_length == -1)
                    return FALSE            
                parse_char(b)
                parse_char(intended_packet_length)
                REPEAT intended_packet_length + 10 ' 12 bytes, but we already parsed the first two
                    b := recvSerial(10):1
                    if(b == -1)
                        hasPacket := FALSE
                        quit
                    parse_char(b)                    
''',
        xml,
    )


def generate_methods(outf, msgs):
    print("Generating methods")

    def field_descriptions(fields):
        ret = ""
        for f in fields:
            field_info = ""
            if f.units:
                field_info += "%s " % f.units
            field_info += "(type:%s" % f.type
            if f.enum:
                field_info += ", values:%s" % f.enum
            field_info += ")"
            ret += "        %-18s        : %s %s\n" % (
                f.name,
                f.description.strip(),
                field_info,
            )
        return ret

    wrapper = textwrap.TextWrapper(initial_indent="", subsequent_indent="        ")

    for m in msgs:
        comment = "%s\n\n%s" % (wrapper.fill(m.description.strip()), field_descriptions(m.fields))

        field_names = []
        for i in range(len(m.fields)):
            f = m.fields[i]
            python_type = mavpytype(f)
            if f.omit_arg:
                field_names.append("%s: %s = %s" % (f.name, python_type, f.const_value))
            elif m.extensions_start is not None and i >= m.extensions_start:
                fdefault = m.fielddefaults[i]
                field_names.append("%s: %s = %s" % (f.name, python_type, fdefault))
            else:
                field_names.append("%s: %s" % (f.name, python_type))

        self_ret_type = " -> MAVLink_" + m.name.lower() + "_message"

        t.write(
            outf,
            '''
            {TODO: figure out if we need to generate methods using this code}
''',
                {
                    "NAMELOWER": m.name.lower(),
                    "ARG_FIELDNAMES": ", ".join(field_names),
                    "COMMENT": comment,
                    "FIELDNAMES": ", ".join(m.fieldnames),
                    "self_ret_type": self_ret_type,
                },
        )
# TODO: move all the field handling stuff out of the individual generator methods so we call it once instead of 3 times
def generate_case(outf, msgs, dialect):    
    wrapper = textwrap.TextWrapper(initial_indent="", subsequent_indent="    ")
    t.write(outf,
'''
{Spin2_v51}
{{
    MAVLink message parser object for ${DIALECT} dialect, (auto-generated by mavgen.py)
}}
OBJ
    mavlink : "${DIALECT}"    
{{
    Handles an incoming MAVLink message.
}}

PUB checkCrc(crcextra, ^mavlink.MAVLink msg): result | newcrc
    mavlink.make_crc([msg], msg.len+12, crcextra)
    newcrc := mavlink.get_crc()
    if (msg.checksum <> newcrc)
        return FALSE
    else
        return TRUE

PUB crcError()
    ' TODO: Handle CRC errors for your solution

PUB handleMessage(^mavlink.MAVLink packet) | msgid
    ' little endian
    msgid := packet.msgid_h << 16
    msgid |= packet.msgid_m << 8
    msgid |= packet.msgid
    
    ' Can't use CASE_FAST; ID values have to be within 255 of each other
    ' maybe use a compare and two CASE statements in a flight controller implementation but that is beyond the scope of pymavgen
    CASE msgid
        mavlink.MSG_ID_MANUAL_CONTROL: ' highest priority is manual control and mode messages
            hdl_manual_control(@packet)
''',{"DIALECT": dialect}, )
    for m in msgs:
        if (m.name == "MANUAL_CONTROL" or m.name == "HEARTBEAT"): # skip manual control and heartbeat; manual control should be first and heartbeat last.
            continue        
        t.write(
            outf,
            '''
        mavlink.${cleanname}_id:
            if(checkCrc(mavlink.${cleanname}_crcx, @packet))
                hdl_${cleanname}(@packet)
            else
                crcError()
''',
                {
                    "classname": m.classname,
                    "field_names": ", ".join(m.spin_names),
                    "docstring": wrapper.fill(m.description.strip()),
                    "cleanname": m.cleanname,
                    "crc_extra": m.crc_extra,
                    "msgid": m.id,
                    "spin_fields": ", ".join(m.spin_fields),
                    "init_fields": "\n\t".join(m.init_fields),
                },
        )
    t.write(
        outf,
        '''
        mavlink.MSG_ID_HEARTBEAT:
            hdl_heartbeat(@packet)
        
''') 
def generate_handler(outf, msgs):
    wrapper = textwrap.TextWrapper(initial_indent="", subsequent_indent="    ")
    for m in msgs:        
        t.write(
            outf,
'''{{${name}(${msgid})
${spin_fields}
}}
PUB hdl_${cleanname}(^mavlink.MAVLink packet) | mavlink.${classname} msg
    BYTEMOVE(@msg, @packet.payload, sizeof(msg))
    ' TODO: implement handler
                
''',
                {
                    "name": m.name.upper(),
                    "classname": m.classname,
                    "field_names": ", ".join(m.spin_names),
                    "docstring": wrapper.fill(m.description.strip()),
                    "cleanname": m.cleanname,
                    "crc_extra": m.crc_extra,
                    "msgid": m.id,
                    "spin_fields": ", ".join(m.spin_fields),
                    "init_fields": "\n\t".join(m.init_fields),
                },
        )


def generate(basename, xml):
    """generate complete spin2 implementation"""
    if basename.endswith(".spin2"):
        filename = basename
    else:
        filename = basename + ".spin2"
    
    msgs = []
    enums = []
    filelist = []
    for x in xml:
        msgs.extend(x.message)
        enums.extend(x.enum)
        filelist.append(os.path.basename(x.filename))

    for m in msgs:
        m.fielddefaults = []
        if xml[0].little_endian:
            m.fmtstr = "<"
        else:
            m.fmtstr = ">"
        m.native_fmtstr = m.fmtstr
        m.instance_field = None
        
        for f in m.ordered_fields:
            m.fmtstr += mavfmt(f)
            m.fielddefaults.append(mavdefault(f))
            m.native_fmtstr += native_mavfmt(f)
            if f.instance:
                m.instance_field = f.name
        m.order_map = [0] * len(m.fieldnames)
        m.len_map = [0] * len(m.fieldnames)
        m.array_len_map = [0] * len(m.fieldnames)
        for i in range(0, len(m.fieldnames)):
            m.order_map[i] = m.ordered_fieldnames.index(m.fieldnames[i])
            m.array_len_map[i] = m.ordered_fields[i].array_length
        for i in range(0, len(m.fieldnames)):
            n = m.order_map[i]
            m.len_map[n] = m.fieldlengths[i]

    print("Generating %s" % filename)
    outf = open(filename, "w")
    dialect = xml[0].filename
    dialect = dialect[dialect.rindex("/")+1:-4]  # remove .xml; we don't use os.sep here it's jank
    xml = xml[0].__dict__
    generate_preamble(outf, msgs, basename, filelist, xml)
    generate_message_ids(outf, msgs)
    generate_message_defs(outf, msgs, enums)
    generate_mavlink_parser(outf, msgs, xml)    
    # generate_methods(outf, msgs) we don't need to generate methods yet. Future.
    outf.close()
    print("Generated %s OK" % filename)
    # generates a handler skeleton
    handlername = filename[:-6] + "_handler.spin2"
    outf = open(handlername, "w")   
    generate_case(outf, msgs, dialect)
    generate_handler(outf, msgs)