import cv2
import mediapipe as mp
import numpy as np
import time
import RPi.GPIO as GPIO

# import RPi.GPIO as GPIO

# varialbe define

total_step = 0
step_num = 0
before_total_step = 0
counter = 0
lastcounter = 0
lastStateCLK = 1
lastStateDT = 1
dir = 0
IRNoSensingCount = 0
IRresetTime = 0
IRrealTime = 0
IRdeltaTime = 0

# mode define

rotationMode = "stop"
encoderOnOff = "Off"
visionOnOff = "Off"
IROnOff = "Off"
workMode = "None"

#channel define

DIR = 26
STP = 19
EN = 0
SW = 16
DT = 20
CLK = 21
IRChannel = 0
visionChannel = 0
IR_R = 23
IR_L = 24

# signal setup

GPIO.setmode(GPIO.BCM)
GPIO.setup(IR_R, GPIO.IN)
GPIO.setup(IR_L, GPIO.IN)
GPIO.setup(STP, GPIO.OUT)
GPIO.setup(DIR, GPIO.OUT)
GPIO.setup(CLK, GPIO.IN)
GPIO.setup(DT, GPIO.IN)
GPIO.setup(SW, GPIO.IN)
GPIO.setup(IRChannel, GPIO.IN)
GPIO.setup(visionChannel, GPIO.IN)

#define function

def IRSensing() :

    global step_num
    global dir
    global IRNoSensingCount
    global IRdeltaTime

    if GPIO.input(IR_R) == 0 :
        IRSignalRight = 0
        IRNoSensingCount = 3
    if GPIO.input(IR_L) == 0 :
        IRSignalLeft = 0
        IRNoSensingCount = 3

    if IRNoSensingCount > 0  :
        step_num = 200
    else :
        step_num = 0

    if IRSignalRight == 0 and IRSignalLeft == 1 :
        dir = 1
    elif IRSignalRight == 1 and IRSignalLeft == 0 :
        dir = 0

    if IRSignalRight == 1 and IRSignalLeft == 1 and IRdeltaTime > 0.1 :
        IRNoSensingCount = IRNoSensingCount - 1

def Drive(stepVariable, directionVariable, timeInterval) :

    global total_step

    GPIO.output(DIR, directionVariable)
    i=0

    while True:

        GPIO.output(STP, GPIO.HIGH)
        time.sleep(timeInterval)
        GPIO.output(STP, GPIO.LOW)
        time.sleep(timeInterval)

        i=i+1

        if i > stepVariable :
            break

    if directionVariable == GPIO.HIGH :
        total_step = total_step + stepVariable
    else :
        total_step = total_step - stepVariable

def AccelDrive(stepVariable) :

    global total_step
    global dir

    GPIO.output(DIR, dir)
    i=0

    while True:

        GPIO.output(STP, GPIO.HIGH)
        time.sleep(0.0001*i)
        GPIO.output(STP, GPIO.LOW)
        time.sleep(0.0001*i)

        i=i+1

        if i > stepVariable :
            break

    if dir == 0 :
        total_step = total_step + stepVariable
    else :
        total_step = total_step - stepVariable

    # please check minsung~

def DriveTest(stepVariable, directionVariable) :

    global total_step

    if directionVariable == 0 :
        total_step = total_step + stepVariable
    else :
        total_step = total_step - stepVariable

# define inturrupt

def encoderCount(channel) :

    global lastStateCLK
    global lastStateDT
    global counter
    global lastcounter

    currentStateCLK = GPIO.input(CLK)
    currentStateDT = GPIO.input(DT)

    if lastStateCLK == 1 and currentStateDT == 1 and currentStateCLK == 0 and lastStateDT == 1:
        lastcounter = counter
        counter = counter + 1
        dir = 0
    elif lastStateDT == 1 and currentStateCLK == 1 and currentStateDT == 0 and lastStateCLK == 1:
        lastcounter = counter
        counter = counter - 1
        dir = 1

    if lastStateCLK == 1 and currentStateDT == 0 and currentStateCLK == 0 and lastStateDT == 1 :
        if dir == 0 :
            lastcounter = counter
            counter = counter + 1
        if dir == 1 :
            lastcounter = counter
            counter = counter - 1

    if lastcounter == counter :
        if lastStateCLK == 0 and currentStateDT == 0 and currentStateCLK == 1 and lastStateDT == 0:
            counter = counter + 1
            dir = 0
        elif lastStateDT == 0 and currentStateCLK == 0 and currentStateDT == 1 and lastStateCLK == 0:
            counter = counter - 1
            dir = 1

    

    if lastStateDT != currentStateDT or currentStateCLK != lastStateCLK :
        lastcounter = counter

    print (currentStateCLK, currentStateDT, counter)

    lastStateCLK = currentStateCLK
    lastStateDT = currentStateDT

