# coding: utf-8
# python 2 only

# Copyright (c) 2026 TormachTips.com. All rights reserved.
# Licensed under the TormachTips Personal Use License.
# Permission is granted only for private personal use and private personal modification.
# No sharing, publication, distribution, resale, sublicensing, screenshots, code excerpts,
# benchmarks, or videos are permitted without prior written permission.
# Requests:         tormach.1100m@gmail.com
# Information page: https://tormachtips.com/plugins.htm

#############################################
##                                         ##
##        Home Position Logger 0.95        ##
##           www.tormachtips.com           ##
##                                         ##
#############################################

# 0.95 - Public release. - 6/17/2026

import os
import time
import glib
import constants
import singletons
from ui_hooks import plugin

CURRENT_VER              = "0.95"
SCRIPT_NAME              = "Home Position Logger"
DESCRIPTION              = "Logs G53 XYZ whenever active WCS position matches the configured target."
ENABLED                  = 1
DEV_MACHINE              = 1
DEV_MACHINE_FLAG         = "/home/operator/gcode/python/dev_machine.txt"
HOME_POS_LOG_PATH        = "/home/operator/gcode/python/home_logger_pos.txt"
CHECK_MS                 = 500      # Poll interval in milliseconds. 500 = check twice per second.
ZERO_TOLERANCE           = 0.00005  # Position match tolerance. 0.00005 rounds safely to 0.0000.
TARGET_X                 = 0.0000   # Active WCS X target to match.
TARGET_Y                 = 0.0000   # Active WCS Y target to match.
TARGET_Z                 = 0.0000   # Active WCS Z target to match when REQUIRE_Z_MATCH = 1.
REQUIRE_Z_MATCH          = 0        # 0 = match X/Y only. 1 = require X/Y/Z to match.
LOG_REPEAT_SAME_POSITION = 0        # 0 = log once per matching position. 1 = log every check while matched.

