import cv2
import time
import pigpio
import datetime
import threading
import os

pi = pigpio.pi()
sMin=0.005
sMax=0.0002
STP1 = 19
DIR1 = 13
EBL1 = 12
STP2 = 18
DIR2 = 24
EBL2 = 4
M=1/32   # microstep
N=200   # stepper
FOV = 20 # Field of view
R=M*360/N       # 1-Stepの回転角度
RR= 1 / R       # １度の回転に必要なSTEP数
pX=0
pY=0
cAz = 0
cEv = 0
track = False
mode ='F'
reverse = False
Limit = False
circle = True
th = 170
LowEv,HighEv = 0, 80
RightAz, LeftAz = 0,100
cAz, cEv = 0, 0
oAz, oEv = 55, 45
pi.set_mode(STP1,pigpio.OUTPUT)
pi.set_mode(DIR1,pigpio.OUTPUT)
pi.set_mode(EBL1,pigpio.OUTPUT)
pi.set_mode(EBL2,pigpio.OUTPUT)
pi.set_mode(STP2,pigpio.OUTPUT)
pi.set_mode(DIR2,pigpio.OUTPUT)
delay_Az = 0.0005
delay_Ev = 0.002

def goto(oAz,oEv):
    global cAz,cEv,STP1,STP2,DIR1,DIR2,delay_Az,delay_Ev
    tA, tE = oAz - cAz, oEv - cEv
    rotAz(tA)
    rotEv(tE)

def key(k):
    global cAz,cEv,STP1,DIR1,STP2,DIR2,th
    global track,reverse,circle,dW,dH,Limit
    global LowEv,HighEv ,RightAz, LeftAz
    if k == ord('b'):
        cAz = 0
        cEv = 0
    elif k == ord('j') or k==81:
        rotAz(-0.5625)
    elif k == ord('J'):
        LeftAz=round(cAz,1)
    elif k == ord('k') or k==83:
        rotAz(0.5625)
    elif k == ord('K'):
        RightAz=round(cAz,1)
    elif k == ord('h'):
        rotAz(-5.625)
    elif k == ord('l'):
        rotAz(5.625)
    elif k == ord('i') or k==82:
        rotEv(1)
    elif k == ord('I'):
        HighEv=round(cEv,1)
    elif k == ord('m') or k==84:
        rotEv(-1)
    elif k == ord('o'):
        goto(30,40)
    elif k == ord('M'):
        LowEv=round(cEv,1)
    elif k == ord('a'):
        th = th - 2
    elif k == ord('s'):
        th = th + 2
    elif k == ord('t'):
        track = not track
    elif k == ord('p'):
        Limit = not Limit
    elif k == ord('r'):
        reverse = not reverse
    elif k == ord('@'):
        print('AzL=',round(LeftAz,1),',AzR=',round(RightAz,1),',EvH=',round(HighEv,1),',EvL=',round(LowEv,1))
    elif k == ord('c'):
        cAz, cEv = 10, 22
        LeftAz, RightAz = -9, 157
        HighEv, LowEv = 70, 20
    elif k == ord('z'):
        tmp=input()
        tmp=tmp.split(',')
        cAz = float(tmp[0])
        cEv = float(tmp[1])
    elif k == ord('x'):
        tmp=input()
        tmp=tmp.split(',')
        dW = float(tmp[0])
        dH = float(tmp[1])
        mH, mW = dH - cEv, dW - cAz
        print('Az:',cAz,' Ev:',cEv,' dW:',mW,' dH:',mH)

        rotAz(mW)
        rotEv(mH)
        cAz, cEv = dW, dH

def do_move(N,STP,delay):
    #print(N,STP,delay)
    for i in range(N):
        pi.write(STP,1)
        time.sleep(delay)
        pi.write(STP,0)
        time.sleep(delay)
    pi.write(EBL1,0)

def check(Az,Ev):
    global LowEv,HighEv ,RightAz, LeftAz
    AzR,AzL,EvH,EvL = False,False,False,False
    f = False
    if Az> RightAz:
        AzR, f =True, True
    if Az< LeftAz:
        AzL, f=True, True
    if Ev>HighEv:
        EvH, f=True, True
    if Ev<LowEv:
        EvL, f=True, True
    return f,AzR,AzL,EvH,EvL

def rotAz(deg):
    pi.write(EBL2,1)
    rot(deg,STP2,DIR2, delay_Az)

def rotEv(deg):
    pi.write(EBL1,1)
    #time.sleep(0.5)
    rot(deg,STP1,DIR1, delay_Ev)

def rot(deg,STP,DIR,delay):
    global cEv,cAz,LeftAz,LowEv,HighEv
    tmp=deg
    if tmp<0:
        dDIR=1
        tmp=-tmp
    else:
        dDIR=0

    if STP==STP2:
        N=int(0.5+200*32*tmp/360)
        if Limit:
            if AzL and (cAz > LeftAz):
                N = 0
            if AzR and (cAz < RightAz):
                N = 0
        if N != 0:
            cAz=cAz+deg
    else:
        N=int(0.5+48*120*tmp/360) # 48/360 1/120 gear and Full STEP
        if Limit:
            if EvL and (cEv<LowEv):
                N = 0
            if EvH and (cEv>HighEv):
                N = 0
        if N != 0:
            cEv=cEv+deg
    pi.write(DIR,dDIR)
    do_move(N,STP,delay)
    #thread2 = threading.Thread(target= do_move,args=(N,STP,delay,))
    #thread2.start()

