#include "measurement_widget.h"
#include <QPainter>
#include <QMouseEvent>
#include <QWheelEvent>
#include <QtMath>
#include <QDebug>
#include <QPolygonF>
#include <QTransform>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgproc.hpp>
#include <QCoreApplication>
#include <QThread>

MeasurementWidget::MeasurementWidget(QWidget *parent)
    : QOpenGLWidget(parent)
{
    setMouseTracking(true);
}

MeasurementWidget::~MeasurementWidget()
{
    makeCurrent();
    if (m_texture) {
        delete m_texture;
        m_texture = nullptr;
    }
    doneCurrent();
}

void MeasurementWidget::initializeGL()
{
    initializeOpenGLFunctions();
    glClearColor(0.15f, 0.15f, 0.18f, 1.0f);
}

void MeasurementWidget::resizeGL(int w, int h)
{
    glViewport(0, 0, w, h);
}

bool MeasurementWidget::loadImage(const QString &filePath)
{
    QImage img(filePath);
    if (img.isNull()) return false;

    setImage(img);
    return true;
}

void MeasurementWidget::setImage(const QImage &image)
{
    m_image = image.convertToFormat(QImage::Format_RGBA8888);
    m_textureNeedsUpdate = true;

    // Reset zoom and center image
    m_zoomLevel = 1.0;
    m_panOffset = QPointF(0, 0);

    // Initialize single measurement/calibration line
    int w = m_image.width();
    int h = m_image.height();

    m_head = QPointF(w * 0.3, h * 0.5);
    m_tail = QPointF(w * 0.7, h * 0.5);
    m_hasLine = true;

    // Reset calibration parameters
    m_mmPerPixel = 0.0;
    m_isCalibrated = false;
    m_hasHomography = false;
    m_calibrationMode = false;
    m_activeCornerIdx = -1;
    m_calibCorners.clear();

    emit imageLoaded(w, h);
    emit calibrationUpdated(m_mmPerPixel);
    emit measurementUpdated(measuredPx(), measuredMm());

    update();
}



double MeasurementWidget::measuredPx() const
{
    if (!m_hasLine) return 0.0;
    return QLineF(m_head, m_tail).length();
}

double MeasurementWidget::measuredMm() const
{
    if (m_hasHomography) {
        QPointF h = m_head;
        QPointF t = m_tail;

        if (m_hasLensCalibration) {
            std::vector<cv::Point2f> src = { cv::Point2f(m_head.x(), m_head.y()), cv::Point2f(m_tail.x(), m_tail.y()) };
            std::vector<cv::Point2f> dst;
            cv::undistortPoints(src, dst, m_cameraMatrix, m_distCoeffs, cv::Mat(), m_cameraMatrix);
            h = QPointF(dst[0].x, dst[0].y);
            t = QPointF(dst[1].x, dst[1].y);
        }

        QPointF headReal = m_homography.map(h);
        QPointF tailReal = m_homography.map(t);
        return QLineF(headReal, tailReal).length();
    }
    return measuredPx() * m_mmPerPixel;
}

void MeasurementWidget::resetLine()
{
    if (!m_image.isNull()) {
        int w = m_image.width();
        int h = m_image.height();
        m_head = QPointF(w * 0.3, h * 0.5);
        m_tail = QPointF(w * 0.7, h * 0.5);
        m_hasLine = true;
        emit measurementUpdated(measuredPx(), measuredMm());
        update();
    }
}

void MeasurementWidget::setLinePoints(const QPointF &head, const QPointF &tail)
{
    m_head = head;
    m_tail = tail;
    m_hasLine = true;
    emit measurementUpdated(measuredPx(), measuredMm());
    update();
}

