DATA_RECORD = 0x00
EOF = 0x01
ESAR = 0x02
ELAR = 0x04
SLAR = 0x05

def calc_crc8(number):
    crc8 = 0
    for i in range(1, len(number)-3, 2):
        byte_x = int(number[i:i+2],16)
        crc8 += byte_x
    return hex((~crc8+1) & 0xff)

def check_ihex_crc(number):
    calc_crc = calc_crc8(number)
    given_crc = hex(int(number[len(number)-3:len(number)-1],16))
    if given_crc == calc_crc:
        return True
    else:
        return False

def mycrc(line):
    crc = 0
    i = 1
    while(i < len(line)):
        crc = crc + int(line[i:i+2],16)
        i = i + 2
    crc = crc & 0xFF
    crc = ((~crc) + 1) & 0xFF
    return crc

def load_ihex(filename):
    base_address = 0
    current_address = 0
    frame_start_address = 0
    frame = []
    datasize = 0
    first = 1
    done = 0
    application = []
    start_address = 0

    with open(filename,'r') as f:
        lines = f.readlines()
        for line in lines:
            if not check_ihex_crc(line):
                print("CRC failed in hex file, exiting...")
                return None
            done += len(line)
            data_length = int(line[1:3],16)
            data_address = int(line[3:7], 16)
            data_type = int(line[7:9],16)
            data = []
            for i in range(9,data_length*2+9, 2):
                data.append(int(line[i:i+2],16))
            if data_type == DATA_RECORD:
                address = base_address + data_address

                if first:
                    first = 0
                    start_address = address
                    current_address = address
                # Fill any blanks between last data and this one
                application.extend([0xff]*(address-current_address))
                # Append the new data
                application.extend(data)
                current_address = address+data_length
            if data_type == EOF:
                break
            if data_type == ESAR:
                base_address = data[0] << 12 | data[1] << 4

            if data_type == ELAR:
                base_address = data[0] << 24 | data[1] << 16

            if data_type == SLAR:
                base_address = data[0] << 24 | data[1] << 16
                
    return (application, start_address)

def save_ihex(filename, application, flash_start_address):
    with open(filename,'w') as f:
        ihexlen = 2
        ihexaddr = 0
        ihexcode = 4
        ihexoffset = (int(flash_start_address) >> 16) & 0xFFFF
        hexline = ":{:02X}{:04X}{:02X}{:04X}".format(ihexlen, ihexaddr, ihexcode, ihexoffset)
        ihexcrc = mycrc(hexline)
        f.write("{}{:02X}\n".format(hexline,ihexcrc))
        for i in range(0, len(application), 16):
            ihexlen = len(application) - i # remaining bytes
            if(ihexlen > 16):
                ihexlen = 16 # max at 16 bytes per line
            ihexaddr = flash_start_address + i
            if((ihexaddr - (ihexoffset << 16)) >= 0x10000):
                # Time to increase the offset
                ihexoffset = ihexoffset + 1
                hexline = ":02000004{:04X}".format(ihexoffset)
                ihexcrc = mycrc(hexline)
                f.write("{}{:02X}\n".format(hexline, ihexcrc))
            ihexcode = 0
            ihexdata = ''.join('{:02X}'.format(x) for x in application[i:i+16])
            address_without_offset = ihexaddr - (ihexoffset << 16)
            hexline = ":{:02X}{:04X}{:02X}{}".format(ihexlen, address_without_offset, ihexcode, ihexdata)
            ihexcrc = mycrc(hexline)
            f.write("{}{:02X}\n".format(hexline,ihexcrc))

        f.write(":00000001FF\n")


if __name__ == '__main__':
    app, startaddr = load_ihex(r'C:\Users\david\Documents\Bigleap\Test Software\DisplayBoard_Factory_Test\firmware_loader\Bigleap_Freertos.hex')
    save_ihex('junk.hex', app, startaddr)