#include "LineTool.h"
#include "ViewportWidget.h"
#include "Camera.h"
#include <QMouseEvent>
#include <QKeyEvent>
#include <QPainter>
#include <QOpenGLShaderProgram>
#include <QtMath>

LineTool::LineTool(ViewportWidget* viewport)
    : SketchTool(viewport)
{
}

void LineTool::activate()
{
    SketchTool::activate();
    m_dimensionModes << "length" << "angle";
    m_dimensionPropertyNames["length"] = "dimensionInput";
    m_dimensionPropertyNames["angle"] = "angleInput";

    m_viewport->setProperty("dimensionInput", "");
    m_viewport->setProperty("angleInput", "");
    m_viewport->setProperty("dimensionEditMode", "length");
    m_viewport->setProperty("isChainedLine", false);
}

void LineTool::mousePressEvent(QMouseEvent *event)
{
    gp_Pnt p;
    QString dimInput = m_viewport->property("dimensionInput").toString();
    QString angleInput = m_viewport->property("angleInput").toString();
    bool lengthFromInput = false;
    bool angleFromInput = false;
    double inputLength = 0;
    double inputAngleDegrees = 0;

    if (m_isDefining) {
        if (!dimInput.isEmpty()) {
            bool ok;
            inputLength = dimInput.toDouble(&ok);
            if (ok) lengthFromInput = true;
        }
        if (!angleInput.isEmpty()) {
            bool ok;
            inputAngleDegrees = angleInput.toDouble(&ok);
            if (ok) angleFromInput = true;
        }
    }

    if (m_isDefining && (lengthFromInput || angleFromInput)) {
        QVector3D worldPos;
        QVector3D startPos(m_firstLinePoint.X(), m_firstLinePoint.Y(), m_firstLinePoint.Z());

        if (angleFromInput) {
            QVector3D refDir;
            if (m_viewport->property("isChainedLine").toBool()) {
                refDir = m_viewport->property("previousLineDirection").value<QVector3D>();
            } else {
                if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY || m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) refDir = QVector3D(1, 0, 0);
                else refDir = QVector3D(0, 1, 0);
            }
            QVector3D currentMouseWorldPos = m_viewport->unproject(event->pos(), m_viewport->currentPlane());
            QVector3D mouseVec = currentMouseWorldPos - startPos;
            double mouseAngle;
            if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) mouseAngle = qRadiansToDegrees(atan2(mouseVec.y(), mouseVec.x()));
            else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) mouseAngle = qRadiansToDegrees(atan2(mouseVec.z(), mouseVec.x()));
            else mouseAngle = qRadiansToDegrees(atan2(mouseVec.z(), mouseVec.y()));
            double refAngle;
            if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) refAngle = qRadiansToDegrees(atan2(refDir.y(), refDir.x()));
            else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) refAngle = qRadiansToDegrees(atan2(refDir.z(), refDir.x()));
            else refAngle = qRadiansToDegrees(atan2(refDir.z(), refDir.y()));
            double relativeMouseAngle = mouseAngle - refAngle;
            while (relativeMouseAngle <= -180.0) relativeMouseAngle += 360.0;
            while (relativeMouseAngle > 180.0) relativeMouseAngle -= 360.0;
            double snappedAngle = 0;
            if (relativeMouseAngle >= 0 && relativeMouseAngle < 90) { // Quadrant 1
                snappedAngle = inputAngleDegrees;
            } else if (relativeMouseAngle >= 90 && relativeMouseAngle <= 180) { // Quadrant 2
                snappedAngle = 180.0 - inputAngleDegrees;
            } else if (relativeMouseAngle < -90) { // Quadrant 3
                snappedAngle = -180.0 + inputAngleDegrees;
            } else { // Quadrant 4
                snappedAngle = -inputAngleDegrees;
            }
            double finalAngleRad = qDegreesToRadians(refAngle + snappedAngle);
            QVector3D finalDir;
            if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) finalDir = QVector3D(cos(finalAngleRad), sin(finalAngleRad), 0);
            else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) finalDir = QVector3D(cos(finalAngleRad), 0, sin(finalAngleRad));
            else finalDir = QVector3D(0, cos(finalAngleRad), sin(finalAngleRad));
            double lineLength;
            if (lengthFromInput) lineLength = inputLength;
            else {
                lineLength = QVector3D::dotProduct(mouseVec, finalDir);
                if (lineLength < 0) lineLength = 0;
            }
            worldPos = startPos + lineLength * finalDir;
        } else if (lengthFromInput) {
            QVector3D currentMouseWorldPos = m_viewport->unproject(event->pos(), m_viewport->currentPlane());
            QVector3D dir = (currentMouseWorldPos - startPos);
            if (dir.length() > 1e-6) {
                dir.normalize();
                worldPos = startPos + inputLength * dir;
            } else {
                if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY || m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) worldPos = startPos + QVector3D(inputLength, 0, 0);
                else worldPos = startPos + QVector3D(0, inputLength, 0);
            }
        }
        p.SetCoord(worldPos.x(), worldPos.y(), worldPos.z());
    } else {
        if (m_viewport->isSnappingOrigin()) {
            p.SetCoord(0, 0, 0);
        } else if (m_viewport->isSnappingVertex()) {
            p = m_viewport->snapVertex();
        } else {
            QVector3D worldPos = m_viewport->unproject(event->pos(), m_viewport->currentPlane());
            if (m_viewport->isSnappingHorizontal()) {
                if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) worldPos.setY(m_firstLinePoint.Y());
                else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) worldPos.setZ(m_firstLinePoint.Z());
                else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::YZ) worldPos.setZ(m_firstLinePoint.Z());
            } else if (m_viewport->isSnappingVertical()) {
                if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) worldPos.setX(m_firstLinePoint.X());
                else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) worldPos.setX(m_firstLinePoint.X());
                else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::YZ) worldPos.setY(m_firstLinePoint.Y());
            }
            p.SetCoord(worldPos.x(), worldPos.y(), worldPos.z());
        }
    }

    if (!m_isDefining) {
        m_firstLinePoint = p;
        m_isDefining = true;
        m_viewport->setProperty("dimensionInput", QVariant(""));
        m_viewport->setProperty("angleInput", QVariant(""));
        m_viewport->setProperty("dimensionEditMode", "length");
        m_viewport->setProperty("isChainedLine", false);
    } else {
        QVector3D start(m_firstLinePoint.X(), m_firstLinePoint.Y(), m_firstLinePoint.Z());
        QVector3D end(p.X(), p.Y(), p.Z());
        m_viewport->setProperty("previousLineDirection", QVariant::fromValue((end - start).normalized()));

        m_viewport->addLine(m_firstLinePoint, p);
        m_firstLinePoint = p;
        m_viewport->setProperty("dimensionInput", QVariant(""));
        m_viewport->setProperty("angleInput", QVariant(""));
        m_viewport->setProperty("dimensionEditMode", "length");
        m_viewport->setProperty("isChainedLine", true);
    }
}

