class TestAlignementIle(TestCase):
    def setUp(self):
        self.positionZoneDepot = (800, 850)
        self.alignementIle = AlignementIle()


    def test_distance_laterale_negative_retourne_deplacement_gauche(self):
        distance_x = -300
        commande, _ = self.alignementIle.ajusterPositionX(distance_x)
        self.assertEqual('left', commande, "Ile se trouvant a gauche, mais retourne un deplacement a droite")

    def test_distance_laterale_positive_retourne_deplacement_droite(self):
        distance_x = 500
        commande, _ = self.alignementIle.ajusterPositionX(distance_x)
        self.assertEqual('right', commande, "Ile se trouvant a droite, mais retourne un deplacement a gauche")

    def test_distance_frontale_positive_retourne_deplacement_avant(self):
        distance_y = 300
        commande, _ = self.alignementIle.ajusterPositionY(distance_y)
        self.assertEqual('forward', commande, "Ile se trouvant en avant, mais retourne un deplacement recule")

    def test_distance_elevee_retourne_deux_ajustements(self):
        distance_x = 400
        distance_y = 320
        ajustements = self.alignementIle.calculerAjustement(distance_x, distance_y)
        self.assertEqual(len(ajustements), 2, "Ile se trouvant hors de portee du robot, mais ne retourne pas le bon nombre d'ajustement")

    def test_distance_laterale_negligeable_retourne_ajustement_frontal_seulement(self):
        distance_x = 28
        distance_y = 320
        ajustements = self.alignementIle.calculerAjustement(distance_x, distance_y)
        self.assertEquals(len(ajustements), 1, "Ile est alignee devant le robot, mais un alignement lateral est calculer")

    def test_distance_negligeable_retourne_aucun_ajustement(self):
        distance_x = 29
        distance_y = 23
        ajustements = self.alignementIle.calculerAjustement(distance_x, distance_y)
        self.assertEquals(ajustements, [], "Ile deja alignee, mais un alignement a ete calcule")
class DetectionIle(object):
    def __init__(self):
        self.alignementIle = AlignementIle()
        self.alignementTerminer = False
        self.positionZone = (810, 850)
        self.rayonZone = 100
        self.ajustements = None
        self._definirIntervallesCouleurs()

    def detecterIle(self, couleurIleCible, image):
        self.imageCamera = image
        self.couleurIle = couleurIleCible

        contoursIle = self._detecterCouleur(self.couleurIle)

        if (contoursIle is not None):
            distance_x, distance_y = self._evaluerEmplacement(contoursIle)
            self.ajustements = self.alignementIle.calculerAjustement(distance_x, distance_y)
        else:
            self.ajustements = None

    def _detecterCouleur(self, couleur):
        if (self.couleurIle == "vert"):
            return self.detecterFormeCouleur(self.intervalleVertTable5)
        elif (self.couleurIle == "jaune"):
            return self.detecterFormeCouleur(self.intervalleJauneTable5)
        elif (self.couleurIle == "bleu"):
            return self.detecterFormeCouleur(self.intervalleBleu)
        elif (self.couleurIle == "rouge"):
            return self.detecterFormeCouleur(self.intervalleRougeTable5)

    def _evaluerEmplacement(self, contoursIle):
        position_x, position_y = self._trouverCentreForme(contoursIle)
        positionZone_x, positionZone_y = self.positionZone
        distance_x = (position_x - positionZone_x)
        distance_y = (positionZone_y - position_y)
        _, rayon = cv2.minEnclosingCircle(contoursIle)
        return distance_x, distance_y

    def detecterFormeCouleur(self, intervalleCouleur):
        intervalleFonce, intervalleClair, couleurForme = intervalleCouleur
        masqueCouleur = cv2.inRange(self.imageCamera, intervalleFonce, intervalleClair)

        _, contoursCouleur, _ = cv2.findContours(masqueCouleur.copy(), cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
        contoursNegligeable = []

        for contours in range(len(contoursCouleur)):
            aire = cv2.contourArea(contoursCouleur[contours])
            if ((aire < 30000) or (aire > 180000)):
                contoursNegligeable.append(contours)

        if (len(contoursNegligeable) > 0):
            contoursCouleur = np.delete(contoursCouleur, contoursNegligeable)

        if (len(contoursCouleur) != 0):
            return contoursCouleur[0]
        else:
            return None

    def _trouverCentreForme(self, contoursForme):
        MatriceCentreMasse = cv2.moments(contoursForme)
        centre_x = int(MatriceCentreMasse['m10'] / MatriceCentreMasse['m00'])
        centre_y = int(MatriceCentreMasse['m01'] / MatriceCentreMasse['m00'])
        return centre_x, centre_y

    def _definirIntervallesCouleurs(self):
        self.intervalleRouge = np.array([15, 0, 75]), np.array([100, 65, 200]), "Rouge"
        self.intervalleBleu = np.array([100, 100, 0]), np.array([190, 170, 80]), "Bleu"
        self.intervalleJaune = np.array([0, 50, 50]), np.array([50, 255, 255]), "Jaune"
        self.intervalleVert = np.array([50, 120, 40]), np.array([100, 170, 80]), "Vert"

        self.intervalleVertTable5 = np.array([0, 60, 0]), np.array([100, 200, 80]), "Vert2"

        self.intervalleRougeTable5 = np.array([15, 0, 75]), np.array([100, 65, 200]), "Rouge2"
        self.intervalleJauneTable5 = np.array([0, 50, 50]), np.array([50, 255, 255]), "Jaune2"

    def _afficherFeed(self):
        cv2.imshow("Image", self.imageCamera)
        cv2.waitKey(0)