"""
img_to_path_dots.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.
Uses an array of dots instead of line scanning. For use with 
LabView script.
"""

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

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

# TODO make proper config values
padding = 10    # um
resolution = 0.5

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()
    config['DEFAULT'] = DEFAULTS
    config.read_file(open(config_path))

    # Set config values
    speed = config.getint(CONFIG_SECTION, 'speed')
    real_dim = np.fromstring(config.get(CONFIG_SECTION, 'dimensions'), dtype=float, sep=",")
    z = config.getfloat(CONFIG_SECTION, 'z')
    beam_size = config.getfloat(CONFIG_SECTION, 'beam_size')
    input_path = config.get(CONFIG_SECTION, 'input')
    output_path = config.get(CONFIG_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")

    # Resize image to produce the required dot resolution
    im = cv2.resize(im, (int(real_dim[0]/resolution), int(real_dim[1]/resolution)))
    im_dim = im.shape

    # 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)

    cv2.imshow("display", im)
    cv2.waitKey(0)


    # Initialise output
    output = ""

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

    # Iterate through pixels
    direction = 0
    current_pos = (0, 0)
    for i in range(im_dim[0]):
        direction = current_pos[1] > im_dim[1]*resolution
        # if direction == 0:
        start = 0
        stop = im_dim[0]
        step = 1
        # else:
        #     start = im_dim[0]
        #     stop = 0
        #     step = -1
        for j in range(start, stop, step):
            if (im[i,j] == 1):
                output += row(i*resolution, j*resolution, z, 0.0, speed)
                output += row(i*resolution, j*resolution, z, 1.0, speed)
                output += row(i*resolution, j*resolution, z, 0.0, speed)
                current_pos = (i*resolution, j*resolution)

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

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