void LineTool::mouseMoveEvent(QMouseEvent *event)
{
    bool oldIsSnappingHorizontal = m_viewport->isSnappingHorizontal();
    bool oldIsSnappingVertical = m_viewport->isSnappingVertical();
    m_viewport->setSnappingHorizontal(false);
    m_viewport->setSnappingVertical(false);

    if (m_isDefining && !m_viewport->isSnappingOrigin() && !m_viewport->isSnappingVertex()) {
        QVector3D worldPos = m_viewport->unproject(m_viewport->currentMousePos(), m_viewport->currentPlane());
        QVector3D startPos(m_firstLinePoint.X(), m_firstLinePoint.Y(), m_firstLinePoint.Z());
        QVector3D delta = worldPos - startPos;

        if (delta.length() > 1e-6) {
            const double snapAngleThreshold = qDegreesToRadians(2.0);
            double angle = 0;

            if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) {
                angle = atan2(delta.y(), delta.x());
            } else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) {
                angle = atan2(delta.z(), delta.x());
            } else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::YZ) {
                angle = atan2(delta.z(), delta.y());
            }

            if (qAbs(sin(angle)) < sin(snapAngleThreshold)) {
                m_viewport->setSnappingHorizontal(true);
            } else if (qAbs(cos(angle)) < sin(snapAngleThreshold)) {
                m_viewport->setSnappingVertical(true);
            }
        }
    }

    if (oldIsSnappingHorizontal != m_viewport->isSnappingHorizontal() || oldIsSnappingVertical != m_viewport->isSnappingVertical()) {
        m_viewport->update();
    }
}