def encoderSwitch() :

    encoderOnOff = "On"

def IRSwitch() :

    if IROnOff == "Off" :
        IROnOff = "On"
    else :
        IROnOff = "Off"

def visionSwitch() :

    if visionOnOff == "Off" :
        visionOnOff = "On"
    else :
        visionOnOff = "Off"

# inturrupt define

GPIO.add_event_detect(IRChannel, GPIO.FALLING, callback=IRSwitch, bouncetime=200)
GPIO.add_event_detect(visionChannel, GPIO.FALLING, callback=visionSwitch, bouncetime=200)
GPIO.add_event_detect(SW, GPIO.FALLING, callback=encoderSwitch, bouncetime=200)
GPIO.add_event_detect(CLK, GPIO.BOTH, callback=encoderCount)
GPIO.add_event_detect(DT, GPIO.BOTH, callback=encoderCount)

# vision setup

global mode

mp_face_mesh = mp.solutions.face_mesh
face_mesh = mp_face_mesh.FaceMesh(min_detection_confidence=0.5, min_tracking_confidence=0.5)

mp_drawing = mp.solutions.drawing_utils

drawing_spec = mp_drawing.DrawingSpec(thickness=1, circle_radius=1)

cap = cv2.VideoCapture(0)

print("start")
time.sleep(2)

# WORK

while True :

    # drive work
    if encoderOnOff == "On" :

        workMode = "Switch"

        if counter > 0 :
            step_num = counter*1000
            dir = 0
        elif counter < 0 :
            step_num = counter*(-1000)
            dir = 1

        step_num = step_num + (before_total_step - total_step)

        if counter != 0 :
            Drive(step_num, dir, 0.0001)
            before_total_step = total_step
            workMode = "None"
            encoderOnOff = "Off"

    if IROnOff == "On" :

        IRNoSensingCount = 0
        IRresetTime = time.time()

        while True :
            
            IRrealTime = time.time()
            IRdeltaTime = IRrealTime - IRresetTime

            step_num = 0
            IRSensing()

            if step_num != 0 :
                workMode = "IR"
                Drive(step_num, dir, 0.0002)

            elif step_num == 0 or IROnOff == "Off" :
                workMode = "None"
                break

            if IRdeltaTime > 0.1 :
                IRresetTime = time.time()

        # fix it later
        if IRNoSensingCount == 0 :
            AccelDrive(50)

    if visionOnOff == "On" :

        success, image = cap.read()
        start = time.time()

        image = cv2.cvtColor(cv2.flip(image, 1), cv2.COLOR_BGR2RGB)
        image.flags.writeable = False
        results = face_mesh.process(image)
        image.flags.writeable = True
        image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)

        img_h, img_w, img_c = image.shape
        face_3d = []
        face_2d = []

        if results.multi_face_landmarks:
            for face_landmarks in results.multi_face_landmarks:
                for idx, lm in enumerate(face_landmarks.landmark):
                    if idx == 33 or idx == 263 or idx == 1 or idx == 61 or idx == 291 or idx == 199:
                        if idx == 1:
                            nose_2d = (lm.x * img_w, lm.y * img_h)
                            nose_3d = (lm.x * img_w, lm.y * img_h, lm.z * 3000)

                        x, y = int(lm.x * img_w), int(lm.y * img_h)
                        face_2d.append([x, y])
                        face_3d.append([x, y, lm.z])

                face_2d = np.array(face_2d, dtype=np.float64)
                face_3d = np.array(face_3d, dtype=np.float64)

                focal_length = 1 * img_w

                cam_matrix = np.array([ [focal_length, 0, img_h / 2],
                                        [0, focal_length, img_w / 2],
                                        [0, 0, 1]])

                dist_matrix = np.zeros((4, 1), dtype=np.float64)
                success, rot_vec, trans_vec = cv2.solvePnP(face_3d, face_2d, cam_matrix, dist_matrix)
                rmat, jac = cv2.Rodrigues(rot_vec)
                angles, mtxR, mtxQ, Qx, Qy, Qz = cv2.RQDecomp3x3(rmat)

                x = angles[0] * 360
                y = angles[1] * 360
                z = angles[2] * 360

                if -20 < y <= -7:
                    text = "Looking Left little"
                elif 20 > y >= 7:
                    text = "Looking Right little"
                elif -20 > y :
                    text = "Looking Left"
                    rotationMode = "rotating left"
                elif 20 < y :
                    text = "Looking Right"
                    rotationMode = "rotating right"
                else:
                    text = "Forward"
                    rotationMode = "stop"

                mp_drawing.draw_landmarks(
                            image=image,
                            landmark_list=face_landmarks,
                            connections=mp_face_mesh.FACEMESH_TESSELATION,
                            landmark_drawing_spec=drawing_spec,
                            connection_drawing_spec=drawing_spec)

        workMode = "vision"
        step_num = 400

        if rotationMode == "rotating right" and total_step < 15555 :
            dir = 1
            Drive(step_num, dir, 0.0001)
        elif rotationMode == "rotating left" and total_step <-15555 :
            dir = 0
            Drive(step_num, dir, 0.0001)
        elif rotationMode == "stop" :
            workMode = "None"

    # if you click H, exit
    if cv2.waitKey(5) & 0xFF == 72:
        total_step = 0

