# --------------------------------------------------------------------------- #
# Includes
# --------------------------------------------------------------------------- #

import numpy as np

def track_ec(samples, threshold_tracker, r): # Creada Agosto 2024
    inferior, medio, superior = samples
    gap = np.abs(superior - inferior)
    midpoint = (inferior + superior) / 2

    if gap >= threshold_tracker: # Si la diferencia en valor absoluto entre los sensores superior e inferior es mayor a threshold_tracker, entramos acá
        if np.abs(medio - midpoint) < r * gap:
            return True # Si medio está a distancia a lo más r*|sup - inf| del punto medio entre sup e inf, puente está en la interfaz
        else:
            if medio >= midpoint + r * gap: # Si medio está más cerca de inferior, significa que el puente está en la parte clara y debe bajar un poco más
                return True
            else: # Si medio está más cerca de superior, significa que el puente debe quedarse quieto esperando que la interfaz baje un poco más
                return False 
    else: # Sensores superior e inferior en parte turbia (porque linea 12 es False), así que el motor no se mueve
        return False
        
def track_gs(samples, threshold_meas):
    if np.min(np.asarray(samples)) < threshold_meas: # Si el laser inferior es menor a threshold_meas, significa que la parte inferior del puente esta en la parte turbia, es decir puente alrededor de la interfaz
        return False
    else:
        return True # Motor se mueve si la parte inferior del puente no está en la parte turbia