void LineTool::finalizeCreation()
{
    QVector3D worldPos;
    QVector3D startPos(m_firstLinePoint.X(), m_firstLinePoint.Y(), m_firstLinePoint.Z());
    // This is duplicated from paintGL to ensure consistent line creation
    QString dimInput = m_viewport->property("dimensionInput").toString();
    QString angleInput = m_viewport->property("angleInput").toString();
    bool lengthFromInput = false;
    bool angleFromInput = false;
    double inputLength = 0;
    double inputAngleDegrees = 0;

    if (!dimInput.isEmpty()) {
        bool ok;
        inputLength = dimInput.toDouble(&ok);
        if (ok) lengthFromInput = true;
    }
    if (!angleInput.isEmpty()) {
        bool ok;
        inputAngleDegrees = angleInput.toDouble(&ok);
        if (ok) angleFromInput = true;
    }

    if (angleFromInput) {
        QVector3D refDir;
        if (m_viewport->property("isChainedLine").toBool()) {
            refDir = m_viewport->property("previousLineDirection").value<QVector3D>();
        } else {
            if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY || m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) refDir = QVector3D(1, 0, 0);
            else refDir = QVector3D(0, 1, 0);
        }
        QVector3D currentMouseWorldPos = m_viewport->unproject(m_viewport->currentMousePos(), m_viewport->currentPlane());
        QVector3D mouseVec = currentMouseWorldPos - startPos;
        double mouseAngle;
        if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) mouseAngle = qRadiansToDegrees(atan2(mouseVec.y(), mouseVec.x()));
        else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) mouseAngle = qRadiansToDegrees(atan2(mouseVec.z(), mouseVec.x()));
        else mouseAngle = qRadiansToDegrees(atan2(mouseVec.z(), mouseVec.y()));
        double refAngle;
        if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) refAngle = qRadiansToDegrees(atan2(refDir.y(), refDir.x()));
        else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) refAngle = qRadiansToDegrees(atan2(refDir.z(), refDir.x()));
        else refAngle = qRadiansToDegrees(atan2(refDir.z(), refDir.y()));
        double relativeMouseAngle = mouseAngle - refAngle;
        while (relativeMouseAngle <= -180.0) relativeMouseAngle += 360.0;
        while (relativeMouseAngle > 180.0) relativeMouseAngle -= 360.0;
        double snappedAngle = 0;
        if (relativeMouseAngle >= 0 && relativeMouseAngle < 90) { // Quadrant 1
            snappedAngle = inputAngleDegrees;
        } else if (relativeMouseAngle >= 90 && relativeMouseAngle <= 180) { // Quadrant 2
            snappedAngle = 180.0 - inputAngleDegrees;
        } else if (relativeMouseAngle < -90) { // Quadrant 3
            snappedAngle = -180.0 + inputAngleDegrees;
        } else { // Quadrant 4
            snappedAngle = -inputAngleDegrees;
        }
        double finalAngleRad = qDegreesToRadians(refAngle + snappedAngle);
        QVector3D finalDir;
        if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) finalDir = QVector3D(cos(finalAngleRad), sin(finalAngleRad), 0);
        else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) finalDir = QVector3D(cos(finalAngleRad), 0, sin(finalAngleRad));
        else finalDir = QVector3D(0, cos(finalAngleRad), sin(finalAngleRad));
        double lineLength;
        if (lengthFromInput) lineLength = inputLength;
        else {
            lineLength = QVector3D::dotProduct(mouseVec, finalDir);
            if (lineLength < 0) lineLength = 0;
        }
        worldPos = startPos + lineLength * finalDir;
    } else if (lengthFromInput) {
        QVector3D currentMouseWorldPos = m_viewport->unproject(m_viewport->currentMousePos(), m_viewport->currentPlane());
        QVector3D dir = (currentMouseWorldPos - startPos);
        if (dir.length() > 1e-6) {
            dir.normalize();
            worldPos = startPos + inputLength * dir;
        } else {
            if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY || m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) worldPos = startPos + QVector3D(inputLength, 0, 0);
            else worldPos = startPos + QVector3D(0, inputLength, 0);
        }
    } else {
        worldPos = m_viewport->unproject(m_viewport->currentMousePos(), m_viewport->currentPlane());
    }

    gp_Pnt p;
    p.SetCoord(worldPos.x(), worldPos.y(), worldPos.z());

    QVector3D prevDir = (worldPos - startPos).normalized();
    m_viewport->setProperty("previousLineDirection", QVariant::fromValue(prevDir));

    m_viewport->addLine(m_firstLinePoint, p);
    m_firstLinePoint = p;
    m_viewport->setProperty("dimensionInput", QVariant(""));
    m_viewport->setProperty("angleInput", QVariant(""));
    m_viewport->setProperty("dimensionEditMode", "length");
    m_viewport->setProperty("isChainedLine", true);
}

