import math
import re
from copy import copy
import uuid

import numpy as np
from PySide6.QtWidgets import QApplication, QWidget, QMessageBox, QInputDialog
from PySide6.QtGui import QPainter, QPen, QColor, QTransform
from PySide6.QtCore import Qt, QPoint, QPointF, Signal, QLine
from python_solvespace import SolverSystem, ResultFlag


class SketchWidget(QWidget):
    constrain_done = Signal()

    def __init__(self):
        super().__init__()

        self.line_draw_buffer = [None, None]
        self.drag_buffer = [None, None]
        self.main_buffer = [None, None]
        self.dynamic_line_end = None  # Cursor position for dynamic drawing

        self.hovered_point = None
        self.selected_line = None

        ### Display Settings
        self.snapping_range = 20  # Range in pixels for snapping
        self.zoom = 1

        # Mouse Input
        self.setMouseTracking(True)
        self.mouse_mode = False
        self.is_construct = False

        self.snap_mode = {
            "point": False,
            "mpoint": False,
            "horiz": False,
            "vert":False,
            "grid": False,
            "angle": False
        }

        # Solver
        self.solv = SolverSystem()

        self.sketch = Sketch2d()

    def act_line_mode(self, checked):
        if checked:
            self.mouse_mode = 'line'
            print(self.mouse_mode)
        else:
            self.mouse_mode = None

    def act_constrain_pt_pt_mode(self):
        self.mouse_mode = 'pt_pt'

    def act_constrain_pt_line_mode(self):
        self.mouse_mode = 'pt_line'

    def act_constrain_horiz_line_mode(self):
        self.mouse_mode = 'horiz'

    def act_constrain_vert_line_mode(self):
        self.mouse_mode = 'vert'

    def act_constrain_distance_mode(self):
        self.mouse_mode = 'distance'

    def act_constrain_mid_point_mode(self):
       self.mouse_mode = 'pb_con_mid'

    def on_snap_mode_change(self, helper_type: str, value: bool):
        self.snap_mode[helper_type] = value

    def on_construct_change(self, checked):
        self.is_construct = checked

    def create_sketch(self, sketch_in ):
        self.sketch = Sketch2d()
        self.sketch.id = sketch_in.id
        self.sketch.origin = sketch_in.origin

    def set_sketch(self, sketch_in):
        """Needs to be an already defined Sketch object coming from the widget itself"""
        self.sketch = sketch_in

    def get_sketch(self):
        return self.sketch

    def reset_buffers(self):
        self.mouse_mode = None
        self.line_draw_buffer = [None, None]
        self.drag_buffer = [None, None]
        self.main_buffer = [None, None]

    def set_points(self, points: list):
        self.points = points
        #self.update()

    def create_workplane(self):
        self.sketch.wp = self.sketch.create_2d_base()

    def create_workplane_projected(self):
        self.sketch.wp = self.sketch.create_2d_base()

    def convert_proj_points(self, proj_points: list):
        ### This needs to create a proper Point2D class with bool construction enbaled
        out_points = []
        for point in proj_points:
            pnt = Point2D(point[0], point[1])
            # Construction
            pnt.is_helper = True
            print(point)
            self.sketch.add_point(pnt)

    def convert_proj_lines(self, proj_lines: list):
        ### same as for point
        out_lines = []
        for line in proj_lines:
            start = Point2D(line[0][0], line[0][1])
            end = Point2D(line[1][0], line[1][1])
            start.is_helper = True
            end.is_helper = True

            self.sketch.add_point(start)
            self.sketch.add_point(end)

            lne = Line2D(start, end)

            #Construction
            lne.is_helper = True
            self.sketch.add_line(lne)

    def find_duplicate_points_2d(self, edges):
        points = []
        seen = set()
        duplicates = []

        for edge in edges:
            for point in edge:
                # Extract only x and y coordinates
                point_2d = (point[0], point[1])
                if point_2d in seen:
                    if point_2d not in duplicates:
                        duplicates.append(point_2d)
                else:
                    seen.add(point_2d)
                points.append(point_2d)

        return duplicates

    def normal_to_quaternion(self, normal):
        normal = np.array(normal)
        #normal = normal / np.linalg.norm(normal)

        axis = np.cross([0, 0, 1], normal)
        if np.allclose(axis, 0):
            axis = np.array([1, 0, 0])
        else:
            axis = axis / np.linalg.norm(axis)  # Normalize the axis

        angle = np.arccos(np.dot([0, 0, 1], normal))

        qw = np.cos(angle / 2)
        sin_half_angle = np.sin(angle / 2)
        qx, qy, qz = axis * sin_half_angle  # This will now work correctly

        return qw, qx, qy, qz

    def create_workplane_space(self, points, normal):
        print("edges", points)
        origin = self.find_duplicate_points_2d(points)
        print(origin)
        x, y = origin[0]
        origin = QPoint(x, y)

        origin_handle = self.get_handle_from_ui_point(origin)
        qw, qx, qy, qz = self.normal_to_quaternion(normal)

        slv_normal = self.sketch.add_normal_3d(qw, qx, qy, qz)
        self.sketch.wp = self.sketch.add_work_plane(origin_handle, slv_normal)
        print(self.sketch.wp)

    def get_handle_nr(self, input_str: str) -> int:
        # Define the regex pattern to extract the handle number
        pattern = r"handle=(\d+)"

        # Use re.search to find the handle number in the string
        match = re.search(pattern, input_str)

        if match:
            handle_number = int(match.group(1))
            print(f"Handle number: {handle_number}")
            return int(handle_number)

        else:
            print("Handle number not found.")
            return 0

    def get_keys(self, d: dict, target: QPoint) -> list:
        result = []
        path = []
        print(d)
        print(target)
        for k, v in d.items():
            path.append(k)
            if isinstance(v, dict):
                self.get_keys(v, target)
            if v == target:
                result.append(copy(path))
            path.pop()

        return result

    def get_handle_from_ui_point(self, ui_point: QPoint):
        """Input QPoint and you shall reveive a slvs entity handle!"""
        for point in self.sketch.points:
            if ui_point == point.ui_point:
                slv_handle = point.handle

                return slv_handle

    def get_line_handle_from_ui_point(self, ui_point: QPoint):
        """Input Qpoint that is on a line and you shall receive the handle of the line!"""
        for target_line_con in self.sketch.lines:
            if self.is_point_on_line(ui_point, target_line_con.crd1.ui_point, target_line_con.crd2.ui_point):
                slv_handle = target_line_con.handle

                return slv_handle

    def get_point_line_handles_from_ui_point(self, ui_point: QPoint) -> tuple:
        """Input Qpoint that is on a line and you shall receive the handles of the points of the line!"""
        for target_line_con in self.sketch.lines:
            if self.is_point_on_line(ui_point, target_line_con.crd1.ui_point, target_line_con.crd2.ui_point):
                lines_to_cons = target_line_con.crd1.handle, target_line_con.crd2.handle

                return lines_to_cons

    def distance(self, p1, p2):
        return math.sqrt((p1.x() - p2.x())**2 + (p1.y() - p2.y())**2)

    def calculate_midpoint(self, point1, point2):
        mx = (point1.x() + point2.x()) // 2
        my = (point1.y() + point2.y()) // 2
        return QPoint(mx, my)

    def is_point_on_line(self, p, p1, p2, tolerance=5):
        # Calculate the lengths of the sides of the triangle
        a = self.distance(p, p1)
        b = self.distance(p, p2)
        c = self.distance(p1, p2)

        # Calculate the semi-perimeter
        s = (a + b + c) / 2

        # Calculate the area using Heron's formula
        area = math.sqrt(s * (s - a) * (s - b) * (s - c))

        # Calculate the height (perpendicular distance from the point to the line)
        if c > 0:
            height = (2 * area) / c
            # Check if the height is within the tolerance distance to the line
            if height > tolerance:
                return False

            # Check if the projection of the point onto the line is within the line segment
            dot_product = ((p.x() - p1.x()) * (p2.x() - p1.x()) + (p.y() - p1.y()) * (p2.y() - p1.y())) / (c ** 2)

            return 0 <= dot_product <= 1
        else:
            return None

    def viewport_to_local_coord(self, qt_pos : QPoint) -> QPoint:
        return QPoint(self.to_quadrant_coords(qt_pos))

    def check_all_points(self) -> list:
        """
        Go through solversystem and check points2d for changes in position after solving
        :return: List with points that now have a different position
        """

        old_points_ui = []
        new_points_ui = []

        for index, old_point_ui in enumerate(self.sketch.points):
            old_points_ui.append((index, old_point_ui.ui_point))

        for i in range(self.sketch.entity_len()):
            # Iterate though full length because mixed list from SS
            entity = self.sketch.entity(i)
            if entity.is_point_2d() and self.sketch.params(entity.params):
                x_tbu, y_tbu = self.sketch.params(entity.params)
                point_solved = QPoint(x_tbu, y_tbu)
                new_points_ui.append(point_solved)

        # Now we have old_points_ui and new_points_ui, let's compare them
        differences = []

        if len(old_points_ui) != len(new_points_ui):
            print(f"Length mismatch {len(old_points_ui)} - {len(new_points_ui)}")

        for (old_index, old_point), new_point in zip(old_points_ui, new_points_ui):
            if old_point != new_point:
                #print(old_point)
                differences.append((old_index, old_point, new_point))

        return differences

    def update_ui_points(self, point_list: list):
        # Print initial state of slv_points_main
        # print("Initial slv_points_main:", self.slv_points_main)
        print("Change list:", point_list)

        """for point in point_list:
            new = Point2D(point.x(), point.y())
            self.sketch.points.append(new)"""

        if len(point_list) > 1:
            for tbu_points_idx in point_list:
                # Each tbu_points_idx is a tuple: (index, old_point, new_point)
                index, old_point, new_point = tbu_points_idx

                # Update the point in slv_points_main
                self.sketch.points[index].ui_point = new_point

        """for points in self.sketch.points:
            print(points.ui_point)"""
            # Print updated state
            # print("Updated slv_points_main:", self.slv_points_main)

    def check_all_lines_and_update(self,changed_points: list):
        for tbu_points_idx in changed_points:
            index, old_point, new_point = tbu_points_idx
            for line_needs_update in self.sketch.lines:
                if old_point == line_needs_update.crd1.ui_point:
                    line_needs_update.crd1.ui_point = new_point
                elif old_point == line_needs_update.crd2.ui_point:
                    line_needs_update.crd2.ui_point = new_point

    def mouseReleaseEvent(self, event):
        local_event_pos = self.viewport_to_local_coord(event.pos())

        if event.button() == Qt.LeftButton and not self.mouse_mode:
            self.drag_buffer[1] = local_event_pos

            #print("Le main buffer", self.drag_buffer)

            if not None in self.main_buffer and len(self.main_buffer) == 2:
                entry = self.drag_buffer[0]
                new_params = self.drag_buffer[1].x(), self.drag_buffer[1].y()
                self.sketch.set_params(entry.params, new_params)

                self.sketch.solve()

                #points_need_update = self.check_all_points()
                #self.update_ui_points(points_need_update)
                #self.check_all_lines_and_update(points_need_update)

                self.update()
                self.drag_buffer = [None, None]

    def mousePressEvent(self, event):
        local_event_pos = self.viewport_to_local_coord(event.pos())


        if event.button() == Qt.LeftButton and not self.mouse_mode:
            self.drag_buffer[0] = self.get_handle_from_ui_point(self.hovered_point)

        if event.button() == Qt.RightButton and self.mouse_mode:
            self.constrain_done.emit()
            self.reset_buffers()

        if event.button() == Qt.LeftButton and self.mouse_mode == "line":
            if self.hovered_point:
                clicked_pos = self.hovered_point
            else:
                clicked_pos = local_event_pos

            if not self.line_draw_buffer[0]:

                u = clicked_pos.x()
                v = clicked_pos.y()

                point = Point2D(u,v)
                print("construct", self.is_construct )
                # Construction mode
                point.is_helper = self.is_construct

                self.sketch.add_point(point)

                self.line_draw_buffer[0] = point

            elif self.line_draw_buffer[0]:

                u = clicked_pos.x()
                v = clicked_pos.y()
                print("construct", self.is_construct)
                point = Point2D(u, v)

                # Construction mode
                point.is_helper = self.is_construct

                self.sketch.add_point(point)

                self.line_draw_buffer[1] = point

            #print("Buffer state", self.line_draw_buffer)

            if self.line_draw_buffer[0] and self.line_draw_buffer[1]:

                line = Line2D(self.line_draw_buffer[0], self.line_draw_buffer[1])

                # Construction mode
                line.is_helper = self.is_construct

                self.sketch.add_line(line)

                # Reset the buffer for the next line segment
                self.line_draw_buffer[0] = self.line_draw_buffer[1]
                self.line_draw_buffer[1] = None

            # Track Relationship
            # Points

        # CONSTRAINTS

        if event.button() == Qt.LeftButton and self.mouse_mode == "pt_pt":
            if self.hovered_point and not self.main_buffer[0]:
                self.main_buffer[0] = self.get_handle_from_ui_point(self.hovered_point)

            elif self.main_buffer[0]:
                self.main_buffer[1] = self.get_handle_from_ui_point(self.hovered_point)

            if self.main_buffer[0] and self.main_buffer[1]:
               # print("buf", self.main_buffer)

                self.sketch.coincident(self.main_buffer[0], self.main_buffer[1], self.sketch.wp)

                if self.sketch.solve() == ResultFlag.OKAY:
                    print("Fuck yeah")

                elif self.sketch.solve() == ResultFlag.DIDNT_CONVERGE:
                    print("Solve_failed - Converge")

                elif self.sketch.solve() == ResultFlag.TOO_MANY_UNKNOWNS:
                    print("Solve_failed - Unknowns")

                elif self.sketch.solve() == ResultFlag.INCONSISTENT:
                    print("Solve_failed - Incons")

                self.constrain_done.emit()
                self.main_buffer = [None, None]

        if event.button() == Qt.LeftButton and self.mouse_mode == "pt_line":
            #print("ptline")
            line_selected = None

            if self.hovered_point and not self.main_buffer[1]:
                self.main_buffer[0] = self.get_handle_from_ui_point(self.hovered_point)

            elif self.main_buffer[0]:
                self.main_buffer[1] = self.get_line_handle_from_ui_point(local_event_pos)

                # Contrain point to line
                if self.main_buffer[1]:
                    self.sketch.coincident(self.main_buffer[0], self.main_buffer[1], self.sketch.wp)

                    if self.sketch.solve() == ResultFlag.OKAY:
                        print("Fuck yeah")
                        self.constrain_done.emit()

                    elif self.sketch.solve() == ResultFlag.DIDNT_CONVERGE:
                        print("Solve_failed - Converge")

                    elif self.sketch.solve() == ResultFlag.TOO_MANY_UNKNOWNS:
                        print("Solve_failed - Unknowns")

                    elif self.sketch.solve() == ResultFlag.INCONSISTENT:
                        print("Solve_failed - Incons")

                    self.constrain_done.emit()
                    # Clear saved_points after solve attempt
                    self.main_buffer = [None, None]

        if event.button() == Qt.LeftButton and self.mouse_mode == "pb_con_mid":
            #print("ptline")
            line_selected = None


            if self.hovered_point and not self.main_buffer[1]:
                self.main_buffer[0] = self.get_handle_from_ui_point(self.hovered_point)

            elif self.main_buffer[0]:
                self.main_buffer[1] = self.get_line_handle_from_ui_point(local_event_pos)

                # Contrain point to line
                if self.main_buffer[1]:
                    self.sketch.midpoint(self.main_buffer[0], self.main_buffer[1], self.sketch.wp)

                    if self.sketch.solve() == ResultFlag.OKAY:
                        print("Fuck yeah")
                        for idx, line in enumerate(self.sketch.lines):
                            # print(line.crd1.ui_point)
                            if self.is_point_on_line(local_event_pos, line.crd1.ui_point, line.crd2.ui_point):
                                self.sketch.lines[idx].constraints.append("mid")
                                print(self.sketch.lines[idx].constraints)


                    elif self.sketch.solve() == ResultFlag.DIDNT_CONVERGE:
                        print("Solve_failed - Converge")

                    elif self.sketch.solve() == ResultFlag.TOO_MANY_UNKNOWNS:
                        print("Solve_failed - Unknowns")

                    elif self.sketch.solve() == ResultFlag.INCONSISTENT:
                        print("Solve_failed - Incons")
                    self.constrain_done.emit()

                    self.main_buffer = [None, None]

        if event.button() == Qt.LeftButton and self.mouse_mode == "horiz":

            line_selected = self.get_line_handle_from_ui_point(local_event_pos)



            if line_selected:
                self.sketch.horizontal(line_selected, self.sketch.wp)

            if self.sketch.solve() == ResultFlag.OKAY:
                print("Fuck yeah")

                # Add succesful constraint to constrain draw list so it gets drawn in paint function
                for idx, line in enumerate(self.sketch.lines):
                    #print(line.crd1.ui_point)
                    if self.is_point_on_line(local_event_pos, line.crd1.ui_point, line.crd2.ui_point):
                        self.sketch.lines[idx].constraints.append("hrz")
                        #print(self.sketch.lines[idx].constraints)

            elif self.sketch.solve() == ResultFlag.DIDNT_CONVERGE:
                print("Solve_failed - Converge")

            elif self.sketch.solve() == ResultFlag.TOO_MANY_UNKNOWNS:
                print("Solve_failed - Unknowns")

            elif self.sketch.solve() == ResultFlag.INCONSISTENT:
                print("Solve_failed - Incons")

        if event.button() == Qt.LeftButton and self.mouse_mode == "vert":
            line_selected = self.get_line_handle_from_ui_point(local_event_pos)

            if line_selected:
                self.sketch.vertical(line_selected, self.sketch.wp)

                if self.sketch.solve() == ResultFlag.OKAY:
                    print("Fuck yeah")
                    for idx, line in enumerate(self.sketch.lines):
                        # print(line.crd1.ui_point)
                        if self.is_point_on_line(local_event_pos, line.crd1.ui_point, line.crd2.ui_point):
                            self.sketch.lines[idx].constraints.append("vrt")
                        # print(self.sketch.lines[idx].constraints)

                elif self.sketch.solve() == ResultFlag.DIDNT_CONVERGE:
                    print("Solve_failed - Converge")

                elif self.sketch.solve() == ResultFlag.TOO_MANY_UNKNOWNS:
                    print("Solve_failed - Unknowns")

                elif self.sketch.solve() == ResultFlag.INCONSISTENT:
                    print("Solve_failed - Incons")

        if event.button() == Qt.LeftButton and self.mouse_mode == "distance":
            # Depending on selected elemnts either point line or line distance
            #print("distance")
            e1 = None
            e2 = None

            if self.hovered_point:
               # print("buf point")
                # Get the point as UI point as buffer
                self.main_buffer[0] = self.hovered_point

            elif self.selected_line:
                # Get the point as UI point as buffer
                self.main_buffer[1] = local_event_pos

            if self.main_buffer[0] and self.main_buffer[1]:
                # Define point line combination
                e1 = self.get_handle_from_ui_point(self.main_buffer[0])
                e2 = self.get_line_handle_from_ui_point(self.main_buffer[1])

            elif not self.main_buffer[0]:
                # Define only line selection
                e1, e2 = self.get_point_line_handles_from_ui_point(local_event_pos)

            if e1 and e2:
                # Ask fo the dimension and solve if both elements are present
                length, ok = QInputDialog.getDouble(self, 'Distance', 'Enter a mm value:', value=100, decimals=2)
                self.sketch.distance(e1, e2, length, self.sketch.wp)

                if self.sketch.solve() == ResultFlag.OKAY:
                    print("Fuck yeah")
                    for idx, line in enumerate(self.sketch.lines):
                        # print(line.crd1.ui_point)
                        if self.is_point_on_line(local_event_pos, line.crd1.ui_point, line.crd2.ui_point):
                            if "dist" not in self.sketch.lines[idx].constraints:
                                self.sketch.lines[idx].constraints.append("dst")

                elif self.sketch.solve() == ResultFlag.DIDNT_CONVERGE:
                    print("Solve_failed - Converge")

                elif self.sketch.solve() == ResultFlag.TOO_MANY_UNKNOWNS:
                    print("Solve_failed - Unknowns")

                elif self.sketch.solve() == ResultFlag.INCONSISTENT:
                    print("Solve_failed - Incons")

                self.constrain_done.emit()
                self.main_buffer = [None, None]

        # Update the main point list with the new elements and draw them
        points_need_update = self.check_all_points()
        self.update_ui_points(points_need_update)
        self.check_all_lines_and_update(points_need_update)



        self.update()

    def mouseMoveEvent(self, event):
        local_event_pos = self.viewport_to_local_coord(event.pos())
        #print(local_event_pos)

        closest_point = None
        min_distance = float('inf')
        threshold = 15  # Distance threshold for highlighting

        if self.mouse_mode == "line" and self.line_draw_buffer[0]:
            # Update the current cursor position as the second point
            self.dynamic_line_end = self.viewport_to_local_coord(event.pos())
            self.update()  # Trigger a repaint

        if self.sketch.points is not None and len(self.sketch.points) > 0:
            for point in self.sketch.points:
                distance = (local_event_pos - point.ui_point).manhattanLength()
                if distance < threshold and distance < min_distance:
                    closest_point = point.ui_point
                    min_distance = distance

            """for point in self.sketch.proj_points:
                distance = (local_event_pos - point).manhattanLength()
                if distance < threshold and distance < min_distance:
                    closest_point = point
                    min_distance = distance"""

            if closest_point != self.hovered_point:
                self.hovered_point = closest_point
                #print(self.hovered_point)

            for line in self.sketch.lines:
                p1 = line.crd1.ui_point
                p2 = line.crd2.ui_point

                if self.is_point_on_line(local_event_pos, p1, p2):
                    self.selected_line = p1, p2

                    if self.snap_mode.get("mpoint"):
                        # Midpointsnap only in drawer not solver
                        mid = self.calculate_midpoint(p1, p2)
                        distance = (local_event_pos - mid).manhattanLength()
                        if distance < threshold and distance < min_distance:
                            self.hovered_point = mid
                        break

                    break
                else:
                    self.selected_line = None

            self.update()

    def mouseDoubleClickEvent(self, event):
        pass

    def drawBackgroundGrid(self, painter):
        """Draw a background grid."""
        grid_spacing = 50
        pen = QPen(QColor(200, 200, 200), 1, Qt.SolidLine)
        painter.setPen(pen)

        # Draw vertical grid lines
        for x in range(-self.width() // 2, self.width() // 2, grid_spacing):
            painter.drawLine(x, -self.height() // 2, x, self.height() // 2)

        # Draw horizontal grid lines
        for y in range(-self.height() // 2, self.height() // 2, grid_spacing):
            painter.drawLine(-self.width() // 2, y, self.width() // 2, y)

    def drawAxes(self, painter):
        painter.setRenderHint(QPainter.Antialiasing)

        # Set up pen for dashed lines
        pen = QPen(Qt.gray, 1, Qt.DashLine)
        painter.setPen(pen)

        middle_x = self.width() // 2
        middle_y = self.height() // 2

        # Draw X axis as dashed line
        painter.drawLine(0, middle_y, self.width(), middle_y)

        # Draw Y axis as dashed line
        painter.drawLine(middle_x, 0, middle_x, self.height())

        # Draw tick marks
        tick_length = int(10  * self.zoom)
        tick_spacing = int(50 * self.zoom)

        pen = QPen(Qt.gray, 1, Qt.SolidLine)
        painter.setPen(pen)

        # Draw tick marks on the X axis to the right and left from the middle point
        for x in range(0, self.width() // 2, tick_spacing):
            painter.drawLine(middle_x + x, middle_y - tick_length // 2, middle_x + x, middle_y + tick_length // 2)
            painter.drawLine(middle_x - x, middle_y - tick_length // 2, middle_x - x, middle_y + tick_length // 2)

        # Draw tick marks on the Y axis upwards and downwards from the middle point
        for y in range(0, self.height() // 2, tick_spacing):
            painter.drawLine(middle_x - tick_length // 2, middle_y + y, middle_x + tick_length // 2, middle_y + y)
            painter.drawLine(middle_x - tick_length // 2, middle_y - y, middle_x + tick_length // 2, middle_y - y)

        # Draw the origin point in red
        painter.setPen(QPen(Qt.red, 4))
        painter.drawPoint(middle_x, middle_y)

    def draw_cross(self, painter, pos: QPoint, size=10):
        # Set up the pen
        pen = QPen(QColor('green'))  # You can change the color as needed
        pen.setWidth(int(2 / self.zoom))  # Set the line widt)h
        painter.setPen(pen)
        x = pos.x()
        y = pos.y()

        # Calculate the endpoints of the cross
        half_size = size // 2

        # Draw the horizontal line
        painter.drawLine(x - half_size, y, x + half_size, y)

        # Draw the vertical line
        painter.drawLine(x, y - half_size, x, y + half_size)

    def to_quadrant_coords(self, point):
        """Translate linear coordinates to quadrant coordinates."""
        center_x = self.width() // 2
        center_y = self.height() // 2
        quadrant_x = point.x() - center_x
        quadrant_y = center_y - point.y()  # Note the change here
        return QPoint(quadrant_x, quadrant_y) / self.zoom

    def from_quadrant_coords(self, point: QPoint):
        """Translate quadrant coordinates to linear coordinates."""
        center_x = self.width() // 2
        center_y = self.height() // 2
        widget_x = center_x + point.x() * self.zoom
        widget_y = center_y - point.y() * self.zoom  # Note the subtraction here
        
        return QPoint(int(widget_x), int(widget_y))

    def from_quadrant_coords_no_center(self, point):
        """Invert Y Coordinate for mesh"""
        center_x = 0
        center_y = 0
        widget_x = point.x()
        widget_y = -point.y()
        return QPoint(int(widget_x), int(widget_y))

    def draw_measurement(self,painter, start_point, end_point):

        pen_normal = QPen(Qt.gray)
        pen_normal.setWidthF(2 / self.zoom)

        pen_planned = QPen(Qt.gray)
        pen_planned.setStyle(Qt.PenStyle.DotLine)
        pen_planned.setWidthF(2 / self.zoom)

        pen_construct = QPen(Qt.cyan)
        pen_construct.setStyle(Qt.PenStyle.DotLine)
        pen_construct.setWidthF(1 / self.zoom)

        pen_solver = QPen(Qt.green)
        pen_solver.setWidthF(2 / self.zoom)

        pen_text = QPen(Qt.white)
        pen_text.setWidthF(1 / self.zoom)

        # Calculate the direction of the line
        dx = end_point.x() - start_point.x()
        dy = end_point.y() - start_point.y()

        # Swap and negate to get a perpendicular vector
        perp_dx = -dy
        perp_dy = dx

        # Normalize the perpendicular vector
        length = (perp_dx ** 2 + perp_dy ** 2) ** 0.5
        if length == 0:  # Prevent division by zero
            return
        perp_dx /= length
        perp_dy /= length

        # Fixed length for the perpendicular lines
        fixed_length = 40  # Adjust as needed
        half_length = fixed_length  # fixed_length / 2

        # Calculate endpoints for the perpendicular line at the start
        start_perp_start_x = start_point.x() + perp_dx
        start_perp_start_y = start_point.y() + perp_dy

        start_perp_end_x = start_point.x() + perp_dx * half_length * (1 / self.zoom)
        start_perp_end_y = start_point.y() + perp_dy * half_length * (1 / self.zoom)

        start_perp_end_x_conn_line = start_point.x() + perp_dx * half_length * 0.75 * (1 / self.zoom)
        start_perp_end_y_conn_line = start_point.y() + perp_dy * half_length * 0.75 * (1 / self.zoom)

        # Draw the perpendicular line at the start
        painter.setPen(pen_construct)  # Different color for the perpendicular line
        painter.drawLine(
            QPointF(start_perp_start_x, start_perp_start_y),
            QPointF(start_perp_end_x, start_perp_end_y)
        )

        # Calculate endpoints for the perpendicular line at the end
        end_perp_start_x = end_point.x() + perp_dx
        end_perp_start_y = end_point.y() + perp_dy

        end_perp_end_x = end_point.x() + perp_dx * half_length * (1 / self.zoom)
        end_perp_end_y = end_point.y() + perp_dy * half_length * (1 / self.zoom)

        end_perp_end_x_conn_line = end_point.x() + perp_dx * half_length * .75 * (1 / self.zoom)
        end_perp_end_y_conn_line = end_point.y() + perp_dy * half_length * .75 * (1 / self.zoom)

        # Draw the perpendicular line at the end
        painter.drawLine(
            QPointF(end_perp_start_x, end_perp_start_y),
            QPointF(end_perp_end_x, end_perp_end_y)
        )

        painter.drawLine(
            QPointF(end_perp_end_x_conn_line, end_perp_end_y_conn_line),
            QPointF(start_perp_end_x_conn_line, start_perp_end_y_conn_line)
        )

        # Save painter state
        painter.save()
        painter.setPen(pen_text)

        # Calculate the distance and midpoint
        dis = self.distance(start_point, end_point)
        mid = self.calculate_midpoint(QPointF(start_perp_end_x_conn_line, start_perp_end_y_conn_line),
                                      QPointF(end_perp_end_x_conn_line, end_perp_end_y_conn_line))

        # mid = self.calculate_midpoint(start_point, end_point)

        # Transform for text
        painter.translate(mid.x(), mid.y())  # Move to the midpoint
        painter.scale(1, -1)  # Flip y-axis back to make text readable

        # Draw the text
        painter.drawText(0, 0, str(round(dis, 2)))  # Draw text at transformed position

        # Restore painter state
        painter.restore()

    def paintEvent(self, event):
        painter = QPainter(self)
        painter.setRenderHint(QPainter.Antialiasing)

        self.drawAxes(painter)

        # Create a QTransform object
        transform = QTransform()

        # Translate the origin to the center of the widget
        center = QPointF(self.width() / 2, self.height() / 2)
        transform.translate(center.x(), center.y())

        # Apply the zoom factor
        transform.scale(self.zoom, -self.zoom)  # Negative y-scale to invert y-axis

        # Set the transform to the painter
        painter.setTransform(transform)

        pen_normal = QPen(Qt.gray)
        pen_normal.setWidthF(2 / self.zoom)

        pen_planned = QPen(Qt.gray)
        pen_planned.setStyle(Qt.PenStyle.DotLine)
        pen_planned.setWidthF(2 / self.zoom)

        pen_construct = QPen(Qt.cyan)
        pen_construct.setStyle(Qt.PenStyle.DotLine)
        pen_construct.setWidthF(1 / self.zoom)

        pen_solver = QPen(Qt.green)
        pen_solver.setWidthF(2 / self.zoom)

        pen_text = QPen(Qt.white)
        pen_text.setWidthF(1 / self.zoom)

        # Draw points and lines
        if self.sketch:
            painter.setPen(pen_normal)
            for point in self.sketch.points:
                if point.is_helper:
                    painter.setPen(pen_construct)
                    painter.drawEllipse(point.ui_point, 10 / self.zoom, 10 / self.zoom)
                else:
                    # Normal point
                    painter.setPen(pen_normal)
                    painter.drawEllipse(point.ui_point, 3 / self.zoom, 3 / self.zoom)

                # Draw the dynamic line
            if self.mouse_mode == "line" and self.line_draw_buffer[0] and self.dynamic_line_end is not None:
                start_point = self.line_draw_buffer[0].ui_point
                end_point = self.dynamic_line_end
                painter.setPen(pen_planned)  # Use a different color for the dynamic line
                painter.drawLine(start_point, end_point)

                self.draw_measurement(painter, start_point, end_point)

            for line in self.sketch.lines:
                if line.is_helper:
                    painter.setPen(pen_construct)
                    p1 = line.crd1.ui_point
                    p2 = line.crd2.ui_point
                    painter.drawLine(p1, p2)
                else:
                    painter.setPen(pen_normal)
                    p1 = line.crd1.ui_point
                    p2 = line.crd2.ui_point
                    painter.drawLine(p1, p2)

                    if not self.selected_line:

                        painter.save()
                        midp = self.calculate_midpoint(p1, p2)
                        painter.translate(midp)
                        painter.scale(1, -1)

                        for i, text in enumerate(line.constraints):
                            painter.drawText(0, i * 15, f"> {text} <")
                        painter.restore()

            # Draw all solver points
            if self.sketch.entity_len():
                painter.setPen(pen_solver)
                for i in range(self.sketch.entity_len()):
                    entity = self.sketch.entity(i)
                    if entity.is_point_2d() and self.sketch.params(entity.params):
                        x, y = self.sketch.params(entity.params)
                        point = QPointF(x, y)
                        painter.drawEllipse(point, 6 / self.zoom, 6 / self.zoom)

            # Highlight point hovered
            if self.hovered_point:
                highlight_pen = QPen(QColor(255, 0, 0))
                highlight_pen.setWidthF(2 / self.zoom)
                painter.setPen(highlight_pen)
                painter.drawEllipse(self.hovered_point, 5 / self.zoom, 5 / self.zoom)

            # Highlight line hovered
            if self.selected_line and not self.hovered_point:
                p1, p2 = self.selected_line
                painter.setPen(QPen(Qt.red, 2 / self.zoom))
                painter.drawLine(p1, p2)

                self.draw_measurement(painter, p1, p2)


            """for cross in self.sketch.proj_points:
                self.draw_cross(painter, cross, 10 / self.zoom)

            for selected in self.sketch.proj_lines:
                pen = QPen(Qt.white, 1, Qt.DashLine)
                painter.setPen(pen)
                painter.drawLine(selected)"""

        painter.end()

    def wheelEvent(self, event):
        delta = event.angleDelta().y()
        self.zoom += (delta / 200) * 0.1
        self.update()
    
    def aspect_ratio(self):
        return self.width() / self.height() * (1.0 / abs(self.zoom))


### GEOMETRY CLASSES
class Point2D:
    def __init__(self, x, y):
        self.id = None
        self.ui_x: int = x
        self.ui_y: int = y
        self.ui_point = QPoint(self.ui_x, self.ui_y)
        self.handle = None
        self.handle_nr: int = None

        # Construction Geometry
        self.is_helper: bool = False

class Line2D:
    def __init__(self, point_s: Point2D, point_e: Point2D):
        self.id = None

        self.crd1: Point2D = point_s
        self.crd2: Point2D = point_e
        self.handle = None
        self.handle_nr: int = None

        # Construction Geometry
        self.is_helper: bool = False
        # String list with applied constraints
        self.constraints: list = []


class Sketch2d(SolverSystem):
    """
    Primary class for internal drawing based on the SolveSpace libray
    """

    def __init__(self):
        self.id = uuid.uuid1()

        self.wp = self.create_2d_base()
        self.points = []
        self.lines = []
        self.origin = [0,0,0]

    def add_point(self, point: Point2D):
        """
        Adds a point into the solversystem and returns the handle.
        Appends the added point to the points list.
        :param point: 2D point in Point2D class format
        :return:
        """

        point.handle = self.add_point_2d(point.ui_x, point.ui_y, self.wp)
        point.handle_nr = self.get_handle_nr(str(point.handle))
        point.id = uuid.uuid1()

        self.points.append(point)

    def add_line(self, line: Line2D):
        """
        Adds a line into the solversystem and returns the handle.
        Appends the added line to the line list.
        :param line:
        :param point: 2D point in Point2D class format
        :return:
        """

        line.id = uuid.uuid1()

        line.handle = self.add_line_2d(line.crd1.handle, line.crd2.handle, self.wp)
        line.handle_nr = self.get_handle_nr(str(line.handle))

        self.lines.append(line)

    ### HELPER AND TOOLS
    def get_handle_nr(self, input_str: str) -> int:
        # Define the regex pattern to extract the handle number
        pattern = r"handle=(\d+)"

        # Use re.search to find the handle number in the string
        match = re.search(pattern, input_str)

        if match:
            handle_number = int(match.group(1))
            print(f"Handle number: {handle_number}")
            return int(handle_number)

        else:
            print("Handle number not found.")
            return 0

if __name__ == "__main__":
    import sys

    app = QApplication(sys.argv)
    window = SketchWidget()
    window.setWindowTitle("Snap Line Widget")
    window.resize(800, 600)
    window.show()
    sys.exit(app.exec())