#/usr/bin/python3
import numpy as np
import cv2
import glob
filename1="imaobj1_rect.jpg"
filename2="imaobj2_rect.jpg"

img1 = cv2.imread(filename1)
if(len(img1.shape)<3):
    img1 = cv2.cvtColor(img1,cv2.COLOR_GRAY2BGR)
img2 = cv2.imread(filename2)
if(len(img2.shape)<3):
    img2 = cv2.cvtColor(img2,cv2.COLOR_GRAY2BGR)

h,  w = img1.shape[:2]
print("h:"+str(h))    
print("w:"+str(w))   

#------------------
def projeterPoint(M,iCw):
    #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)
#------------------
alphau=2149.349810656287900
alphav=2146.276553276586100

pu=412.626457723086160 
pv=355.723365602020240 


rvec1 = np.float32([ -3.125480e-001 , -2.316874e+000 , 1.048207e-001 ])
tvec1 = 0.001 * np.float32([ [4.813190e+001]  , [-1.258122e+002] , [1.066916e+003] ])
rvec2 = np.float32([ 6.592006e-001  , -2.262313e+000 , -3.064294e-001])
tvec2 = 0.001 * np.float32([ [2.557518e+001]  , [-6.888417e+001] , [1.091155e+003] ])

matR1,jac1=cv2.Rodrigues(rvec1) 
print("matR1:"+str(matR1))     
matR2,jac2=cv2.Rodrigues(rvec2)    
print("matR2:"+str(matR2))     


#affichage comme TD synthese
cRT1w=np.hstack((matR1,tvec1))
cRT1w = np.vstack((cRT1w,[0,0,0,1]))
cRT2w=np.hstack((matR2,tvec2))
cRT2w = np.vstack((cRT2w,[0,0,0,1]))

print("cRTw: "+str(cRT1w))
#print("mtx: "+str(mtx))
iCc=np.float32([[alphau,0,pu,0],[0,alphav,pv,0],[0,0,1,0]])
print("iCc:"+str(iCc))
iC1w=iCc@cRT1w
print("iC1w:"+str(iC1w))
iC2w=iCc@cRT2w
print("iC2w:"+str(iC2w))


#---------------------------
#TODO: afficher un repère de 10cm de coté sur chaque image sur chaque image
#---------------------------




#liste des points pour les 2 image, mu puis mv
listem1 = np.int_([[490,96], [329,83] , [510,119] , [359,107],
                        [493,156],[339,151],[525,189],[373,180],[507,230],
                        [353,223],[534,253],[385,250],[514,302],[361,304],
                        [544,333],[393,336],[523,377],[369,383],
                        [400,252], # ce point n'est pas observé dans l'image
                        [254,264]])

listem2 = np.int_([[448,194], [290,184] , [450,233] , [297,228],
                        [420,249],[265,243],[427,297],[272,294],[391,311],
                        [241,308],[399,349],[250,354],[366,364],[212,367],
                        [371,408],[216,417],[338,416],[179,420],
                        [316,263], # ce point n'est pas observé dans l'image
                        [162,228]])

if listem2.shape[0]!=listem1.shape[0]:
    print("les 2 listes de points doivent avoir le même nombre de points")
    exit(-1)
listeM=np.zeros((listem2.shape[0],3), np.float32)

#valeur par defaut pour le point 3D reconstruit
X = np.zeros((3,1), np.float32)
Xz0 = np.zeros((3,1), np.float32)

for i,(mu,mv) in enumerate(listem1):   
    img1 = cv2.circle(img1,(mu,mv), 3,(255,0,255), -1)
    font = cv2.FONT_HERSHEY_SIMPLEX
    cv2.putText(img1,str(i),(mu+10,mv), font, 1,(255,0,255),2,cv2.LINE_AA)
    #récupère le point correspondant dans l'image 2
    mu2=listem2[i][0]
    mv2=listem2[i][1]
    img2 = cv2.circle(img2,(mu2,mv2), 3,(255,0,255), -1)
    font = cv2.FONT_HERSHEY_SIMPLEX
    cv2.putText(img2,str(i),(mu2+10,mv2), font, 1,(255,0,255),2,cv2.LINE_AA)
    #---------------------------
    #TODO: Remplir listeM[i][:] avec les coordonnées 3D du point i
    #---------------------------
   





#image composée à partir des 2 images 1 et 2
imagecomposee = np.hstack((img1,img2))
filenameout="imaobj_rect-out.jpg"
cv2.imwrite(filenameout,imagecomposee)
cv2.imshow('dst',imagecomposee)
cv2.waitKey(-1)
cv2.destroyAllWindows()
#-----------------------------------------------
#matplot 3D
#https://www.geeksforgeeks.org/three-dimensional-plotting-in-python-using-matplotlib/
#mettre à True pour activer le rendu 3D après fermeture de la fenêtre opencv
if False:
    # importing mplot3d toolkits
    from mpl_toolkits import mplot3d
    import numpy as np
    import matplotlib.pyplot as plt
    
    fig = plt.figure()
    
    # syntax for 3-D projection
    ax = plt.axes(projection ='3d')
    
    # defining axes
    x = listeM[:,0]
    y = listeM[:,1]
    z = listeM[:,2]
    ax.scatter(x, y, z)
    ax.axis('equal')
    
    # syntax for plotting
    ax.set_title('3d Scatter plot geeks for geeks')
    plt.show()