void MeasurementWidget::paintGL()
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    if (m_image.isNull()) return;

    makeCurrent();

    if (m_textureNeedsUpdate) {
        if (m_texture) delete m_texture;
        m_texture = new QOpenGLTexture(m_image);
        m_texture->setMinificationFilter(QOpenGLTexture::Linear);
        m_texture->setMagnificationFilter(QOpenGLTexture::Linear);
        m_textureNeedsUpdate = false;
    }

    // Render 2D image via QPainter over OpenGL viewport
    QPainter painter(this);
    painter.setRenderHint(QPainter::Antialiasing, true);
    painter.setRenderHint(QPainter::TextAntialiasing, true);
    painter.setRenderHint(QPainter::SmoothPixmapTransform, true);

    // Calculate image destination rectangle with zoom and pan
    double widgetAspect = (double)width() / height();
    double imageAspect = (double)m_image.width() / m_image.height();

    double drawW, drawH;
    if (widgetAspect > imageAspect) {
        drawH = height() * m_zoomLevel;
        drawW = drawH * imageAspect;
    } else {
        drawW = width() * m_zoomLevel;
        drawH = drawW / imageAspect;
    }

    double drawX = (width() - drawW) / 2.0 + m_panOffset.x();
    double drawY = (height() - drawH) / 2.0 + m_panOffset.y();

    QRectF targetRect(drawX, drawY, drawW, drawH);
    painter.drawImage(targetRect, m_image);

    // Draw the single measurement/calibration line
    if (m_hasLine && !m_calibrationMode) {
        QColor color(255, 45, 45); // Smooth Vivid Red
        drawLineOverlay(painter, m_head, m_tail, color);
    }

    if (m_calibrationMode) {
        // Draw the 4 corners and connecting lines
        QPolygonF wPoly;
        for (const auto &p : m_calibCorners) {
            wPoly << imageToWidgetCoords(p);
        }

        // Draw lines connecting them to form a quad
        QPen polyPen(QColor(0, 191, 255), 2.0, Qt::DashLine); // Deep Sky Blue
        painter.setPen(polyPen);
        painter.setBrush(Qt::NoBrush);
        if (wPoly.size() == 4) {
            painter.drawPolygon(wPoly);
        }

        // Draw handles at each corner
        double handleRadius = 8.0;
        painter.setPen(QPen(QColor(0, 191, 255), 2.0));
        QStringList labels = {"1 (Sup-Izq)", "2 (Sup-Der)", "3 (Inf-Der)", "4 (Inf-Izq)"};
        for (int i = 0; i < wPoly.size(); ++i) {
            if (i == m_activeCornerIdx) {
                painter.setBrush(QBrush(QColor(255, 165, 0, 200))); // Orange when dragging
            } else {
                painter.setBrush(QBrush(QColor(0, 191, 255, 150)));
            }
            painter.drawEllipse(wPoly[i], handleRadius, handleRadius);

            // Draw label
            painter.setFont(QFont("Arial", 8, QFont::Bold));
            painter.setPen(Qt::white);
            painter.drawText(wPoly[i] + QPointF(12, 4), labels[i]);
            painter.setPen(QPen(QColor(0, 191, 255), 2.0)); // Restore pen
        }
    }
}

void MeasurementWidget::drawLineOverlay(QPainter &painter, const QPointF &imgHead, const QPointF &imgTail,
                                        const QColor &color)
{
    QPointF wHead = imageToWidgetCoords(imgHead);
    QPointF wTail = imageToWidgetCoords(imgTail);

    painter.setRenderHint(QPainter::Antialiasing, true);

    // Pure single red line (1.0px thin stroke with antialiasing)
    QPen linePen(color, 1.0, Qt::SolidLine, Qt::RoundCap);
    painter.setPen(linePen);
    painter.drawLine(wHead, wTail);

    // Compact drag handles (short perpendicular ticks)
    QLineF line(wHead, wTail);
    double dx = line.dx();
    double dy = line.dy();
    double len = line.length();
    if (len > 0.0) {
        double px = -dy / len;
        double py = dx / len;
        double tickHalfLen = 6.0; // short length perpendicular to measurement line

        QPointF headP1 = wHead + QPointF(px * tickHalfLen, py * tickHalfLen);
        QPointF headP2 = wHead - QPointF(px * tickHalfLen, py * tickHalfLen);
        painter.drawLine(headP1, headP2);

        QPointF tailP1 = wTail + QPointF(px * tickHalfLen, py * tickHalfLen);
        QPointF tailP2 = wTail - QPointF(px * tickHalfLen, py * tickHalfLen);
        painter.drawLine(tailP1, tailP2);
    }

    // End labels
    painter.setFont(QFont("Arial", 8, QFont::Bold));
    painter.setPen(Qt::white);
    painter.drawText(wHead + QPointF(10, -6), "A");
    painter.drawText(wTail + QPointF(10, -6), "B");
}

