#!/usr/bin/env python3

#LIBRERIAS
import spidev #Libreria SPI utiizada para hacer la comunicación entre el adc MCP3008
import RPi.GPIO as GPIO
import time
import cv2
import imutils
import numpy as np
import requests

#SPI
spi = spidev.SpiDev()
spi.open(0,0)
spi.max_speed_hz=1000000

#CAMARA IN
cam= cv2.VideoCapture(0)

#Rango de color (azul)
lower = np.array([120,80,100], np.uint8)
upper = np.array([130,255,255], np.uint8)

#GPIO Motor
intMotor = 12
icont=1
a = 1
b = 1
r = 1
EnvA = 0
EnvB = 0
EnvR = 0
numEnv = 0
flagg=False
fil_azul = False
rechazo = False
falsito = False


# GPIO MOTOR
GPIO.setmode(GPIO.BCM)
GPIO.setup(intMotor, GPIO.OUT)

#FUNCION SENSOR
def readChannel(channel):
  val = spi.xfer2([1,(8+channel)<<4,0])
  data = ((val[1]&3) << 8) + val[2]
  return data

#FUNCIONES ON/OFF MOTOR
def motor_on(pin):
    GPIO.output(pin, GPIO.HIGH)  # Turn motor on
def motor_off(pin):
    GPIO.output(pin, GPIO.LOW)  # Turn motor off
    
#lectura del sensor
def lect():
    v=(readChannel(0)/1023.0)*3.3
    dist = 16.2537 * v**4 - 129.893 * v**3 + 382.268 * v**2 - 512.611 * v + 301.439
    distancia = int(dist)#round(float(dist),1)
    return distancia    
    
#funcion preterminado
def inicio(rest):
    dista = lect()
    falsito1 = falsito
    it = rest
    nube = numEnv
    fa = a
    fb = b
    fr = r
    az = fil_azul
    rechi = rechazo
    contador = icont
    recon = None
    if dista < 21:
        nube = -1
        fa = 0
        fb = 0
        fr = 0
        az = False
        rechi = False
        falsito1 = False
        contador = -1
        recon = True
        motor_on(intMotor)
    if dista < 10:
        recon = False
    if dista > 21:
        recon = True
    if dista > 80:
        motor_off(intMotor)
        recon = False
        fa = 0
        fb = 0
        fr = 0
    return it, nube, fa, fb, fr, az, rechi, contador, falsito1, recon

def paro(pp):
    dista = lect()
    pp = falsito
    if dista > 20 or dista < 150 and pp == False:
        motor_off(intMotor)
    bs = "Banda detenida"
    return bs

def activar(par):
    dista = lect()
    rech = rechazo
    if dista < 70:
        if par == True or rech == True:
            motor_on(intMotor)
        #print("Banda Activa")
    else:
        motor_off(intMotor)
    return

    
def detecCamara():
    dista = lect()
    flag1 = False
    par = True
    fal = falsito
    if dista > 30:
        paro(fal)
        flag1= True
    return flag1, par
        
def detecBrazo():
    dista = lect()
    flagy = flagg
    stop = False
    flo = False
    pari = False
    rech = rechazo
    fal = falsito
    if dista > 52:
        if rech == False:
            pari = False
            flagy = False
            paro(fal)
            flo = True
    return flo, pari   
        
if __name__ == "__main__":
 
 EnvI = -1
