import numpy as np


def line_to_border(line,size):
    #line:(a,b,c), ax+by+c=0
    #size:(W,H) 
    H,W=size[1],size[0]
    a,b,c=line[0],line[1],line[2]
    epsa=1e-8 if a>=0 else -1e-8
    epsb=1e-8 if b>=0 else -1e-8
    intersection_list=[]
    
    y_left=-c/(b+epsb)
    y_right=(-c-a*(W-1))/(b+epsb)
    x_top=-c/(a+epsa)
    x_down=(-c-b*(H-1))/(a+epsa)

    if y_left>=0 and y_left<=H-1:
        intersection_list.append([0,y_left])
    if y_right>=0 and y_right<=H-1:
        intersection_list.append([W-1,y_right])
    if x_top>=0 and x_top<=W-1:
        intersection_list.append([x_top,0])
    if x_down>=0 and x_down<=W-1:
        intersection_list.append([x_down,H-1]) 
    if len(intersection_list)!=2:
        return None
    intersection_list=np.asarray(intersection_list)
    return intersection_list

def find_point_in_line(end_point):
    x_span,y_span=end_point[1,0]-end_point[0,0],end_point[1,1]-end_point[0,1]
    mv=np.random.uniform()
    point=np.asarray([end_point[0,0]+x_span*mv,end_point[0,1]+y_span*mv])
    return point

def epi_line(point,F):
    homo=np.concatenate([point,np.ones([len(point),1])],axis=-1)
    epi=np.matmul(homo,F.T)
    return epi

def dis_point_to_line(line,point):
    homo=np.concatenate([point,np.ones([len(point),1])],axis=-1)
    dis=line*homo
    dis=dis.sum(axis=-1)/(np.linalg.norm(line[:,:2],axis=-1)+1e-8)
    return abs(dis)

def SGD_oneiter(F1,F2,size1,size2):
    H1,W1=size1[1],size1[0]
    factor1 = 1 / np.linalg.norm(size1)
    factor2 = 1 / np.linalg.norm(size2)
    p0=np.asarray([(W1-1)*np.random.uniform(),(H1-1)*np.random.uniform()])
    epi1=epi_line(p0[np.newaxis],F1)[0]
    border_point1=line_to_border(epi1,size2)
    if border_point1 is None:
        return -1
    
    p1=find_point_in_line(border_point1)
    epi2=epi_line(p0[np.newaxis],F2)
    d1=dis_point_to_line(epi2,p1[np.newaxis])[0]*factor2
    epi3=epi_line(p1[np.newaxis],F2.T)
    d2=dis_point_to_line(epi3,p0[np.newaxis])[0]*factor1
    return (d1+d2)/2

def compute_SGD(F1,F2,size1,size2):
    np.random.seed(1234)
    N=1000
    max_iter=N*10
    count,sgd=0,0
    for i in range(max_iter):
        d1=SGD_oneiter(F1,F2,size1,size2)
        if d1<0:
            continue
        d2=SGD_oneiter(F2,F1,size1,size2)
        if d2<0:
            continue
        count+=1
        sgd+=(d1+d2)/2
        if count==N:
            break
    if count==0:
        return 1
    else:
        return sgd/count

def compute_inlier_rate(x1,x2,size1,size2,F_gt,th=0.003):
    t1,t2=np.linalg.norm(size1)*th,np.linalg.norm(size2)*th
    epi1,epi2=epi_line(x1,F_gt),epi_line(x2,F_gt.T)
    dis1,dis2=dis_point_to_line(epi1,x2),dis_point_to_line(epi2,x1)
    mask_inlier=np.logical_and(dis1<t2,dis2<t1)
    return mask_inlier.mean() if len(mask_inlier)!=0 else 0