QPointF MeasurementWidget::widgetToImageCoords(const QPointF &widgetPos) const
{
    if (m_image.isNull()) return QPointF();

    double widgetAspect = (double)width() / height();
    double imageAspect = (double)m_image.width() / m_image.height();

    double drawW, drawH;
    if (widgetAspect > imageAspect) {
        drawH = height() * m_zoomLevel;
        drawW = drawH * imageAspect;
    } else {
        drawW = width() * m_zoomLevel;
        drawH = drawW / imageAspect;
    }

    double drawX = (width() - drawW) / 2.0 + m_panOffset.x();
    double drawY = (height() - drawH) / 2.0 + m_panOffset.y();

    double normX = (widgetPos.x() - drawX) / drawW;
    double normY = (widgetPos.y() - drawY) / drawH;

    return QPointF(normX * m_image.width(), normY * m_image.height());
}

QPointF MeasurementWidget::imageToWidgetCoords(const QPointF &imgPos) const
{
    if (m_image.isNull()) return QPointF();

    double widgetAspect = (double)width() / height();
    double imageAspect = (double)m_image.width() / m_image.height();

    double drawW, drawH;
    if (widgetAspect > imageAspect) {
        drawH = height() * m_zoomLevel;
        drawW = drawH * imageAspect;
    } else {
        drawW = width() * m_zoomLevel;
        drawH = drawW / imageAspect;
    }

    double drawX = (width() - drawW) / 2.0 + m_panOffset.x();
    double drawY = (height() - drawH) / 2.0 + m_panOffset.y();

    double normX = imgPos.x() / m_image.width();
    double normY = imgPos.y() / m_image.height();

    return QPointF(drawX + normX * drawW, drawY + normY * drawH);
}

MeasurementWidget::DragHandle MeasurementWidget::hitTestLine(const QPointF &imgPos, const QPointF &head, const QPointF &tail) const
{
    QPointF wPos = imageToWidgetCoords(imgPos);
    QPointF wHead = imageToWidgetCoords(head);
    QPointF wTail = imageToWidgetCoords(tail);

    const double handleRadius = 15.0; // Hit test tolerance in pixels

    if (QLineF(wPos, wHead).length() <= handleRadius) return DragHandle::Head;
    if (QLineF(wPos, wTail).length() <= handleRadius) return DragHandle::Tail;

    // Check if clicked near line body
    QLineF line(wHead, wTail);
    double lineLen = line.length();
    if (lineLen > 0) {
        double u = ((wPos.x() - wHead.x()) * (wTail.x() - wHead.x()) + (wPos.y() - wHead.y()) * (wTail.y() - wHead.y())) / (lineLen * lineLen);
        if (u >= 0.0 && u <= 1.0) {
            QPointF proj = wHead + u * (wTail - wHead);
            if (QLineF(wPos, proj).length() <= handleRadius) {
                return DragHandle::Body;
            }
        }
    }

    return DragHandle::None;
}

void MeasurementWidget::mousePressEvent(QMouseEvent *event)
{
    if (m_image.isNull()) return;

    if (m_calibrationMode) {
        if (event->button() == Qt::LeftButton) {
            QPointF imgPos = widgetToImageCoords(event->position());
            QPointF wPos = event->position();
            m_activeCornerIdx = -1;
            const double handleRadius = 15.0;
            for (int i = 0; i < m_calibCorners.size(); ++i) {
                QPointF wCorner = imageToWidgetCoords(m_calibCorners[i]);
                if (QLineF(wPos, wCorner).length() <= handleRadius) {
                    m_activeCornerIdx = i;
                    m_dragStartImgPos = imgPos;
                    update();
                    break;
                }
            }
            if (m_activeCornerIdx == -1) {
                m_isPanning = true;
                m_lastMousePos = event->pos();
                setCursor(Qt::ClosedHandCursor);
            }
            return;
        }
        if (event->button() == Qt::RightButton) {
            m_isPanning = true;
            m_lastMousePos = event->pos();
            setCursor(Qt::ClosedHandCursor);
            return;
        }
        return;
    }

    if (event->button() == Qt::LeftButton) {
        QPointF imgPos = widgetToImageCoords(event->position());
        m_activeHandle = hitTestLine(imgPos, m_head, m_tail);
        if (m_activeHandle != DragHandle::None) {
            m_dragStartImgPos = imgPos;
            m_dragStartHead = m_head;
            m_dragStartTail = m_tail;
            update();
        } else {
            // Clicked background -> Pan image on left click drag
            m_isPanning = true;
            m_lastMousePos = event->pos();
            setCursor(Qt::ClosedHandCursor);
        }
        return;
    }

    if (event->button() == Qt::RightButton) {
        m_isPanning = true;
        m_lastMousePos = event->pos();
        setCursor(Qt::ClosedHandCursor);
        return;
    }
}