void LineTool::paintGL()
{
    if (m_isDefining) {
        QVector<GLfloat> vertices;
        QVector3D worldPos;
        QVector3D startPos(m_firstLinePoint.X(), m_firstLinePoint.Y(), m_firstLinePoint.Z());

        QString dimInput = m_viewport->property("dimensionInput").toString();
        QString angleInput = m_viewport->property("angleInput").toString();
        bool lengthFromInput = false;
        bool angleFromInput = false;
        double inputLength = 0;
        double inputAngleDegrees = 0;

        if (!dimInput.isEmpty()) {
            bool ok;
            inputLength = dimInput.toDouble(&ok);
            if (ok) lengthFromInput = true;
        }
        if (!angleInput.isEmpty()) {
            bool ok;
            inputAngleDegrees = angleInput.toDouble(&ok);
            if (ok) angleFromInput = true;
        }

        if (angleFromInput) {
            QVector3D refDir;
            if (m_viewport->property("isChainedLine").toBool()) {
                refDir = m_viewport->property("previousLineDirection").value<QVector3D>();
            } else {
                if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY || m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) {
                    refDir = QVector3D(1, 0, 0);
                } else { // YZ
                    refDir = QVector3D(0, 1, 0);
                }
            }

            QVector3D currentMouseWorldPos = m_viewport->unproject(m_viewport->currentMousePos(), m_viewport->currentPlane());
            QVector3D mouseVec = currentMouseWorldPos - startPos;

            // Quadrant snapping
            double mouseAngle;
            if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) mouseAngle = qRadiansToDegrees(atan2(mouseVec.y(), mouseVec.x()));
            else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) mouseAngle = qRadiansToDegrees(atan2(mouseVec.z(), mouseVec.x()));
            else mouseAngle = qRadiansToDegrees(atan2(mouseVec.z(), mouseVec.y()));

            double refAngle;
            if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) refAngle = qRadiansToDegrees(atan2(refDir.y(), refDir.x()));
            else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) refAngle = qRadiansToDegrees(atan2(refDir.z(), refDir.x()));
            else refAngle = qRadiansToDegrees(atan2(refDir.z(), refDir.y()));

            double relativeMouseAngle = mouseAngle - refAngle;
            while (relativeMouseAngle <= -180.0) relativeMouseAngle += 360.0;
            while (relativeMouseAngle > 180.0) relativeMouseAngle -= 360.0;

            double snappedAngle = 0;
            if (relativeMouseAngle >= 0 && relativeMouseAngle < 90) { // Quadrant 1
                snappedAngle = inputAngleDegrees;
            } else if (relativeMouseAngle >= 90 && relativeMouseAngle <= 180) { // Quadrant 2
                snappedAngle = 180.0 - inputAngleDegrees;
            } else if (relativeMouseAngle < -90) { // Quadrant 3
                snappedAngle = -180.0 + inputAngleDegrees;
            } else { // Quadrant 4
                snappedAngle = -inputAngleDegrees;
            }

            double finalAngleRad = qDegreesToRadians(refAngle + snappedAngle);
            QVector3D finalDir;
            if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) finalDir = QVector3D(cos(finalAngleRad), sin(finalAngleRad), 0);
            else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) finalDir = QVector3D(cos(finalAngleRad), 0, sin(finalAngleRad));
            else finalDir = QVector3D(0, cos(finalAngleRad), sin(finalAngleRad));

            double lineLength;
            if (lengthFromInput) {
                lineLength = inputLength;
            } else {
                lineLength = QVector3D::dotProduct(mouseVec, finalDir);
                 if (lineLength < 0) lineLength = 0;
            }
            worldPos = startPos + lineLength * finalDir;

        } else if (lengthFromInput) {
            QVector3D currentMouseWorldPos = m_viewport->unproject(m_viewport->currentMousePos(), m_viewport->currentPlane());
            QVector3D dir = (currentMouseWorldPos - startPos);
            if (dir.length() > 1e-6) {
                dir.normalize();
                worldPos = startPos + inputLength * dir;
            } else {
                if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY || m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) {
                    worldPos = startPos + QVector3D(inputLength, 0, 0);
                } else {
                    worldPos = startPos + QVector3D(0, inputLength, 0);
                }
            }
        } else {
            worldPos = m_viewport->unproject(m_viewport->currentMousePos(), m_viewport->currentPlane());
            if (m_viewport->isSnappingOrigin()) {
                worldPos.setX(0); worldPos.setY(0); worldPos.setZ(0);
            } else if (m_viewport->isSnappingVertex()) {
                worldPos.setX(m_viewport->snapVertex().X()); worldPos.setY(m_viewport->snapVertex().Y()); worldPos.setZ(m_viewport->snapVertex().Z());
            } else if (m_viewport->isSnappingHorizontal()) {
                if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) worldPos.setY(m_firstLinePoint.Y());
                else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) worldPos.setZ(m_firstLinePoint.Z());
                else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::YZ) worldPos.setZ(m_firstLinePoint.Z());
            } else if (m_viewport->isSnappingVertical()) {
                if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) worldPos.setX(m_firstLinePoint.X());
                else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) worldPos.setX(m_firstLinePoint.X());
                else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::YZ) worldPos.setY(m_firstLinePoint.Y());
            }
        }

        vertices << m_firstLinePoint.X() << m_firstLinePoint.Y() << m_firstLinePoint.Z();
        vertices << worldPos.x() << worldPos.y() << worldPos.z();

        m_viewport->shaderProgram()->setUniformValue(m_viewport->colorLoc(), QVector4D(1.0f, 1.0f, 0.0f, 1.0f));
        m_viewport->vbo().bind();
        m_viewport->vbo().allocate(vertices.constData(), vertices.size() * sizeof(GLfloat));
        glDrawArrays(GL_LINES, 0, 2);

        // Draw dimension line
        QVector3D lineVec = worldPos - startPos;
        float lineLength = lineVec.length();

        if (lineLength > 1e-6) {
            double refAngle, lineAngle, angleDiff;
            {
                QVector3D refDir;
                if (m_viewport->property("isChainedLine").toBool()) {
                    refDir = m_viewport->property("previousLineDirection").value<QVector3D>();
                } else {
                    if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY || m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) refDir = QVector3D(1, 0, 0);
                    else refDir = QVector3D(0, 1, 0);
                }

                if (angleFromInput) {
                    QVector3D currentMouseWorldPos = m_viewport->unproject(m_viewport->currentMousePos(), m_viewport->currentPlane());
                    QVector3D mouseVec = currentMouseWorldPos - startPos;

                    double mouseAngle;
                    if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) mouseAngle = qRadiansToDegrees(atan2(mouseVec.y(), mouseVec.x()));
                    else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) mouseAngle = qRadiansToDegrees(atan2(mouseVec.z(), mouseVec.x()));
                    else mouseAngle = qRadiansToDegrees(atan2(mouseVec.z(), mouseVec.y()));

                    double refAngleForQuadrant;
                    if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) refAngleForQuadrant = qRadiansToDegrees(atan2(refDir.y(), refDir.x()));
                    else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) refAngleForQuadrant = qRadiansToDegrees(atan2(refDir.z(), refDir.x()));
                    else refAngleForQuadrant = qRadiansToDegrees(atan2(refDir.z(), refDir.y()));

                    double relativeMouseAngle = mouseAngle - refAngleForQuadrant;
                    while (relativeMouseAngle <= -180.0) relativeMouseAngle += 360.0;
                    while (relativeMouseAngle > 180.0) relativeMouseAngle -= 360.0;

                    if (relativeMouseAngle >= 90 || relativeMouseAngle < -90) {
                        refDir = -refDir;
                    }
                } else {
                    if (m_viewport->property("isChainedLine").toBool()) {
                        refDir = -refDir;
                    }
                }

                if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) {
                    refAngle = atan2(refDir.y(), refDir.x());
                    lineAngle = atan2(lineVec.y(), lineVec.x());
                } else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) {
                    refAngle = atan2(refDir.z(), refDir.x());
                    lineAngle = atan2(lineVec.z(), lineVec.x());
                } else { // YZ
                    refAngle = atan2(refDir.z(), refDir.y());
                    lineAngle = atan2(lineVec.z(), lineVec.y());
                }

                angleDiff = lineAngle - refAngle;
                while (angleDiff <= -M_PI) angleDiff += 2 * M_PI;
                while (angleDiff > M_PI) angleDiff -= 2 * M_PI;
                lineAngle = refAngle + angleDiff;
            }

            vertices.clear();

            QVector3D perpVec;
            if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) {
                perpVec = QVector3D(-lineVec.y(), lineVec.x(), 0).normalized();
            } else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) {
                perpVec = QVector3D(-lineVec.z(), 0, lineVec.x()).normalized();
            } else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::YZ) {
                perpVec = QVector3D(0, -lineVec.z(), lineVec.y()).normalized();
            }

            if (angleDiff < 0) {
                perpVec = -perpVec;
            }

            float offset = 0.05f * -m_viewport->camera()->zoom();
            QVector3D dimStart = startPos + offset * perpVec;
            QVector3D dimEnd = worldPos + offset * perpVec;

            vertices << dimStart.x() << dimStart.y() << dimStart.z();
            vertices << dimEnd.x() << dimEnd.y() << dimEnd.z();

            float arrowLength = 0.02f * -m_viewport->camera()->zoom();
            float arrowWidth = 0.005f * -m_viewport->camera()->zoom();
            QVector3D lineDir = lineVec.normalized();

            QVector3D arrow_base_end = dimEnd - arrowLength * lineDir;
            QVector3D arrowP1_end = arrow_base_end + arrowWidth * perpVec;
            QVector3D arrowP2_end = arrow_base_end - arrowWidth * perpVec;
            vertices << dimEnd.x() << dimEnd.y() << dimEnd.z();
            vertices << arrowP1_end.x() << arrowP1_end.y() << arrowP1_end.z();
            vertices << dimEnd.x() << dimEnd.y() << dimEnd.z();
            vertices << arrowP2_end.x() << arrowP2_end.y() << arrowP2_end.z();

            QVector3D arrow_base_start = dimStart + arrowLength * lineDir;
            QVector3D arrowP1_start = arrow_base_start + arrowWidth * perpVec;
            QVector3D arrowP2_start = arrow_base_start - arrowWidth * perpVec;
            vertices << dimStart.x() << dimStart.y() << dimStart.z();
            vertices << arrowP1_start.x() << arrowP1_start.y() << arrowP1_start.z();
            vertices << dimStart.x() << dimStart.y() << dimStart.z();
            vertices << arrowP2_start.x() << arrowP2_start.y() << arrowP2_start.z();

            m_viewport->shaderProgram()->setUniformValue(m_viewport->colorLoc(), QVector4D(0.7f, 0.7f, 0.7f, 1.0f));
            glLineWidth(1.0f);
            m_viewport->vbo().bind();
            m_viewport->vbo().allocate(vertices.constData(), vertices.size() * sizeof(GLfloat));
            glDrawArrays(GL_LINES, 0, vertices.size() / 3);
            glLineWidth(2.0f);

            // Draw angle dimension
            vertices.clear();
            const int numSegments = 30;
            const float radius = 0.1f * -m_viewport->camera()->zoom();
            for (int i = 0; i <= numSegments; ++i) {
                double angle = refAngle + (lineAngle - refAngle) * i / numSegments;
                QVector3D p;
                if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) p = startPos + radius * QVector3D(cos(angle), sin(angle), 0);
                else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) p = startPos + radius * QVector3D(cos(angle), 0, sin(angle));
                else p = startPos + radius * QVector3D(0, cos(angle), sin(angle));
                vertices << p.x() << p.y() << p.z();
            }
            glLineWidth(1.0f);
            m_viewport->vbo().bind();
            m_viewport->vbo().allocate(vertices.constData(), vertices.size() * sizeof(GLfloat));
            glDrawArrays(GL_LINE_STRIP, 0, vertices.size() / 3);

            // Arrowheads for arc
            QVector<GLfloat> arrowVertices;
            float arcArrowLength = 0.02f * -m_viewport->camera()->zoom();
            float arcArrowWidth = 0.005f * -m_viewport->camera()->zoom();

            double sign = (angleDiff >= 0) ? 1.0 : -1.0;

            // End arrowhead
            QVector3D endPoint(vertices[vertices.size()-3], vertices[vertices.size()-2], vertices[vertices.size()-1]);
            double endAngle = lineAngle;
            QVector3D radialDir_end, tangentDir_end;
            if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) {
                radialDir_end = QVector3D(cos(endAngle), sin(endAngle), 0);
                tangentDir_end = QVector3D(-sin(endAngle), cos(endAngle), 0);
            } else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) {
                radialDir_end = QVector3D(cos(endAngle), 0, sin(endAngle));
                tangentDir_end = QVector3D(-sin(endAngle), 0, cos(endAngle));
            } else {
                radialDir_end = QVector3D(0, cos(endAngle), sin(endAngle));
                tangentDir_end = QVector3D(0, -sin(endAngle), cos(endAngle));
            }
            QVector3D arc_arrow_base_end = endPoint - sign * arcArrowLength * tangentDir_end;
            QVector3D arc_arrowP1_end = arc_arrow_base_end + arcArrowWidth * radialDir_end;
            QVector3D arc_arrowP2_end = arc_arrow_base_end - arcArrowWidth * radialDir_end;
            arrowVertices << endPoint.x() << endPoint.y() << endPoint.z() << arc_arrowP1_end.x() << arc_arrowP1_end.y() << arc_arrowP1_end.z();
            arrowVertices << endPoint.x() << endPoint.y() << endPoint.z() << arc_arrowP2_end.x() << arc_arrowP2_end.y() << arc_arrowP2_end.z();

            // Start arrowhead
            QVector3D startPoint(vertices[0], vertices[1], vertices[2]);
            double startAngle = refAngle;
            QVector3D radialDir_start, tangentDir_start;
             if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) {
                radialDir_start = QVector3D(cos(startAngle), sin(startAngle), 0);
                tangentDir_start = QVector3D(-sin(startAngle), cos(startAngle), 0);
            } else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) {
                radialDir_start = QVector3D(cos(startAngle), 0, sin(startAngle));
                tangentDir_start = QVector3D(-sin(startAngle), 0, cos(startAngle));
            } else {
                radialDir_start = QVector3D(0, cos(startAngle), sin(startAngle));
                tangentDir_start = QVector3D(0, -sin(startAngle), cos(startAngle));
            }
            QVector3D arc_arrow_base_start = startPoint + sign * arcArrowLength * tangentDir_start;
            QVector3D arc_arrowP1_start = arc_arrow_base_start + arcArrowWidth * radialDir_start;
            QVector3D arc_arrowP2_start = arc_arrow_base_start - arcArrowWidth * radialDir_start;
            arrowVertices << startPoint.x() << startPoint.y() << startPoint.z() << arc_arrowP1_start.x() << arc_arrowP1_start.y() << arc_arrowP1_start.z();
            arrowVertices << startPoint.x() << startPoint.y() << startPoint.z() << arc_arrowP2_start.x() << arc_arrowP2_start.y() << arc_arrowP2_start.z();

            m_viewport->vbo().bind();
            m_viewport->vbo().allocate(arrowVertices.constData(), arrowVertices.size() * sizeof(GLfloat));
            glDrawArrays(GL_LINES, 0, arrowVertices.size() / 3);
            glLineWidth(2.0f);
        }

        if (!lengthFromInput && !angleFromInput && (m_viewport->isSnappingHorizontal() || m_viewport->isSnappingVertical())) {
            vertices.clear();
            QVector3D startPos(m_firstLinePoint.X(), m_firstLinePoint.Y(), m_firstLinePoint.Z());
            QVector3D midPoint = (startPos + worldPos) / 2.0;
            const float indicatorSize = 0.02f * -m_viewport->camera()->zoom();
            const float indicatorOffset = 0.02f * -m_viewport->camera()->zoom();
            if (m_viewport->isSnappingHorizontal()) {
                if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) {
                    vertices << midPoint.x() - indicatorSize << midPoint.y() + indicatorOffset << midPoint.z();
                    vertices << midPoint.x() + indicatorSize << midPoint.y() + indicatorOffset << midPoint.z();
                } else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) {
                    vertices << midPoint.x() - indicatorSize << midPoint.y() << midPoint.z() + indicatorOffset;
                    vertices << midPoint.x() + indicatorSize << midPoint.y() << midPoint.z() + indicatorOffset;
                } else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::YZ) {
                    vertices << midPoint.x() << midPoint.y() - indicatorSize << midPoint.z() + indicatorOffset;
                    vertices << midPoint.x() << midPoint.y() + indicatorSize << midPoint.z() + indicatorOffset;
                }
            } else { // m_isSnappingVertical
                if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) {
                    vertices << midPoint.x() + indicatorOffset << midPoint.y() - indicatorSize << midPoint.z();
                    vertices << midPoint.x() + indicatorOffset << midPoint.y() + indicatorSize << midPoint.z();
                } else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) {
                    vertices << midPoint.x() + indicatorOffset << midPoint.y() << midPoint.z() - indicatorSize;
                    vertices << midPoint.x() + indicatorOffset << midPoint.y() << midPoint.z() + indicatorSize;
                } else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::YZ) {
                    vertices << midPoint.x() << midPoint.y() + indicatorOffset << midPoint.z() - indicatorSize;
                    vertices << midPoint.x() << midPoint.y() + indicatorOffset << midPoint.z() + indicatorSize;
                }
            }
            m_viewport->vbo().bind();
            m_viewport->vbo().allocate(vertices.constData(), vertices.size() * sizeof(GLfloat));
            glDrawArrays(GL_LINES, 0, 2);
        }
    }
}