cap.release()

import cv2 import mediapipe as mp import numpy as np import time import RPi.GPIO as GPIO

import RPi.GPIO as GPIO

varialbe define

total_step = 0 step_num = 0 before_total_step = 0 counter = 0 lastcounter = 0 lastStateCLK = 1 lastStateDT = 1 dir = 0 ySensing = 0

mode define

rotationMode = "stop" encoderOnOff = "Off" visionOnOff = "Off" IROnOff = "Off" workMode = "None"

#channel define

DIR = 26 STP = 19 EN = 0 SW = 16 DT = 20 CLK = 21 IRChannel = 0 visionChannel = 0 IR_R = 23 IR_L = 24

signal setup

GPIO.setmode(GPIO.BCM) GPIO.setup(IR_R, GPIO.IN) GPIO.setup(IR_L, GPIO.IN) GPIO.setup(STP, GPIO.OUT) GPIO.setup(DIR, GPIO.OUT) GPIO.setup(CLK, GPIO.IN) GPIO.setup(DT, GPIO.IN) GPIO.setup(SW, GPIO.IN) GPIO.setup(IRChannel, GPIO.IN) GPIO.setup(visionChannel, GPIO.IN)

#define function

def IRSensing() :

global step_num
global dir

IRSignalRight = GPIO.input(IR_R)
IRSignalLeft = GPIO.input(IR_L)

if IRSignalRight == 0 and IRSignalLeft == 1 :
    dir = 1
    step_num = 200
elif IRSignalRight == 1 and IRSignalLeft == 0 :
    dir = 0
    step_num = 200

def Drive(stepVariable, directionVariable, timeInterval) :

global total_step

GPIO.output(DIR, directionVariable)
i=0

while True:

    GPIO.output(STP, GPIO.HIGH)
    time.sleep(timeInterval)
    GPIO.output(STP, GPIO.LOW)
    time.sleep(timeInterval)

    i=i+1

    if i > stepVariable :
        break

if directionVariable == GPIO.HIGH :
    total_step = total_step + stepVariable
else :
    total_step = total_step - stepVariable

def AccelDrive(stepVariable) :

global total_step
global dir

GPIO.output(DIR, dir)
i=0

while True:

    GPIO.output(STP, GPIO.HIGH)
    time.sleep(0.0001*i)
    GPIO.output(STP, GPIO.LOW)
    time.sleep(0.0001*i)

    i=i+1

    if i > stepVariable :
        break

if dir == 0 :
    total_step = total_step + stepVariable
else :
    total_step = total_step - stepVariable

# please check minsung~