# C4.py
import cv2
import numpy as np
import time
from  motorm import turn 
print('import tensorflow...') #................................
from tensorflow.keras.preprocessing import image
from tensorflow.keras.optimizers import RMSprop
from tensorflow import keras
import tensorflow as tf
import os
import math
import shutil
from tensorflow.keras.models import Sequential
from tensorflow.keras.layers import Dense
from tensorflow.keras.layers import Flatten
from tensorflow.keras.layers import Conv2D
from tensorflow.keras.layers import MaxPooling2D
from tensorflow.keras.layers import Dropout
from tensorflow.keras.utils import to_categorical
from sklearn.model_selection import train_test_split
print('import tensorflow done...') #................................
def Sload_images_from_folder(iszx,isy,folder):
    nitems=os.listdir(folder); nsitems=sorted(nitems)
    images=[]; img5=[]
    for filename in nsitems:
        img = cv2.imread(os.path.join(folder,filename), cv2.IMREAD_GRAYSCALE)
        if img is not None:
            img1=cv2.resize(img,(iszx,iszy))/255
            images.append(img1); img5.append(img)
    lens=len(nsitems)
    return lens,nsitems,np.array(images[:lens]),img5

LAB=['BK','DD','DL','DR','LD','LL','RD','RR']; ncat=len(LAB)
iszx=128;iszy=iszx//3; ncat=len(LAB); nfile=20
mod1='model_TRK'+str(nfile)+'_b.h5'
model=keras.models.load_model(mod1)
print('load ',mod1,' ... done')

pathT='DRN/TEST'
ntest,fimgs,test,img5=Sload_images_from_folder(iszx,iszy,pathT)
print('load is done path=',pathT,fimgs)



def sh0(img,win): cv2.imshow(win,img);
def cw(k): cv2.waitKey(k)

def Bottom1(img):
    img1=np.copy(img)
    imgb=cv2.resize(img1,(LX2,LY2))
    lowT=np.array([100,100,50]); uppT=np.array([120,255,255])
    hsv=cv2.cvtColor(imgb,cv2.COLOR_BGR2HSV);
    #for i in range(80): print(hsv[59,i,:])
    mask1=cv2.inRange(hsv,lowT,uppT)
    imgc=cv2.bitwise_and(imgb,imgb,mask=mask1)
    #sh0(img,'img'); sh0(mask1,'mask1'); sh0(imgc,'imgc'); 
    #img2=cv2.resize(mask1,(LX2,LY2))
    img2=np.copy(mask1)
    nps2=np.sum(img2[LY2-4:LY2,:])
    if(nps2<1000): DR=-100; return -100,0,0,0,img1
    Y=LY2-1; XM=int(LX2/2); RED=[]; 
    for x in range(LX2): 
        if(img2[Y,x]>100): RED.append(x)
    LRED=len(RED)
    if(LRED > 0):
        MIDL=int(np.median(RED))
        cv2.line(img1,(MIDL*F,0),(MIDL*F,Y*F),(0,255,0),4)
        if(30<=MIDL<=50): DR=1
        elif(50<MIDL<80): DR=7
        elif(0<=MIDL<30): DR=3
    else: MIDL=-1; DR=-2
    #sh0(img1,'img1'); cw(0)
    return DR,nps2,LRED,MIDL,img1

def ckrdim():
    img2=np.copy(test[n])
    sh0(img2,'img2'); cw(0)
    X_test=np.copy(img2)
    X_test=X_test.reshape(1,iszx,iszy,1).astype("float32")
    pred=np.argmax(model.predict(X_test),axis=1)
    print('pred=',pred)


F=8; LX=640; LY=480; LX2=int(LX/F); LY2=int(LY/F)
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, LX)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, LY)
ret,frame = cap.read()
sh0(frame,'frame'); cw(1000)
n=0; maxstep=400
while(n<maxstep and cap.isOpened()):
    n+=1; ret,frame=cap.read(); Y,X,_=frame.shape
    if(ret==False): break

    t0=time.time()
    img1=frame[Y//2:,:,:]; 
    sh0(img1,'img1a'); #print('is=',img1.shape)
    lowT=np.array([100,60,50]); uppT=np.array([120,255,255])
    hsv=cv2.cvtColor(img1,cv2.COLOR_BGR2HSV);
    mask0=cv2.inRange(hsv,lowT,uppT)
    mask1=np.copy(mask0)
    img2=cv2.resize(mask1,(iszx,iszy))
    img22=cv2.resize(mask1,(iszx,iszy))/255
    X_test=np.copy(img22)
    X_test=X_test.reshape(1,iszx,iszy,1).astype("float32")
    #preda=np.argmax(model.predict(X_test),axis=1);
    #cat=LAB[preda[0]]
    preda=50;cat='50'
    t1=time.time(); dt=round(t1-t0,3)
    print('preda=',preda,cat,dt,X_test.shape,img2.shape,img1.shape,frame.shape)
    #cv2.putText(img2,cat,(50,50),cv2.FONT_HERSHEY_SIMPLEX,
    #      1.5,255,2,cv2.LINE_AA)
    sh0(img2,'img2a'); #cw(0)
    fn='DRB/DR_b_'+str(n).zfill(3)+'.png'; cv2.imwrite(fn,img2)
    #continue

    DR,nps2,LRED,MIDL,img1=Bottom1(frame)
    print(n,DR,'nps2=',nps2,LRED,MIDL)
    '''
    if(cat=='DD'): turn(0.6,0.6,0.08); turn(0,0,0.01);
    if(cat=='BK'): turn(-0.3,-0.3,0.03); turn(0,0,0.03);
    if(cat=='DL'): turn(0.0,0.4,0.10); turn(0,0,0.01);
    if(cat=='DR'): turn(0.4,0.0,0.10); turn(0,0,0.01);
    '''

    if(DR==1): turn(0.4,0.4,0.08); turn(0,0,0.01);
    if(DR==-2): turn(-0.3,-0.3,0.05); turn(0,0,0.03);
    if(DR==3): turn(0.0,0.4,0.10); turn(0,0,0.01);
    if(DR==7): turn(0.4,0.0,0.10); turn(0,0,0.01);
    if(DR==-100): 
        turn(0.6,-0.6,0.4); turn(0,0,0.01);
        for k in range(100):
            ret,im=cap.read();
            DR,nps2,LRED,MIDL,img1=Bottom1(im)
            print(k,DR)
            if(DR==1): break
            turn(0.3,-0.3,0.05); turn(0,0,0.11);
    cv2.imshow('img1',img1); #cw(0)
    if cv2.waitKey(1) & 0xFF == ord('q'): break
cap.release()
cv2.destroyAllWindows()
print('C4 is done...')