void LineTool::paint2D(QPainter& painter, const QMatrix4x4& modelView, const QMatrix4x4& projection)
{
    if (m_isDefining) {
        QVector3D worldPos;
        QVector3D startPos(m_firstLinePoint.X(), m_firstLinePoint.Y(), m_firstLinePoint.Z());
        QString dimText;
        QString angleText;

        QString dimInput = m_viewport->property("dimensionInput").toString();
        QString angleInput = m_viewport->property("angleInput").toString();
        bool lengthFromInput = false;
        bool angleFromInput = false;
        double inputLength = 0;
        double inputAngleDegrees = 0;
        double lineLength = 0;

        if (!dimInput.isEmpty()) {
            bool ok;
            inputLength = dimInput.toDouble(&ok);
            if (ok) lengthFromInput = true;
        }
        if (!angleInput.isEmpty()) {
            bool ok;
            inputAngleDegrees = angleInput.toDouble(&ok);
            if (ok) angleFromInput = true;
        }

        if (angleFromInput) {
            QVector3D refDir;
            if (m_viewport->property("isChainedLine").toBool()) {
                refDir = m_viewport->property("previousLineDirection").value<QVector3D>();
            } else {
                if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY || m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) {
                    refDir = QVector3D(1, 0, 0);
                } else { // YZ
                    refDir = QVector3D(0, 1, 0);
                }
            }

            QVector3D currentMouseWorldPos = m_viewport->unproject(m_viewport->currentMousePos(), m_viewport->currentPlane());
            QVector3D mouseVec = currentMouseWorldPos - startPos;

            double mouseAngle;
            if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) mouseAngle = qRadiansToDegrees(atan2(mouseVec.y(), mouseVec.x()));
            else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) mouseAngle = qRadiansToDegrees(atan2(mouseVec.z(), mouseVec.x()));
            else mouseAngle = qRadiansToDegrees(atan2(mouseVec.z(), mouseVec.y()));

            double refAngle;
            if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) refAngle = qRadiansToDegrees(atan2(refDir.y(), refDir.x()));
            else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) refAngle = qRadiansToDegrees(atan2(refDir.z(), refDir.x()));
            else refAngle = qRadiansToDegrees(atan2(refDir.z(), refDir.y()));

            double relativeMouseAngle = mouseAngle - refAngle;
            while (relativeMouseAngle <= -180.0) relativeMouseAngle += 360.0;
            while (relativeMouseAngle > 180.0) relativeMouseAngle -= 360.0;

            double snappedAngle = 0;
            if (relativeMouseAngle >= 0 && relativeMouseAngle < 90) { // Quadrant 1
                snappedAngle = inputAngleDegrees;
            } else if (relativeMouseAngle >= 90 && relativeMouseAngle <= 180) { // Quadrant 2
                snappedAngle = 180.0 - inputAngleDegrees;
            } else if (relativeMouseAngle < -90) { // Quadrant 3
                snappedAngle = -180.0 + inputAngleDegrees;
            } else { // Quadrant 4
                snappedAngle = -inputAngleDegrees;
            }

            double finalAngleRad = qDegreesToRadians(refAngle + snappedAngle);
            QVector3D finalDir;
            if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) finalDir = QVector3D(cos(finalAngleRad), sin(finalAngleRad), 0);
            else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) finalDir = QVector3D(cos(finalAngleRad), 0, sin(finalAngleRad));
            else finalDir = QVector3D(0, cos(finalAngleRad), sin(finalAngleRad));

            if (lengthFromInput) {
                lineLength = inputLength;
            } else {
                lineLength = QVector3D::dotProduct(mouseVec, finalDir);
                 if (lineLength < 0) lineLength = 0;
            }
            worldPos = startPos + lineLength * finalDir;

        } else if (lengthFromInput) {
            lineLength = inputLength;
            QVector3D currentMouseWorldPos = m_viewport->unproject(m_viewport->currentMousePos(), m_viewport->currentPlane());
            QVector3D dir = (currentMouseWorldPos - startPos);
            if (dir.length() > 1e-6) {
                dir.normalize();
                worldPos = startPos + inputLength * dir;
            } else {
                if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY || m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) {
                    worldPos = startPos + QVector3D(inputLength, 0, 0);
                } else {
                    worldPos = startPos + QVector3D(0, inputLength, 0);
                }
            }
        } else {
            worldPos = m_viewport->unproject(m_viewport->currentMousePos(), m_viewport->currentPlane());
            if (m_viewport->isSnappingOrigin()) {
                worldPos.setX(0); worldPos.setY(0); worldPos.setZ(0);
            } else if (m_viewport->isSnappingVertex()) {
                worldPos.setX(m_viewport->snapVertex().X()); worldPos.setY(m_viewport->snapVertex().Y()); worldPos.setZ(m_viewport->snapVertex().Z());
            } else if (m_viewport->isSnappingHorizontal()) {
                if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) worldPos.setY(m_firstLinePoint.Y());
                else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) worldPos.setZ(m_firstLinePoint.Z());
                else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::YZ) worldPos.setZ(m_firstLinePoint.Z());
            } else if (m_viewport->isSnappingVertical()) {
                if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) worldPos.setX(m_firstLinePoint.X());
                else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) worldPos.setX(m_firstLinePoint.X());
                else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::YZ) worldPos.setY(m_firstLinePoint.Y());
            }
            lineLength = (worldPos - startPos).length();
        }

        QVector3D lineVec = worldPos - startPos;
        if (lineVec.length() > 1e-6) {
            double refAngle, lineAngle;
            QVector3D refDir;
            if (m_viewport->property("isChainedLine").toBool()) {
                refDir = m_viewport->property("previousLineDirection").value<QVector3D>();
            } else {
                if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY || m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) refDir = QVector3D(1, 0, 0);
                else refDir = QVector3D(0, 1, 0);
            }

            QVector3D currentMouseWorldPosForText = m_viewport->unproject(m_viewport->currentMousePos(), m_viewport->currentPlane());
            QVector3D mouseVecForText = currentMouseWorldPosForText - startPos;

            if (angleFromInput) {
                if (mouseVecForText.length() > 1e-6) {
                    double mouseAngle;
                    if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) mouseAngle = qRadiansToDegrees(atan2(mouseVecForText.y(), mouseVecForText.x()));
                    else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) mouseAngle = qRadiansToDegrees(atan2(mouseVecForText.z(), mouseVecForText.x()));
                    else mouseAngle = qRadiansToDegrees(atan2(mouseVecForText.z(), mouseVecForText.y()));

                    double refAngleForQuadrant;
                    if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) refAngleForQuadrant = qRadiansToDegrees(atan2(refDir.y(), refDir.x()));
                    else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) refAngleForQuadrant = qRadiansToDegrees(atan2(refDir.z(), refDir.x()));
                    else refAngleForQuadrant = qRadiansToDegrees(atan2(refDir.z(), refDir.y()));

                    double relativeMouseAngle = mouseAngle - refAngleForQuadrant;
                    while (relativeMouseAngle <= -180.0) relativeMouseAngle += 360.0;
                    while (relativeMouseAngle > 180.0) relativeMouseAngle -= 360.0;

                    if (relativeMouseAngle >= 90 || relativeMouseAngle < -90) {
                        refDir = -refDir;
                    }
                }
            } else {
                if (m_viewport->property("isChainedLine").toBool()) {
                    refDir = -refDir;
                }
            }

            if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) {
                refAngle = atan2(refDir.y(), refDir.x());
                lineAngle = atan2(lineVec.y(), lineVec.x());
            } else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) {
                refAngle = atan2(refDir.z(), refDir.x());
                lineAngle = atan2(lineVec.z(), lineVec.x());
            } else { // YZ
                refAngle = atan2(refDir.z(), refDir.y());
                lineAngle = atan2(lineVec.z(), lineVec.y());
            }

            double angleDiff = lineAngle - refAngle;
            while (angleDiff <= -M_PI) angleDiff += 2 * M_PI;
            while (angleDiff > M_PI) angleDiff -= 2 * M_PI;
            lineAngle = refAngle + angleDiff;

            QVector3D perpVec;
            if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) {
                perpVec = QVector3D(-lineVec.y(), lineVec.x(), 0).normalized();
            } else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) {
                perpVec = QVector3D(-lineVec.z(), 0, lineVec.x()).normalized();
            } else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::YZ) {
                perpVec = QVector3D(0, -lineVec.z(), lineVec.y()).normalized();
            }

            if (angleDiff < 0) {
                perpVec = -perpVec;
            }

            float offset = 0.05f * -m_viewport->camera()->zoom();
            QVector3D dimStart = startPos + offset * perpVec;
            QVector3D dimEnd = worldPos + offset * perpVec;
            QVector3D textPos3D = (dimStart + dimEnd) / 2.0f + 0.015f * -m_viewport->camera()->zoom() * perpVec;

            QVector3D screenPos = m_viewport->project(textPos3D, modelView, projection, m_viewport->rect());

            painter.setRenderHint(QPainter::Antialiasing);
            QFontMetrics fm(painter.font());

            if (screenPos.z() < 1.0f) {
                dimText = lengthFromInput ? dimInput : QString::number(lineLength, 'f', 2);

                QRect textRect = fm.boundingRect(dimText + "_");
                textRect.moveCenter(screenPos.toPoint());

                if (m_viewport->property("dimensionEditMode").toString() == "length") {
                    painter.fillRect(textRect.adjusted(-4, -2, 4, 2), QColor(64, 128, 255));
                } else {
                    painter.fillRect(textRect.adjusted(-4, -2, 4, 2), QColor(50, 50, 50));
                }
                painter.setPen(Qt::white);
                painter.drawText(textRect, Qt::AlignCenter, dimText);
            }

            // Angle dimension text
            double angleDiffDegrees = qRadiansToDegrees(angleDiff);
            while (angleDiffDegrees <= -180.0) angleDiffDegrees += 360.0;
            while (angleDiffDegrees > 180.0) angleDiffDegrees -= 360.0;

            angleText = angleFromInput ? angleInput : QString::number(qAbs(angleDiffDegrees), 'f', 1) + QChar(0x00B0);

            const float radius = 0.1f * -m_viewport->camera()->zoom();
            double midAngle = refAngle + (lineAngle - refAngle) / 2.0;
            QVector3D textPos3DAngle;
            float textOffset = 0.035f * -m_viewport->camera()->zoom();

            if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XY) textPos3DAngle = startPos + (radius + textOffset) * QVector3D(cos(midAngle), sin(midAngle), 0);
            else if (m_viewport->currentPlane() == ViewportWidget::SketchPlane::XZ) textPos3DAngle = startPos + (radius + textOffset) * QVector3D(cos(midAngle), 0, sin(midAngle));
            else textPos3DAngle = startPos + (radius + textOffset) * QVector3D(0, cos(midAngle), sin(midAngle));

            QVector3D screenPosAngle = m_viewport->project(textPos3DAngle, modelView, projection, m_viewport->rect());
            if (screenPosAngle.z() < 1.0f) {
                QRect angleTextRect = fm.boundingRect(angleText + "_");
                angleTextRect.moveCenter(screenPosAngle.toPoint());
                if (m_viewport->property("dimensionEditMode").toString() == "angle") {
                    painter.fillRect(angleTextRect.adjusted(-4, -2, 4, 2), QColor(64, 128, 255));
                } else {
                    painter.fillRect(angleTextRect.adjusted(-4, -2, 4, 2), QColor(50, 50, 50));
                }
                painter.setPen(Qt::white);
                painter.drawText(angleTextRect, Qt::AlignCenter, angleText);
            }
        }
    }
}