"""
img_to_path.py
Andrew Lorimer, November 2024
Monash University

Converts a path in a monochrome image to a series of points 
in a defined format for laser writing with the confocal 
setup. Configuration is done in img_to_path.ini. This version
for use with LabView script.
"""

import configparser as cp
import cv2
import numpy as np
import sys

DEFAULTS = {'speed': '10',
            'dimensions': '100, 100',
            'z': '0',
            'beam_size':'0.1'}
DEFAULT_CONFIG_PATH = "img_to_path.ini"

def row(x, y, z, on, v):
    """
    Format a data row. Note on/off value defines whether the laser turns on/off 
    at the end of the line.
    """
    return "1\t{:.3f}\t{:.3f}\t{:.3f}\t{:.3f}\t{:d}\n".format(x, y, z, on, v)

def im_to_real(x, y):
    """Convert (x, y) image coordinates (px) to real world coordinates (um)"""
    return np.array((x, y)) / im_dim[0] * real_dim


if __name__ == "__main__":
    
    # Get config file path from arguments or use default path
    if len(sys.argv) > 1 and sys.argv[1]:
        config_path = sys.argv[1]
    else:
        config_path = DEFAULT_CONFIG_PATH

    # Set up config parser
    config = cp.ConfigParser(allow_unnamed_section=True)
    config['DEFAULT'] = DEFAULTS
    config.read_file(open(config_path))

    # Set config values
    speed = config.getint(cp.UNNAMED_SECTION, 'speed')
    real_dim = np.fromstring(config.get(cp.UNNAMED_SECTION, 'dimensions'), dtype=float, sep=",")
    z = config.getfloat(cp.UNNAMED_SECTION, 'z')
    beam_size = config.getfloat(cp.UNNAMED_SECTION, 'beam_size')
    input_path = config.get(cp.UNNAMED_SECTION, 'input')
    output_path = config.get(cp.UNNAMED_SECTION, 'output')

    # Read image
    im = cv2.imread(input_path, cv2.IMREAD_GRAYSCALE)
    im_dim = im.shape

    # Check if aspect ratios match
    if (im_dim[0] / im_dim[1] != real_dim[0]/real_dim[1]):
        print("Warning: input and output aspect ratios do not match - result will be distorted")

    # Threshold image to binary
    _, im = cv2.threshold(im, 127, 255, cv2.THRESH_BINARY)

    # Convert to 0 or 1
    im = im / 255.0
    if np.median(im) > 0.5:
        im = -(im - 1)

    # Find required scan interval based on beam size
    line_height = real_dim[1] / im_dim[1]
    if line_height > beam_size:
        # Need to interpolate (do scans in between each row of pixels)
        number_scans = int(np.round(line_height/beam_size))
    else:
        # Need to remove some scans
        number_scans = 1

    # Initialise output
    output = ""

    # Go to origin point with laser off
    output += row(0.0, 0.0, z, 0.0, speed)
    current = (0.0, 0.0)

    # Iterate through rows of pixels
    for i in range(im_dim[0]):
        for k in range(number_scans):
            diff = np.diff(im[i])
            indices_0_to_1 = list(np.where(diff == 1)[0] + 1)
            indices_1_to_0 = list(np.where(diff == -1)[0] + 1)
            if (len(indices_0_to_1) == 0 and len(indices_1_to_0) == 0):
                continue

            if (im[i, 0] == 1):
                # Start at left pixel
                point_real = im_to_real(i, 0)
            else:
                # Start at first transition to 1
                j = indices_0_to_1.pop(0)
                point_real = im_to_real(i, j)
            output += row(point_real[0]+k*beam_size, point_real[1], z, 1.0, speed)
            current = point_real

            while True:
                if len(indices_1_to_0) != 0:
                    j = indices_1_to_0.pop(0)
                    point_real = im_to_real(i, j)
                    output += row(point_real[0]+k*beam_size, point_real[1], z, 0.0, speed)
                    current = point_real

                if len(indices_0_to_1) != 0:
                    j = indices_0_to_1.pop(0)
                    point_real = im_to_real(i, j)
                    output += row(point_real[0]+k*beam_size, point_real[1], z, 1.0, speed)
                    current = point_real
                else:
                    break

    # Turn laser off at current position at go back to origin
    output += row(current[0], current[1], z, 0.0, speed)
    output += row(0.0, 0.0, z, 0.0, speed)

    # Write output
    with open(output_path, 'w') as f:
        f.write(output)