void MeasurementWidget::mouseMoveEvent(QMouseEvent *event)
{
    if (m_isPanning) {
        QPoint delta = event->pos() - m_lastMousePos;
        m_panOffset += QPointF(delta.x(), delta.y());
        m_lastMousePos = event->pos();
        update();
        return;
    }

    QPointF imgPos = widgetToImageCoords(event->position());

    if (m_calibrationMode) {
        if (m_activeCornerIdx != -1) {
            double x = qBound(0.0, imgPos.x(), (double)m_image.width());
            double y = qBound(0.0, imgPos.y(), (double)m_image.height());
            m_calibCorners[m_activeCornerIdx] = QPointF(x, y);
            update();
            return;
        }
        // Update cursor on hover
        QPointF wPos = event->position();
        bool overCorner = false;
        const double handleRadius = 15.0;
        for (int i = 0; i < m_calibCorners.size(); ++i) {
            QPointF wCorner = imageToWidgetCoords(m_calibCorners[i]);
            if (QLineF(wPos, wCorner).length() <= handleRadius) {
                overCorner = true;
                break;
            }
        }
        if (overCorner) {
            setCursor(Qt::PointingHandCursor);
        } else {
            setCursor(Qt::OpenHandCursor);
        }
        return;
    }

    if (m_activeHandle != DragHandle::None) {
        if (m_activeHandle == DragHandle::Head) {
            m_head = imgPos;
        } else if (m_activeHandle == DragHandle::Tail) {
            m_tail = imgPos;
        } else if (m_activeHandle == DragHandle::Body) {
            QPointF delta = imgPos - m_dragStartImgPos;
            m_head = m_dragStartHead + delta;
            m_tail = m_dragStartTail + delta;
        }
        emit measurementUpdated(measuredPx(), measuredMm());
        update();
        return;
    }

    // Update cursor feedback based on hover
    DragHandle hover = hitTestLine(imgPos, m_head, m_tail);
    if (hover == DragHandle::Head || hover == DragHandle::Tail) {
        setCursor(Qt::CrossCursor);
    } else if (hover == DragHandle::Body) {
        setCursor(Qt::SizeAllCursor);
    } else {
        setCursor(Qt::OpenHandCursor);
    }
}

void MeasurementWidget::mouseReleaseEvent(QMouseEvent *event)
{
    if (m_calibrationMode) {
        if (m_isPanning && (event->button() == Qt::LeftButton || event->button() == Qt::RightButton)) {
            m_isPanning = false;
            setCursor(Qt::OpenHandCursor);
        }
        if (event->button() == Qt::LeftButton && m_activeCornerIdx != -1) {
            m_activeCornerIdx = -1;
            setCursor(Qt::OpenHandCursor);
        }
        return;
    }

    if (m_isPanning && (event->button() == Qt::LeftButton || event->button() == Qt::RightButton)) {
        m_isPanning = false;
        QPointF imgPos = widgetToImageCoords(event->position());
        DragHandle hover = hitTestLine(imgPos, m_head, m_tail);
        if (hover == DragHandle::Head || hover == DragHandle::Tail) setCursor(Qt::CrossCursor);
        else if (hover == DragHandle::Body) setCursor(Qt::SizeAllCursor);
        else setCursor(Qt::OpenHandCursor);
    }

    if (event->button() == Qt::LeftButton && m_activeHandle != DragHandle::None) {
        m_activeHandle = DragHandle::None;
        QPointF imgPos = widgetToImageCoords(event->position());
        DragHandle hover = hitTestLine(imgPos, m_head, m_tail);
        if (hover == DragHandle::Head || hover == DragHandle::Tail) setCursor(Qt::CrossCursor);
        else if (hover == DragHandle::Body) setCursor(Qt::SizeAllCursor);
        else setCursor(Qt::OpenHandCursor);
    }
}

void MeasurementWidget::wheelEvent(QWheelEvent *event)
{
    double zoomFactor = 1.15;
    if (event->angleDelta().y() < 0) {
        zoomFactor = 1.0 / zoomFactor;
    }

    double newZoom = m_zoomLevel * zoomFactor;
    if (newZoom < 0.2) newZoom = 0.2;
    if (newZoom > 25.0) newZoom = 25.0;

    QPointF mousePos = event->position();
    QPointF imgPosBefore = widgetToImageCoords(mousePos);

    m_zoomLevel = newZoom;

    QPointF widgetPosAfter = imageToWidgetCoords(imgPosBefore);
    m_panOffset += (mousePos - widgetPosAfter);

    update();
}