class UserPlugin(plugin):
    def __init__(self):
        plugin.__init__(self, SCRIPT_NAME)
        self.ui = None
        self._last_logged_signature = ""
        dev_machine_found = os.path.exists(DEV_MACHINE_FLAG)
        if dev_machine_found:
            plugin_enabled = DEV_MACHINE
        else:
            plugin_enabled = ENABLED
        if plugin_enabled:
            glib.timeout_add(3000, self.start_process)
            return
        if dev_machine_found:
            self.error_handler.write("[%s] Dev machine found. Plugin loaded, but disabled by DEV_MACHINE." % SCRIPT_NAME, constants.ALARM_LEVEL_QUIET)
        else:
            self.error_handler.write("[%s] Plugin loaded, but disabled." % SCRIPT_NAME, constants.ALARM_LEVEL_QUIET)
            self.error_handler.write("[%s] To enable, open script, find ENABLED = 0 and change to ENABLED = 1" % SCRIPT_NAME, constants.ALARM_LEVEL_QUIET)

    def start_process(self):
        try:
            self.ui = singletons.g_Machine
            if self.ui is None:
                return True
            self._ensure_log_dir()
            glib.timeout_add(CHECK_MS, self.check_home_position)
            self.error_handler.write("[%s] Started. Watching active WCS target every %d ms." % (SCRIPT_NAME, CHECK_MS), constants.ALARM_LEVEL_QUIET)
        except Exception as e:
            self.error_handler.write("[%s] Startup error: %s" % (SCRIPT_NAME, str(e)), constants.ALARM_LEVEL_LOW)
        return False

    def _ensure_log_dir(self):
        log_dir = os.path.dirname(HOME_POS_LOG_PATH)
        if log_dir and os.path.isdir(log_dir) == False:
            os.makedirs(log_dir)

    def _poll_status(self):
        try:
            if self.ui is None:
                self.ui = singletons.g_Machine
            if self.ui is None:
                return None
            if hasattr(self.ui, "status"):
                self.ui.status.poll()
                return self.ui.status
        except:
            pass
        return None

    def _active_wcs_name(self, status):
        names = {
            1: "G54",
            2: "G55",
            3: "G56",
            4: "G57",
            5: "G58",
            6: "G59",
            7: "G59.1",
            8: "G59.2",
            9: "G59.3"}
        try:
            index = int(getattr(status, "g5x_index", 1))
        except:
            index = 1
        return names.get(index, "G54")

    def _active_wcs_index(self, status):
        try:
            index = int(getattr(status, "g5x_index", 1))
            if index >= 1 and index <= 9:
                return index
        except:
            pass
        return 1

    def _get_positions(self, status):
        # PathPilot/LinuxCNC status.position is G53 machine position.
        # Active WCS is machine position minus the active G5X offset.
        # Z also subtracts tool and G92 offsets to match the displayed WCS Z.
        pos = status.position[:3]
        index = self._active_wcs_index(status)
        off = status.g5x_offsets[index][:3]
        try:
            tool_z = float(status.tool_offset[2])
        except:
            tool_z = 0.0
        try:
            g92_z = float(status.g92_offset[2])
        except:
            g92_z = 0.0
        machine_x = float(pos[0])
        machine_y = float(pos[1])
        machine_z = float(pos[2])
        wcs_x = machine_x - float(off[0])
        wcs_y = machine_y - float(off[1])
        wcs_z = machine_z - float(off[2]) - tool_z - g92_z
        return machine_x, machine_y, machine_z, wcs_x, wcs_y, wcs_z

    def _matches_target(self, value, target):
        return abs(float(value) - float(target)) < ZERO_TOLERANCE

    def _format_axis(self, value):
        value = float(value)
        if abs(value) < ZERO_TOLERANCE:
            value = 0.0
        return "%.4f" % value

    def _append_home_position(self, status, machine_x, machine_y, machine_z, wcs_x, wcs_y, wcs_z):
        timestamp = time.strftime("%Y-%m-%d %H:%M:%S")
        wcs_name = self._active_wcs_name(status)
        if REQUIRE_Z_MATCH:
            line = "%s | G53 X%s Y%s Z%s | trigger %s X%s Y%s Z%s | target X%s Y%s Z%s" % (
                timestamp,
                self._format_axis(machine_x),
                self._format_axis(machine_y),
                self._format_axis(machine_z),
                wcs_name,
                self._format_axis(wcs_x),
                self._format_axis(wcs_y),
                self._format_axis(wcs_z),
                self._format_axis(TARGET_X),
                self._format_axis(TARGET_Y),
                self._format_axis(TARGET_Z))
        else:
            line = "%s | G53 X%s Y%s Z%s | trigger %s X%s Y%s | target X%s Y%s" % (
                timestamp,
                self._format_axis(machine_x),
                self._format_axis(machine_y),
                self._format_axis(machine_z),
                wcs_name,
                self._format_axis(wcs_x),
                self._format_axis(wcs_y),
                self._format_axis(TARGET_X),
                self._format_axis(TARGET_Y))
        handle = open(HOME_POS_LOG_PATH, "a")
        try:
            handle.write(line + "\n")
        finally:
            handle.close()

    def _position_matches_target(self, wcs_x, wcs_y, wcs_z):
        x_matches = self._matches_target(wcs_x, TARGET_X)
        y_matches = self._matches_target(wcs_y, TARGET_Y)
        if REQUIRE_Z_MATCH:
            z_matches = self._matches_target(wcs_z, TARGET_Z)
            return x_matches and y_matches and z_matches
        return x_matches and y_matches

    def check_home_position(self):
        try:
            status = self._poll_status()
            if status is None:
                return True
            machine_x, machine_y, machine_z, wcs_x, wcs_y, wcs_z = self._get_positions(status)
            if self._position_matches_target(wcs_x, wcs_y, wcs_z):
                signature = "%s|%s|%s|%s|%s,%s,%s" % (
                    self._active_wcs_name(status),
                    self._format_axis(machine_x),
                    self._format_axis(machine_y),
                    self._format_axis(machine_z),
                    self._format_axis(wcs_x),
                    self._format_axis(wcs_y),
                    self._format_axis(wcs_z))
                if LOG_REPEAT_SAME_POSITION or signature != self._last_logged_signature:
                    self._append_home_position(status, machine_x, machine_y, machine_z, wcs_x, wcs_y, wcs_z)
                    self._last_logged_signature = signature
            else:
                self._last_logged_signature = ""
        except Exception as e:
            self.error_handler.write("[%s] Check error: %s" % (SCRIPT_NAME, str(e)), constants.ALARM_LEVEL_LOW)
        return True

DESCRIPTION_LONG = """<font face="Verdana" size="2"><b>Home Position Logger</b></font>
<p><font face="Verdana" size="2">Checks the active WCS position every 500 ms. When X/Y match the configured target, it appends the current G53 XYZ machine position to /home/operator/gcode/python/home_pos.txt. Set REQUIRE_Z_MATCH = 1 to also require Z to match TARGET_Z.</font></p>"""