from picamera.

array import PiRGBArray

from picamera import PiCamera
import time
import cv2
import RPi.GPIO as GPIO

GPIO.setmode(GPIO.BOARD)
control_pinsX = [13,11,15,12]
control_pinsY = [36,32,38,40]
screenwidth = 640
screenheight = 480

for pin in control_pinsX:#do method on vertical + check for stutter

    GPIO.setup(pin, GPIO.OUT)
    GPIO.output(pin, 0)
for pin2 in control_pinsY:
    GPIO.setup(pin2, GPIO.OUT)
    GPIO.output(pin2, 0)
halfstep_seqL = [
    [1,0,0,0],
    [1,1,0,0],
    [0,1,0,0],
    [0,1,1,0],
    [0,0,1,0],
    [0,0,1,1],
    [0,0,0,1],
    [1,0,0,1]
  
]
halfstep_seqR = [
    [1,0,0,1],
    [0,0,0,1],
    [0,0,1,1],
    [0,0,1,0],
    [0,1,1,0],
    [0,1,0,0],
    [1,1,0,0],
    [1,0,0,0]
  
]

try:
    face_cascade =
cv2.CascadeClassifier('/home/pi/Working123/data/haarcascade_frontalface_default.xml')
  
    camera = PiCamera()
    camera.rotation =180
    camera.resolution = (640, 480)
#    camera.framerate = 24
  
    rawCapture = PiRGBArray(camera, size = (screenwidth, screenheight))
  
  
  
    time.sleep(0.1)
  
  
    for frame in camera.capture_continuous(rawCapture, format = "bgr", use_video_port = True):
        img = frame.array
    
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    
        faces = face_cascade.detectMultiScale(gray, 1.3, 5)
    

    
        for (x, y, w, h) in faces:
            cv2.rectangle(img, (x, y), (x+w, y+h), (255, 0, 0), 2)
            #roi_color = img[y:y+h, x:x+w]
            xmid = (screenwidth-(w*1.5))/2
            ymid = (screenheight-h)/2
      

            if x<xmid - 50:
                for i in range(1):
                    for halfstep in range(8):
                        for pin in range(4):
                            GPIO.output(control_pinsX[pin], halfstep_seqL[halfstep][pin])
                        time.sleep(0.05)

                print("movv right")
                time.sleep(0.05)
        
            elif x>xmid + 50:
                print("movv left")
        
                for i in range(1):
                    for halfstep in range(8):
                        for pin in range(4):
                            GPIO.output(control_pinsX[pin], halfstep_seqR[halfstep][pin])
                        time.sleep(0.05)
                time.sleep(0.05)
      
            if y<ymid - 30:
                print("movv up")
        
                for i in range(1):
                    for halfstep in range(8):
                        for pin2 in range(4):
                            GPIO.output(control_pinsY[pin2], halfstep_seqL[halfstep][pin2])
                        time.sleep(0.05)
                time.sleep(0.05)
        
            elif y>ymid + 30:
                print("movv down")
        
                for i in range(1):
                    for halfstep in range(8):
                        for pin2 in range(4):
                            GPIO.output(control_pinsY[pin2], halfstep_seqR[halfstep][pin2])
                        time.sleep(0.05)
                time.sleep(0.05)
          
            #time.sleep(2)
            break
    
    
    
        cv2.imshow("Frame", img)
    
        key = cv2.waitKey(1)
    
        rawCapture.truncate(0)
      
        if key == ord("q"):
            break
finally:
    cv2.destroyAllWindows()
    GPIO.cleanup()