#!/usr/bin/env python3
## -*- coding: utf-8 -*-

# import the necessary packages
from picamera.array import PiRGBArray
from picamera import PiCamera
import os
import time
import numpy as np
import sys
import cv2
import warnings
warnings.filterwarnings("ignore")

import random as rd

V_FOWD = "forward"
V_BACK = "backward"
V_LOST = "lost"
V_MORE = "more_than_one"
       
from time import sleep
import RPi.GPIO as gpio  # https://pypi.python.org/pypi/RPi.GPIO

DIR_NONE = 0
DIR_LEFT = 1
DIR_RIGHT = 2
DIR_FORWARD = 3
DIR_BACKWARD = 4
DIR_BREAK = 5

class Stepper:
    # instantiate stepper
    # pins = [stepPin, directionPin, enablePin]
    def __init__(self, pins):
        
        self.is_moving = True
        self.dir_changed = False
        self.cur_dir = DIR_NONE
        self.default_speed = 0.0001 / 2
        self.thread = None
        
        # setup pins
        self.pins = pins

        self.stepPin_l = self.pins[0][0]
        self.directionPin_l = self.pins[0][1]
        self.enablePin_l = self.pins[0][2]

        self.stepPin_r = self.pins[1][0]
        self.directionPin_r = self.pins[1][1]
        self.enablePin_r = self.pins[1][2]

        # use the broadcom layout for the gpio
        gpio.setmode(gpio.BCM)

        # set gpio pins
        gpio.setup(self.stepPin_l, gpio.OUT)
        gpio.setup(self.directionPin_l, gpio.OUT)
        gpio.setup(self.enablePin_l, gpio.OUT)

        gpio.setup(self.stepPin_r, gpio.OUT)
        gpio.setup(self.directionPin_r, gpio.OUT)
        gpio.setup(self.enablePin_r, gpio.OUT)

        # set enable to high (i.e. power is NOT going to the motor)
        gpio.output(self.enablePin_l, True)
        gpio.output(self.enablePin_r, True)

        # print(
        # "Stepper initialized (step=" + str(self.stepPin_l) + ", direction=" + self.directionPin + ", enable=" + self.enablePin + ")")

    # clears GPIO settings
    def cleanGPIO(self):
        gpio.cleanup()
        
    def forward(self):
        self.cur_dir = DIR_FORWARD
        self.dir_changed = True
        
    def backward(self):
        self.cur_dir = DIR_BACKWARD
        self.dir_changed = True
        
    def left(self):
        self.cur_dir = DIR_LEFT
        self.dir_changed = True
        
    def right(self):
        self.cur_dir = DIR_RIGHT
        self.dir_changed = True
        
    def stop(self):
        self.cur_dir = DIR_BREAK
        self.dir_changed = True

    # step the motor
    # steps = number of steps to take
    # dir = direction stepper will move
    # speed = defines the denominator in the waitTime equation: waitTime = 0.000001/speed. As "speed" is increased, the waitTime between steps is lowered
    # stayOn = defines whether or not stepper should stay "on" or not. If stepper will need to receive a new step command immediately, this should be set to "True." Otherwise, it should remain at "False."
    def step(self, steps, dir_l, dir_r, speed=1, stayOn=False):
                
        # set enable to low (i.e. power IS going to the motor)
        gpio.output(self.enablePin_l, False)
        gpio.output(self.enablePin_r, False)

        # set the output to true for left and false for right
        turnLeft_l, turnLeft_r = True, True
        if (dir_l == 'right'):
            turnLeft_l = False
            
        elif (dir_l != 'left'):
            print("STEPPER ERROR: no direction supplied")
            return False
        
        if (dir_r == 'right'):
            turnLeft_r = False
            
        elif (dir_r != 'left'):
            print("STEPPER ERROR: no direction supplied")
            return False
        
        gpio.output(self.directionPin_l, turnLeft_l)
        gpio.output(self.directionPin_r, turnLeft_r)

        stepCounter = 0
        waitTime = 0.000001 / speed  # waitTime controls speed

        while stepCounter < steps:
            
            if self.dir_changed:
                break
            
            # gracefully exit if ctr-c is pressed
            # exitHandler.exitPoint(True) #exitHandler.exitPoint(True, cleanGPIO)

            # turning the gpio on and off tells the easy driver to take one step
            gpio.output(self.stepPin_l, True), gpio.output(self.stepPin_r, True)
            gpio.output(self.stepPin_l, False), gpio.output(self.stepPin_r, False)
            stepCounter += 1

            # wait before taking the next step thus controlling rotation speed
            sleep(waitTime)

        if (stayOn == False):
            # set enable to high (i.e. power is NOT going to the motor)
            gpio.output(self.enablePin_l, True)
            gpio.output(self.enablePin_r, True)

        # print("stepperDriver complete (turned " + dir + " " + str(steps) + " steps)")
        
    def moving_thread_callback(self):
        
        time.sleep(1)
        while self.is_moving:
            if self.dir_changed:
                
                #print("dir changed")
                self.dir_changed = False
                
                if self.cur_dir == DIR_BREAK:
                    
                    gpio.output(self.enablePin_l, True)
                    gpio.output(self.enablePin_r, True)
                    
                    self.cur_dir = DIR_NONE
                
                elif self.cur_dir == DIR_LEFT:
                    self.step(200000, "right", "right", self.default_speed / 2, stayOn=True)
                
                elif self.cur_dir == DIR_RIGHT:
                    self.step(200000, "left", "left", self.default_speed / 2, stayOn=True)
                    
                elif self.cur_dir == DIR_FORWARD:
                    self.step(200000, "left", "right", self.default_speed, stayOn=True)
                
                elif self.cur_dir == DIR_BACKWARD:
                    self.step(200000, "right", "left", self.default_speed, stayOn=True)
                
            else:
                # sleep a bit
                time.sleep(0.1)
                #print("idle")
            
    def run(self):
        self.thread = Thread(target=self.moving_thread_callback)
        self.thread.start()
        
    def shutdown(self):
        self.is_moving = False