void MeasurementWidget::setCalibrationMode(bool active)
{
    if (m_image.isNull()) return;
    m_calibrationMode = active;
    if (active && m_calibCorners.isEmpty()) {
        double w = m_image.width();
        double h = m_image.height();
        m_calibCorners.clear();
        m_calibCorners.append(QPointF(w * 0.1, h * 0.1));
        m_calibCorners.append(QPointF(w * 0.9, h * 0.1));
        m_calibCorners.append(QPointF(w * 0.9, h * 0.9));
        m_calibCorners.append(QPointF(w * 0.1, h * 0.9));
    }
    update();
}

void MeasurementWidget::cancelCalibrationMode()
{
    m_calibrationMode = false;
    m_activeCornerIdx = -1;
    update();
}

void MeasurementWidget::applyPerspectiveCalibration(double realX, double realY)
{
    if (m_image.isNull() || m_calibCorners.size() != 4) return;

    QPolygonF srcQuad;
    srcQuad << m_calibCorners[0] << m_calibCorners[1] << m_calibCorners[2] << m_calibCorners[3];

    QPolygonF destQuad;
    destQuad << QPointF(0, 0) << QPointF(realX, 0) << QPointF(realX, realY) << QPointF(0, realY);

    if (QTransform::quadToQuad(srcQuad, destQuad, m_homography)) {
        m_hasHomography = true;
        m_isCalibrated = true;

        m_calibrationMode = false;
        m_activeCornerIdx = -1;

        // Calculate a representative/average scale factor for compatibility
        double imgW = QLineF(m_calibCorners[0], m_calibCorners[1]).length();
        if (imgW > 0) {
            m_mmPerPixel = realX / imgW;
        }

        emit calibrationUpdated(m_mmPerPixel);
        emit measurementUpdated(measuredPx(), measuredMm());
        update();
    }
}

void MeasurementWidget::resetCalibration()
{
    m_mmPerPixel = 0.0;
    m_isCalibrated = false;
    m_hasHomography = false;
    m_hasLensCalibration = false;
    m_calibrationMode = false;
    m_activeCornerIdx = -1;
    m_calibCorners.clear();

    emit calibrationUpdated(m_mmPerPixel);
    emit measurementUpdated(measuredPx(), measuredMm());
    update();
}

