import cv2 as cv
import numpy as np
import math
import image

# 引入图像代价计算相关参数
canny = image.canny
G = image.G
minG = min(min(row) for row in G)
maxG = max(max(row) for row in G)


# 边缘代价
def fZ(p):
    x = p[0]
    y = p[1]
    if canny[x, y] == 255:
        return 0
    else:
        return 1


# 梯度幅值
def fG(p):
    x = p[0]
    y = p[1]
    # return 1 - (G[x, y] - minG) / (maxG - minG)
    return 1 - G[x, y] / maxG


# 梯度方向
def fD(p, q):
    px = p[0]
    py = p[1]
    qx = q[0]
    qy = q[1]
    # Dp、Dq
    Dp = np.array([image.sobely[px, py], -image.sobelx[px, py]])
    Dq = np.array([image.sobely[qx, qy], -image.sobelx[qx, qy]])
    p1 = np.array([px, py])
    q1 = np.array([qx, qy])
    q_p = q1 - p1
    p_q = p1 - q1
    # L
    L = q_p if np.dot(Dp, q_p) >= 0 else p_q
    # dp、dq
    dp = np.dot(Dp, L)
    dq = np.dot(L, Dq)
    t = max(math.fabs(dp), math.fabs(dq))
    times = 1
    while t > 1:
        t /= 10
        times *= 10
    dp /= times
    dq /= times
    return (1 / np.pi) * (np.arccos(dp) + np.arccos(dq))


# 像素点p和q之间的局部代价
def total(p, q):
    return 0.43 * fz[p[0], p[1]] + 0.43 * fD(p, q) + 0.14 * fg[q[0], q[1]]


# 将整个图像的fz和fg提前储存方便使用并减少不必要的计算
fz = np.zeros((image.row, image.col))
fg = fz
for i in range(image.row):
    for j in range(image.col):
        fz[i, j] = fZ((i, j))
        fg[i, j] = fG((i, j))