import cv2
import time
from grovepi import *
from enum import Enum

hog = cv2.HOGDescriptor()
hog.setSVMDetector(cv2.HOGDescriptor_getDefaultPeopleDetector())

cap = cv2.VideoCapture(0)
cap.open("peatones-dublin.mp4", cv2.CAP_FFMPEG)

class Estado(Enum):
    ROJO = 1
    AMARILLO = 2
    VERDE = 3

class Ultrasonico:

    def __init__(self, pin):
        self.pin = pin
        self.activo = True
        self.distancia = 0

        pinMode(pin, "INPUT")

    def activar(self):
        self.activo = True

    def desactivar(self):
        self.activo = False

    def getDistancia(self):
        try:
            distance = ultrasonicRead(self.pin)
            return distance
        except IOError as e:
            print("Error: " + str(e))

class Camara:

    def __init__(self):
        self.activo = True
        self.cap = cv2.VideoCapture(0)

    def activar(self):
        self.activo = True

    def desactivar(self):
        self.activo = False

    def getImagen(self):
        ret, frame = self.cap.read()

        return ret, frame

class Buzzer:

    def __init__(self, pin):
        self.pin = pin
        self.activo = True
        self.volumen = 50

        pinMode(pin, "OUTPUT")

    def activar(self):
        self.activo = True

    def desactivar(self):
        self.activo = False

    # TODO
    def pulsar(self, tiempo):
        pass

    def setVolumen(self, newVolumen):
        self.volumen = newVolumen

class Led:

    def __init__(self, pin):
        self.pin = pin
        self.activo = False

        pinMode(pin, "OUTPUT")

    def prender(self):
        digitalWrite(self.pin, 1)

    def apagar(self):
        digitalWrite(self.pin, 0)

class Boton:

    def __init__(self, pin):
        self.pin = pin

        pinMode(pin, "INPUT")

    def getEstado(self):
        try:
            status = digitalRead(self.pin)
            print("Button pressed")
            return status
        except IOError as e:
            print("Error: " + str(e))

class Sistema:

    def __init__(self, pinUts = 2, pinBoton = 3, pinBuzzer = 4, \
                pinLedRojo = 5, pinLedAmarillo = 6, pinLedVerde = 7):
        self.ultrasonico = Ultrasonico(pinUts)
        self.boton = Boton(pinBoton)
        self.buzzer = Buzzer(pinBuzzer)
        self.ledRojo = Led(pinLedRojo)
        self.ledAmarillo = Led(pinLedAmarillo)
        self.ledVerde = Led(pinLedVerde)
        self.camara = Camara()
        self.timer = Timer()
        self.timerEstado = Timer()

        self.prioridadPeatones = 0
        self.prioridadVehiculos = 100
        self.cantidadPeatones = 0
        self.distanciaVehiculos = 0
        self.estado = Estado.ROJO

    def setEstado(self, nuevoEstado):
        self.estado = nuevoEstado

        if nuevoEstado == Estado.ROJO:
            self.ledAmarillo.apagar()
            self.ledVerde.apagar()
            self.ledRojo.prender()
        elif nuevoEstado == Estado.AMARILLO:
            self.ledVerde.apagar()
            self.ledRojo.apagar()
            self.ledAmarillo.prender()
        else:
            self.ledRojo.apagar()
            self.ledAmarillo.apagar()
            self.ledVerde.prender()

    def cambiarPaso(self):
        if self.estado == Estado.ROJO:
            self.setEstado(Estado.VERDE)

        elif self.estado == Estado.VERDE:
            self.setEstado(Estado.AMARILLO)
            time.sleep(3)
            self.setEstado(Estado.ROJO)

    def isPasoPeatones(self):
        return self.estado == Estado.ROJO

    # TODO
    def activarBuzzer(self):
        self.buzzer.activar()

    # TODO
    def calcularPrioridades(self):
        self.prioridadPeatones = 0
        self.prioridadVehiculos = 100

    def getCantidadPersonas(self):
        """Se usan HOGDescriptors para reconocer personas.
           Devuelve los rectángulos que encierran a las personas detectadas"""

        ret, frame = self.camara.getImagen()

        if not ret:
            print("Error obteniendo frame.")
            return -1

        # Reducir resolución para mayor rendimiento
        frame = cv2.resize(frame, (960, 540))
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

        personas, _ = hog.detectMultiScale(
            gray,
            winStride=(4,4),
            padding=(32,32),
            scale=1.3
            )

        return personas

class Timer:

    def __init__(self, startTime=time.time()):
        self.startTime = startTime
        self.currentTime = startTime

    def getDiferenciaTiempo(self):
        return self.currentTime - self.startTime

    def reiniciar(self):
        self.startTime = time.time()

    def actualizarTiempo(self):
        self.currentTime = time.time()

SEGUNDOS_SEMAFORO = 30
METROS_DISTANCIA_AUTO = 5
SEGUNDOS_INTERVALO_FOTO = 3
SEGUNDOS_INCREMENTO_TIMER = 3

def main():
    sistema = Sistema()
    personas = 0

    while True:
        distancia = sistema.ultrasonico.getDistancia()

        # Foto cada 3 segundos
        if sistema.timer.getDiferenciaTiempo() >= SEGUNDOS_INTERVALO_FOTO:
            personas = sistema.getCantidadPersonas()
            sistema.timer.reiniciar()

            if personas == -1:
                continue

        # Si hay personas esperando y ha pasado un segundo desde el último chequeo
        if sistema.isPasoPeatones() and sistema.timer.getDiferenciaTiempo() >= 1:
            sistema.timer.reiniciar()
            if distancia <= METROS_DISTANCIA_AUTO:
                sistema.timerEstado.currentTime += SEGUNDOS_INCREMENTO_TIMER

        # Si hay autos esperando y ha pasado un segundo desde el último chequeo
        elif not sistema.isPasoPeatones() and sistema.timer.getDiferenciaTiempo() >= 1:
            sistema.timer.reiniciar()
            if personas >= 3 and distancia >= METROS_DISTANCIA_AUTO:
                sistema.timerEstado.currentTime += SEGUNDOS_INCREMENTO_TIMER

        # Si el timer indica que hay que cambiar el paso
        if sistema.timerEstado.getDiferenciaTiempo() >= SEGUNDOS_SEMAFORO:
            sistema.cambiarPaso()
            sistema.timerEstado.reiniciar()

        sistema.timer.actualizarTiempo()
        sistema.timerEstado.actualizarTiempo()

if __name__ == "__main__":
    main()

    cap.release()
    cv2.destroyAllWindows()