bool MeasurementWidget::calibrateWithChessboard(int &innerCornersX, int &innerCornersY, double squareSizeMm)
{
    if (m_image.isNull()) return false;

    // Convert QImage to cv::Mat
    cv::Mat mat(m_image.height(), m_image.width(), CV_8UC4, 
                const_cast<uchar*>(m_image.bits()), 
                m_image.bytesPerLine());
    
    cv::Mat gray;
    cv::cvtColor(mat, gray, cv::COLOR_RGBA2GRAY);

    // If the image is very high resolution, resize it for the initial corner detection search
    // to reduce high-frequency printer noise/banding and speed up detection significantly (using 600px width).
    cv::Mat graySearch = gray;
    double scale = 1.0;
    if (gray.cols > 600) {
        scale = 600.0 / gray.cols;
        int newHeight = static_cast<int>(gray.rows * scale);
        cv::resize(gray, graySearch, cv::Size(600, newHeight), 0, 0, cv::INTER_AREA);
    }

    std::vector<cv::Point2f> searchCorners;
    bool found = false;
    int bestX = 0;
    int bestY = 0;

    if (innerCornersX > 0 && innerCornersY > 0) {
        // Try user specified dimensions
        cv::Size patternSize(innerCornersX, innerCornersY);
        found = cv::findChessboardCorners(graySearch, patternSize, searchCorners,
                                               cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE);
        if (!found) {
            cv::Mat graySearchInv = 255 - graySearch;
            found = cv::findChessboardCorners(graySearchInv, patternSize, searchCorners,
                                              cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE);
        }
        if (found) {
            bestX = innerCornersX;
            bestY = innerCornersY;
        }
    } else {
        // Auto-detect size: scan common sizes from largest to smallest (10 down to 4)
        int totalSteps = (10 - 4 + 1) * (10 - 4 + 1);
        int step = 0;
        for (int cx = 10; cx >= 4; --cx) {
            for (int cy = 10; cy >= 4; --cy) {
                step++;
                emit calibrationProgress(step * 100 / totalSteps);
                QCoreApplication::processEvents();
                QThread::msleep(8); // Tiny sleep for smooth visual progress bar

                cv::Size patternSize(cx, cy);
                found = cv::findChessboardCorners(graySearch, patternSize, searchCorners,
                                                       cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE);
                if (!found) {
                    cv::Mat graySearchInv = 255 - graySearch;
                    found = cv::findChessboardCorners(graySearchInv, patternSize, searchCorners,
                                                      cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE);
                }
                if (found) {
                    bestX = cx;
                    bestY = cy;
                    emit calibrationProgress(100);
                    QCoreApplication::processEvents();
                    break;
                }
            }
            if (found) break;
        }
    }

    if (!found) {
        return false;
    }

    // Assign detected dimensions back to reference variables to update UI
    innerCornersX = bestX;
    innerCornersY = bestY;

    // Scale corner coordinates back to the original image resolution
    std::vector<cv::Point2f> corners;
    for (const auto &p : searchCorners) {
        corners.push_back(cv::Point2f(p.x / scale, p.y / scale));
    }

    // Refine corner positions on the original high-res image
    cv::cornerSubPix(gray, corners, cv::Size(11, 11), cv::Size(-1, -1),
                     cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.1));

    // Prepare calibration points
    std::vector<std::vector<cv::Point3f>> objectPoints;
    std::vector<std::vector<cv::Point2f>> imagePoints;

    std::vector<cv::Point3f> obj;
    for (int i = 0; i < innerCornersY; ++i) {
        for (int j = 0; j < innerCornersX; ++j) {
            obj.push_back(cv::Point3f(j * squareSizeMm, i * squareSizeMm, 0.0f));
        }
    }

    objectPoints.push_back(obj);
    imagePoints.push_back(corners);

    // Calibrate camera
    m_cameraMatrix = cv::Mat::eye(3, 3, CV_64F);
    m_distCoeffs = cv::Mat::zeros(5, 1, CV_64F);
    std::vector<cv::Mat> rvecs, tvecs;

    double rms = cv::calibrateCamera(objectPoints, imagePoints,
                                     cv::Size(m_image.width(), m_image.height()),
                                     m_cameraMatrix, m_distCoeffs, rvecs, tvecs,
                                     cv::CALIB_FIX_K3);

    qDebug() << "Calibration RMS error:" << rms;

    // Extreme corners:
    // Top-Left (index 0) -> maps to (0, 0)
    // Top-Right (index innerCornersX - 1) -> maps to ((innerCornersX-1)*sq, 0)
    // Bottom-Right (index innerCornersX * innerCornersY - 1) -> maps to ((innerCornersX-1)*sq, (innerCornersY-1)*sq)
    // Bottom-Left (index innerCornersX * (innerCornersY - 1)) -> maps to (0, (innerCornersY-1)*sq)
    
    std::vector<cv::Point2f> srcPointsDistorted = {
        corners[0],
        corners[innerCornersX - 1],
        corners[innerCornersX * innerCornersY - 1],
        corners[innerCornersX * (innerCornersY - 1)]
    };

    // Undistort these 4 points
    std::vector<cv::Point2f> srcPointsUndistorted;
    cv::undistortPoints(srcPointsDistorted, srcPointsUndistorted, m_cameraMatrix, m_distCoeffs, cv::Mat(), m_cameraMatrix);

    QPolygonF srcQuad;
    srcQuad << QPointF(srcPointsUndistorted[0].x, srcPointsUndistorted[0].y)
           << QPointF(srcPointsUndistorted[1].x, srcPointsUndistorted[1].y)
           << QPointF(srcPointsUndistorted[2].x, srcPointsUndistorted[2].y)
           << QPointF(srcPointsUndistorted[3].x, srcPointsUndistorted[3].y);

    double realX = (innerCornersX - 1) * squareSizeMm;
    double realY = (innerCornersY - 1) * squareSizeMm;

    QPolygonF destQuad;
    destQuad << QPointF(0, 0) << QPointF(realX, 0) << QPointF(realX, realY) << QPointF(0, realY);

    if (QTransform::quadToQuad(srcQuad, destQuad, m_homography)) {
        m_hasHomography = true;
        m_hasLensCalibration = true;
        m_isCalibrated = true;

        m_calibrationMode = false;
        m_activeCornerIdx = -1;

        // Compute average scale
        double imgW = QLineF(srcQuad[0], srcQuad[1]).length();
        if (imgW > 0) {
            m_mmPerPixel = realX / imgW;
        }

        emit calibrationUpdated(m_mmPerPixel);
        emit measurementUpdated(measuredPx(), measuredMm());
        update();
        return true;
    }

    return false;
}