# ---------------------------------
# RUN MOTORS
# ---------------------------------
#[stepPin, directionPin, enablePin]
st = Stepper([[16, 12, 0], [24, 25, 0]])
st.run()

# initialize the camera and grab a reference to the raw camera capture
camera = PiCamera()
camera.resolution = (1024, 1280)
camera.framerate = 32
rawCapture = PiRGBArray(camera, size=camera.resolution)

dmin = round(camera.resolution[0] / 10.0)
dmax = round(camera.resolution[0] / 2.0)
dead_zone = camera.resolution[0] / 5.0 # 120
normal_size = camera.resolution[1] / 10 # 80
size_dead_zone = normal_size / 15

detector = cv2.CascadeClassifier('/home/pi/robot/haarcascade_frontalface_default.xml')

# allow the camera to warmup
time.sleep(0.1)

# ---------------------------------
# MAIN DEMO
# ---------------------------------
def main_demo():

    frame_counter = 0

    # capture frames from the camera
    for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
        
        frame_counter += 1
     
        # grab the raw NumPy array representing the image
        image = frame.array 
        
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
        faces = detector.detectMultiScale(gray, 1.3, 4, minSize=(dmin, dmin), maxSize=(dmax, dmax))
        #print(faces)
       
        if len(faces) > 0:
            widths = [f[2] for f in faces]
            max_face = faces[np.argmax(widths, axis=0)]
            x,y,w,h = max_face
            
            cx = x + w / 2
            cy = y + h / 2
            
            print(w, h)
            
            if cx > (camera.resolution[0] + dead_zone) / 2:
                st.right()
                pass
                
            elif cx < (camera.resolution[0] - dead_zone) / 2:
                st.left()
                pass
            
            elif cy < camera.resolution[1] / 2.6 - dead_zone / 2:
                st.backward()
                pass
            
            elif w < normal_size - size_dead_zone:
                st.forward()
                pass
                
            else:
                st.stop()
                pass
            
        else:
            st.stop()
        
        # show the frame
        #cv2.imshow("Frame", image)
        key = cv2.waitKey(1) & 0xFF

        # clear the stream in preparation for the next frame
        rawCapture.truncate(0)

        # if the `q` key was pressed, break from the loop
        if key == ord("q"):
            break
        

main_demo()
