import cv2
import numpy as np
import math
print("==============================================================")
taillecube=1
listeM = np.float32([[0,0,0], [taillecube,0,0],[taillecube,taillecube,0], [0,taillecube,0],
                   [0,0,taillecube], [taillecube,0,taillecube],[taillecube,taillecube,taillecube], [0,taillecube,taillecube]])

print("listeM:"+str(listeM))         
listeseg=[[0,1],[1,2],[2,3],[3,0],[4,5],[5,6],[6,7],[7,4],[0,4],[1,5],[2,6],[3,7]]                   
print("listeseg:"+str(listeseg))
print("==============================================================")

cRw=np.float32([[1,0,0],[0,-1,0],[0,0,-1]])
cTw=np.float32([[0],[0],[3]])
cRTw = np.hstack((cRw,cTw))
cRTw = np.vstack((cRTw,[0,0,0,1]))

print("cRTw:"+str(cRTw))
print("==============================================================")

eu=0.00001 #10um
ev=eu
#champ de vision en degré
fovhdegres=100
demifovh=(fovhdegres/2)*math.pi/180
alphau=400/math.tan(demifovh)
print("alphau:"+str(alphau))
f=alphau*eu
print("f:"+str(f)+" m")

alphav=alphau
demifovv=math.atan(300/alphav)
fovvdegres=demifovv*2*180/math.pi

print("fovvdegres:"+str(fovvdegres))
#=======================================
pu=400
pv=300
iCc=np.float32([[alphau,0,pu,0],[0,alphav,pv,0],[0,0,1,0]])
print("iCc:"+str(iCc))
iCw=iCc@cRTw
print("iCw:"+str(iCw))
#------------------
def projeterPoint(M):
    print("M:"+str(M))
    M = np.append(M, 1)
    print("M:"+str(M))
    m=iCw@M
    #deshomogénéisation
    mu=m[0]/m[2]
    mv=m[1]/m[2] 
    #arrondi pour avoir des positions entières
    mu=int(round(mu,0))
    mv=int(round(mv,0))
    print("mu:"+str(mu))
    print("mv:"+str(mv))   
    return  (mu,mv)

#------------------
M= np.float32([0,0,0])
m= projeterPoint(M)


# Create a black image 
img = np.zeros((pv*2,pu*2), np.uint8)

for seg in listeseg:
    print("seg:"+str(seg))
    m1=projeterPoint(listeM[seg[0]])
    m2=projeterPoint(listeM[seg[1]])
    print("m1:"+str(m1))
    print("m2:"+str(m2))
    cv2.line(img,m1,m2,255,1,cv2.LINE_AA)   

cv2.imshow('image',img)
cv2.waitKey(0)
cv2.destroyAllWindows()