def jyusin(img):
    global W2,H2
    mu = cv2.moments(img, False)
    try:
        x,y= int(mu["m10"]/mu["m00"]) , int(mu["m01"]/mu["m00"])
        c=(0,255,0)
        focus = True
    except ZeroDivisionError:
        x = W2
        y = H2
        c=(0,0,255)
        focus = False
    return focus,x,y,c

def finish(n):
    global writer,track,Limit,f_name
    print(f_name,'closed')
    writer.release()
    new_name=f_name.replace('.avi','-'+str(n)+'.avi')
    os.rename(f_name,new_name)
    writer ,f_name= None, ""
    track = False
    Limit = False
    goto(55,50)
    track = True
    Limit = True

fontFace =cv2.FONT_HERSHEY_SIMPLEX
# VideoCapture オブジェクトを取得します
capture = cv2.VideoCapture(0)
#capture.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'mp4v')
capture.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)
capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080)
capture.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"MJPG"))
# 幅
W = int(capture.get(cv2.CAP_PROP_FRAME_WIDTH))
# 高さ
H = int(capture.get(cv2.CAP_PROP_FRAME_HEIGHT))
print('Size W:',W,'  H:',H)
half = False
W2, H2 = int(W/2), int(H/2)
WW, HH = W2, H2
if W>1024:      # resize
    W2, H2=int(W/4), int(H/4)
    half = True
fourcc = cv2.VideoWriter_fourcc(*'h264')
fourcc = cv2.VideoWriter_fourcc(*'XVID')
#fourcc = cv2.VideoWriter_fourcc(*'mp4v')
writer = None
PATH="/home/mars/pWork/"
AzR,AzL,EvH,EvL = False,False,False,False
f_name=""
i=0
white,blue,red,green=(255,255,255),(255,0,0),(0,0,255),(0,255,000)
config='optrack.ini'
if os.path.exists(config):
    f = open(config)
    s = f.read()
    t = s.split(',')
    cAz, cEv, LowEv, HighEv ,RightAz, LeftAz = float(t[0]),float(t[1]),float(t[2]),float(t[3]),float(t[4]),float(t[5])

while(True):
    ret, frame = capture.read()
    if half:
        frame = cv2.resize(frame, dsize=(WW, HH))
    now=datetime.datetime.today()
    if reverse:
        img1 =  cv2.cvtColor(cv2.bitwise_not(frame) ,cv2.COLOR_BGR2GRAY)
    else:
        img1 = cv2.cvtColor(frame ,cv2.COLOR_BGR2GRAY)
    # 二値化(閾値100を超えた画素を255にする
    ret, img_th = cv2.threshold(img1, th, 255, cv2.THRESH_BINARY)
    cv2.imshow('frame2',img_th)
    k=cv2.waitKey(1) & 0xFF
    key(k)
    if k == ord('q'):
        break

    # 重心検出
    focus,x,y,c = jyusin(img_th)
    cc = white
    if focus:
        if track:
            i=i+1
            if writer is None:
                f_name ='video/'+now.strftime("%Y-%m-%d_%H:%M:%S")+'.avi'
                writer=cv2.VideoWriter(f_name, fourcc, 15, (WW,HH))
                print('Record Start:',f_name)
                time_start = time.time()
            else:
                writer.write(frame)
                cc = green
                if (time.time() - time_start) >20:
                    finish(i)
                    i = 0
        else:
            if writer is not None:
                finish(i)
                i = 0

    else:
        if (writer is not None):
            writer.write(frame)
            if  ((time.time()- time_start)>20):
                finish(i)
                i = 0

    if circle:
        cv2.circle(frame, (x,y), 4, color=c, thickness=1)
        cv2.circle(frame, (x,y), 10, color=c, thickness=1)
        cv2.circle(frame, (x,y), 16, color=c, thickness=1)

    f, AzL,AzR,EvH,EvL=check(cAz,cEv)
    cf = green
    if f:
        cf = red
    dW = (WW*0.5 - x)*0.001
    if track and abs(dW)>0.02:
        rotAz(-dW)    #  cAz = cAz + dW * R

    dH = (HH*0.5 - y)*0.001
    if track and abs(dH)>0.02:
        rotEv(dH)     # cEv = cEv + dH*R

    text='T:'+str(track)+' L:'+str(Limit)+' '+now.strftime("%H:%M:%S")+ ' TH=' +str(th)+'  X='+str(x)+'  Y='+str(y)+" F#:"+str(i)+" File:"+f_name
    text2="Az:"+str(round(cAz,2))+"  Ev:"+str(round(cEv,2))+" F:"+str(f)+"  AzL:"+str(AzL)+" AzR:"+str(AzR)+" EvH:"+str(EvH)+" EvL:"+str(EvL)
    text3="Az_step:"+str(round(dW,2))+"  Ev_step:"+str(round(dH,2)) + " Video Reverse:" + str(reverse)
    text4="AzL:"+str(LeftAz)+" AzR:"+str(RightAz)+" EvH:"+str(HighEv)+" EvL:"+str(LowEv)
    img = cv2.putText(frame, text, (10,20), fontFace, 0.5,color=cc)
    img = cv2.putText(frame, text2, (10,40), fontFace, 0.5,color=cf)
    img = cv2.putText(frame, text3, (10,60), fontFace, 0.5,color=cc)
    img = cv2.putText(frame, text4, (10,80), fontFace, 0.5,color=cc)
    cv2.imshow('frame',img)

print('Done!')
f=open(config,'w')
msg=str(cAz)+','+str(cEv)+','+str(LowEv)+','+str(HighEv)+','+str(RightAz)+','+str(LeftAz)
f.write(msg)
f.close()
capture.release()
if writer is not None:
    writer.release()
cv2.destroyAllWindows()
pi.write(EBL1,0)
pi.write(EBL2,0)