#  enviarR = requests.get("https://api.thingspeak.com/update?api_key=W259HB8Q70VXD68L&field4="+str(EnvI))

 while(1):
    _, frame = cam.read()
    grayImage=cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)
    image=cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)
    output=frame.copy()
    mask =cv2.inRange(image,lower,upper)
    
    valor_color= cv2.findContours(mask,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
    valor_color= imutils.grab_contours(valor_color)
    
    circles=cv2.HoughCircles(grayImage,cv2.HOUGH_GRADIENT,2,10000,
                             param1=200,
                             param2= 10 ,
                             minRadius=2,
                             maxRadius=35)
    
    circlesR=cv2.HoughCircles(grayImage,cv2.HOUGH_GRADIENT,2,10000,
                             param1=100,
                             param2=30,
                             minRadius=40,
                             maxRadius=60)
    
    circles=np.uint16(np.around(circles))
    circlesR=np.uint16(np.around(circlesR))
    
    for circuloActual in circles[0,:]:
        centroX = circuloActual[0]
        centroY = circuloActual[1]
        radio = circuloActual[2]
        cv2.circle(output,(centroX,centroY),radio,(0,255,0),2)
        cv2.circle(output,(centroX,centroY),2,(0,255,0),3)
        
    for circuloActualR in circlesR[0,:]:
        centroXR = circuloActualR[0]
        centroYR = circuloActualR[1]
        radio = circuloActualR[2]
        cv2.circle(output,(centroXR,centroYR),radio,(0,0,255),2)
        cv2.circle(output,(centroXR,centroYR),2,(0,0,255),3)
        
    #print("x",centroX)
    #print("y",centroY)
        
    for c in valor_color:
        area = cv2.contourArea(c)
        if area>30000:
            nuevocontorno=cv2.convexHull(c)
            cv2.drawContours(frame,[nuevocontorno],0,(255,0,0),3)
            m = cv2.moments(c)
            if (m["m00"]==0): m["m00"]=1
            cx = int(m["m10"]/m["m00"])
            cy = int(m["m01"]/m["m00"])
            cv2.circle(frame,(cx,cy),7,(255,255,255),-1)
            cv2.putText(frame,"Azul",(cx-20,cy-20),cv2.FONT_ITALIC,2,(255,255,255),2)
            fil_azul = True

    cv2.imshow('Deteccion Circulo',output)
    #cv2.imshow("Mascara",mask)
    cv2.imshow("Deteccion Color",frame)
    
    e,d = detecCamara()
    ai,bi = detecBrazo()
    f,h,j,k,l,m, rec, conte, sito, reccn = inicio(1)    
    numEnv = h
    a = j
    b = k
    r = l
    fil_azul = m
    rechazo = rec
    icont = conte
    EnvI = f
    falsito = sito
    #print(reccn)
    if reccn == True:
        icont+=1
        
        print(icont)
        
    if icont == 30 and a < 7 and b < 15 and r < 15:
        rechazo = True
        falsito = False
        print("No detectado:", rechazo)
        EnvR += 1
        enviarR = requests.get("https://api.thingspeak.com/update?api_key=W259HB8Q70VXD68L&field1="+str(EnvA)+"&field2="+str(EnvB)+"&field3="+str(EnvR)+"&field4="+str(EnvI))

        
    if 800 > centroX > 150 and 280 > centroY > 230 :
        if fil_azul == True:
            a += 1
            print("Azul: ", a)
            if icont == 30 and a > 9:
                print("Agnolish Azul")
                rechazo = False
                falsito = True
                b = 0
                r = 0
                EnvA += 1
                while numEnv < 2:
                    print("Enviando")
                    enviarR = requests.get("https://api.thingspeak.com/update?api_key=W259HB8Q70VXD68L&field1="+str(EnvA)+"&field2="+str(EnvB)+"&field3="+str(EnvR)+"&field4="+str(EnvI))
                    numEnv = numEnv + 1                   
        else:
            b += 1
            #print("Blanco: ", b)    
            if icont == 30 and b > 15:
                print("Agnolish Blanco")
                rechazo = False
                falsito = True
                par = bi
                #print (par)
                a = 0
                r = 0
                EnvB += 1
                while numEnv < 2:
                    print("Enviando")
                    enviarR = requests.get("https://api.thingspeak.com/update?api_key=W259HB8Q70VXD68L&field1="+str(EnvA)+"&field2="+str(EnvB)+"&field3="+str(EnvR)+"&field4="+str(EnvI))
                    numEnv = numEnv + 1
                
    if 800 > centroXR > 150 and 264 > centroYR > 245:
        r += 1
        #print("Rech: ", r)
        if icont == 30 and r > 20:
            print("Rechazo")
            falsito = True
            a = 0
            b = 0
            EnvR += 1
            while numEnv < 2:
                print("Enviando")
                enviarR = requests.get("https://api.thingspeak.com/update?api_key=W259HB8Q70VXD68L&field1="+str(EnvA)+"&field2="+str(EnvB)+"&field3="+str(EnvR)+"&field4="+str(EnvI))
                numEnv = numEnv + 1
            rechazo = True
            print("Rechazo oficial: ", rechazo)
       
    if icont > 30 and ai == False:
        par = falsito
        #print (par)
        activar(par)
    if falsito == True:
        par = rechazo
        #print (par)
        activar(par)
    else:
        ai = bi
        
    if ai == True:
            d = False
            par = bi
            #print (par)
            icont=1
 
    if cv2.waitKey(1) == 27:
        break

cam.release()   
cv2.destroyAllWindows()
sys.exit()