#include <math.h>
#include <stdbool.h>

// 求解从平面1到平面2的单应矩阵H, 将结果存入dst
// 求解失败(例如存在三点共线)则返回false
// c的前两列为4个特征点在平面1上的坐标，后两列为在平面2上的坐标
bool homography_compute(float dst[3][3], const float c[4][4]) {
    // 因为H有8个自由度，所以可以用四组坐标点建立一个8元一次线性方程组
    // clang-format off
    float A[] = {
        c[0][0], c[0][1], 1,       0,       0, 0, -c[0][0]*c[0][2], -c[0][1]*c[0][2], c[0][2],
              0,       0, 0, c[0][0], c[0][1], 1, -c[0][0]*c[0][3], -c[0][1]*c[0][3], c[0][3],
        c[1][0], c[1][1], 1,       0,       0, 0, -c[1][0]*c[1][2], -c[1][1]*c[1][2], c[1][2],
              0,       0, 0, c[1][0], c[1][1], 1, -c[1][0]*c[1][3], -c[1][1]*c[1][3], c[1][3],
        c[2][0], c[2][1], 1,       0,       0, 0, -c[2][0]*c[2][2], -c[2][1]*c[2][2], c[2][2],
              0,       0, 0, c[2][0], c[2][1], 1, -c[2][0]*c[2][3], -c[2][1]*c[2][3], c[2][3],
        c[3][0], c[3][1], 1,       0,       0, 0, -c[3][0]*c[3][2], -c[3][1]*c[3][2], c[3][2],
              0,       0, 0, c[3][0], c[3][1], 1, -c[3][0]*c[3][3], -c[3][1]*c[3][3], c[3][3],
    };
    // clang-format on

    // vvv 高斯消元法 vvv
    // Eliminate.
    for (int col = 0; col < 8; col++) {
        // Find best row to swap with.
        float max_val = 0;
        int max_val_idx = -1;
        for (int row = col; row < 8; row++) {
            float val = fabsf(A[row * 9 + col]);
            if (val > max_val) {
                max_val = val;
                max_val_idx = row;
            }
        }

        // Matrix is singular
        if (max_val < 1e-10) return false;

        // Swap to get best row.
        if (max_val_idx != col) {
            for (int i = col; i < 9; i++) {
                float tmp = A[col * 9 + i];
                A[col * 9 + i] = A[max_val_idx * 9 + i];
                A[max_val_idx * 9 + i] = tmp;
            }
        }

        // Do eliminate.
        for (int i = col + 1; i < 8; i++) {
            float f = A[i * 9 + col] / A[col * 9 + col];
            A[i * 9 + col] = 0;
            for (int j = col + 1; j < 9; j++) A[i * 9 + j] -= f * A[col * 9 + j];
        }
    }

    // Back solve.
    for (int col = 7; col >= 0; col--) {
        float sum = 0;
        for (int i = col + 1; i < 8; i++) {
            sum += A[col * 9 + i] * A[i * 9 + 8];
        }
        A[col * 9 + 8] = (A[col * 9 + 8] - sum) / A[col * 9 + col];
    }
    // ^^^ 高斯消元法 ^^^

    dst[0][0] = A[8], dst[0][1] = A[17], dst[0][2] = A[26];
    dst[1][0] = A[35], dst[1][1] = A[44], dst[1][2] = A[53];
    dst[2][0] = A[62], dst[2][1] = A[71], dst[2][2] = 1;

    return true;
}

void homography_project(const float H[3][3], float x, float y, float* ox, float* oy) {
    float xx = H[0][0] * x + H[0][1] * y + H[0][2];
    float yy = H[1][0] * x + H[1][1] * y + H[1][2];
    float zz = H[2][0] * x + H[2][1] * y + H[2][2];
    *ox = xx / zz;
    *oy = yy / zz;
}

int main() {
    // vvv 初始化 vvv
    // 求解从(x,y)所在平面(摄像头图像)到(x',y')所在平面(地板)的单应矩阵H
    // 假设摄像头图像的分辨率是188x120，测量得到的4个特征点的坐标如下
    float C[4][4] = {
        // clang-format off
        // x,   y,     x',    y'
          {1,   2,     0.50,  0.32},  // 左上角
          {2,   118,   0.13,  0.14},  // 左下角
          {186, 116,   0.13, -0.14},  // 右下角
          {185, 4,     0.50, -0.32},  // 右上角
        // clang-format on
    };
    float H[3][3];
    homography_compute(H, C);
    /// ^^^ 初始化 ^^^

    // vvv 主循环 vvv
    while (1) {
        // 假设在处理摄像头图像的时候碰到了一个点(20, 40)
        int x = 20, y = 40;
        float px, py;
        homography_project(H, x, y, &px, &py);
        // 得到的(px, py)就是这个点在地板上的坐标
    }
    /// ^^^ 主循环 ^^^
}

int main() {
    // vvv 初始化 vvv
    float C[4][4] = {
        // clang-format off
        // x',    y',     x,   y
          {0.50,  0.32,   1,   2  },  // 左上角
          {0.13,  0.14,   2,   118},  // 左下角
          {0.13, -0.14,   186, 116},  // 右下角
          {0.50, -0.32,   185, 4  },  // 右上角
        // clang-format on
    };
    float H[3][3];
    homography_compute(H, C);
    /// ^^^ 初始化 ^^^

    // vvv 主循环 vvv
    while (1) {
        // 假设现在要获取地板上坐标为(10, 5)处的颜色值
        int px = 10, py = 5;
        float x, y;
        homography_project(H, px, py, &x, &y);
        // 得到的(x, y)就是这个点在摄像头图像上的坐标

        // 最近邻插值，就是直接取最近的像素点的颜色值；效果并不好，会出现锯齿，仅作为演示
        int x_ = lroundf(x), y_ = lroundf(y);
        uint8_t color;
        // 必须额外进行边界检查
        if (0 <= x && x < WIDTH && 0 <= y && y < HEIGHT)
            color = image[y_][x_];
        else
            color = 0;  // 超出图像范围的点，颜色自定
    }
    /// ^^^ 主循环 ^^^
}