class Trajectoire(TestCase):
    def setUp(self):
        self.carte = Carte()
        self.carte.ajouterElementCarto(self.analyseImageWorld.elementsCartographiques)
        self.carte.trajectoire.initGrilleCellule([])

    def test_trouverTrajet(self):
        depart = (random.randrange(5,1595), random.randrange(5,845))
        arrive = (random.randrange(5,1595), random.randrange(5,845))
        self.carte.trajectoire.trouverTrajet(depart, arrive)
        bool = self.trajetValide(self.carte.trajectoire.tra, depart, arrive)

        self.assertTrue(bool)

    def trajetValide(self, trajet, arrive, depart):
        if (trajet == []):
            return True
        elif (depart[0]-6 < trajet[0][0] and trajet[0][0] < depart[0]+6) and (depart[1]-9 < trajet[1][1] and trajet[1][1] < depart[1]+9):
            return True
        else:
            return False

    def test_trouverLongueurTrajetCarre(self):
        depart = (0, 0)
        milieu = (0, 855)
        arrive = (0, 1600)
        trajet = [depart, milieu, arrive]
        dist = self.carte.trajectoire.trouverLongueurTrajetCarre(trajet)
        bool = 110**2 + 220**2 < dist < 120**2 + 230**2

        self.assertTrue(bool)

    def test_distanceAuCarre(self):
        self.algo.distanceAuCarre(0, 0, 8, 8)
        self.assertTrue(self.carte.trajectoire.grilleCellule.distanceAuCarre.called)
class TestAlgorithmeTrajectoire(TestCase):
    def setUp(self):
        self.carte = Carte()
        self.carte.ajouterElementCarto(self.analyseImageWorld.elementsCartographiques)
        self.carte.trajectoire.initGrilleCellule([])
        self.algo = AlgorithmeTrajectoire(self.carte.trajectoire.grilleCellule)

    def test_trouverTrajet(self):
        depart = (random.randrange(5,1595), random.randrange(5,845))
        arrive = (random.randrange(5,1595), random.randrange(5,845))
        self.algo.trouverTrajet(depart, arrive)
        bool = self.trajetValide(self.carte.trajectoire.tra, depart, arrive)

        self.assertTrue(bool)

    def trajetValide(self, trajet, arrive, depart):
        if (trajet == []):
            return True
        elif (depart[0]-6 < trajet[0][0] and trajet[0][0] < depart[0]+6) and (depart[1]-9 < trajet[1][1] and trajet[1][1] < depart[1]+9):
            return True
        else:
            return False

    def test_simplifierTrajectoire(self):
        depart = (20, 20)
        milieuInutile = (20, 400)
        arrive = (20, 820)
        self.algo.trajet = [depart, milieuInutile, arrive]
        self.algo.simplifierTrajet()

        self.assertEqual(len(self.carte.trajectoire.trajectoire), 2)

    def test_SectionnerTrajectoire(self):
        depart = (20, 20)
        arrive = (20, 820)
        self.algo.trajet = [depart, arrive]
        self.algo.sectionnerTrajet()
        bool = len(self.algo.trajet) >= 2

        self.assertTrue(bool)

    def test_eliminerDetourInutile(self):
        depart = (20, 20)
        milieuInutile = (200, 400)
        arrive = (20, 820)
        self.algo.trajet = [depart, milieuInutile, arrive]
        self.algo.eliminerDetourInutile()
        bool = len(self.algo.trajet) == 2

        self.assertTrue(bool)

    def test_distanceAuCarre(self):
        self.algo.distanceAuCarre(0, 0, 8, 8)
        self.assertTrue(self.algo.grilleCellule.distanceAuCarre.called)
 def __init__(self, numeroTable):
     Thread.__init__(self)
     self.startTimer = default_timer()
     self.numeroTable = numeroTable
     self.trajectoireReel = None
     self.trajectoirePrevue = None
     self.angleDesire = None
     self.tensionCondensateur = "0"
     self.descriptionIleCible = "?"
     self.manchester = "?"
     self.roundTerminee = False
     self.rapport = 0.84
     self.coordonneeXMilieu = 787
     self.coordonneeYMilieu = 419
     self.carte = Carte()
     self.demarrerConnectionTCP()
     self.demarrerFeedVideo()
     self.demarrerAnalyseImageWorld()
     self.demarrerImageVirtuelle()
 def setUp(self):
     self.carte = Carte()
     self.carte.ajouterElementCarto(self.analyseImageWorld.elementsCartographiques)
     self.carte.trajectoire.initGrilleCellule([])
class StationBase(Thread):
    def __init__(self, numeroTable):
        Thread.__init__(self)
        self.startTimer = default_timer()
        self.numeroTable = numeroTable
        self.trajectoireReel = None
        self.trajectoirePrevue = None
        self.angleDesire = None
        self.tensionCondensateur = "0"
        self.descriptionIleCible = "?"
        self.manchester = "?"
        self.roundTerminee = False
        self.rapport = 0.84
        self.coordonneeXMilieu = 787
        self.coordonneeYMilieu = 419
        self.carte = Carte()
        self.demarrerConnectionTCP()
        self.demarrerFeedVideo()
        self.demarrerAnalyseImageWorld()
        self.demarrerImageVirtuelle()

    def run(self):
        self.attendreFinDeDetectionPrimaire()
        self.initialisationTrajectoire()
        self.attendreRobotPret()
        self.demarerRoutine()
        time.sleep(10)

    def demarerRoutine(self):
        self.deplacement(ETAPE_RECHARGE)
        self.aligner(ETAPE_ALIGNEMENT_STATION)
        self.trouverTresorEtCible()
        self.attendreRobot()
        self.attendreThreadCible()
        while not self.threadCommunication.tresorTrouve:
            self.deplacement(ETAPE_TRESOR)
            self.aligner(ETAPE_ALIGNEMENT_TRESOR)
            time.sleep(0.01)
        self.deplacement(ETAPE_ILE)
        self.aligner(ETAPE_ALIGNEMENT_ILE)
        self.roundTerminee = True

    def deplacement(self, type):
        print '\n--------------------------------------------------'
        print 'Etape de deplacement : %s' % type
        print '--------------------------------------------------'
        destination = self.identifierDestination(type)
        self.trouverTrajectoirePrevu(destination, type)
        while (self.trajectoireReel is not None) and (len(self.trajectoireReel) > MIN_TRAJECTOIRE):
            self.orienter('deplacement')
            self.deplacer()
        self.correctionsFinales(type)
        print '\n--------------------------------------------------'
        print 'Arriver.'
        print '--------------------------------------------------'

    def identifierDestination(self, etape):
        print '\nIndentifier destination'
        destination = None
        if etape == ETAPE_RECHARGE:
            destination = self.carte.getStationRecharge().getCentre()
        elif etape == ETAPE_TRESOR:
            destination = self.carte.cible.tresorChoisi.getCentre()
            print 'identifier destination tresor'
            print destination
        elif etape == ETAPE_ILE:
            destination = self.carte.cible.ileChoisie.getCentre()
        if destination is None:
            print 'erreur! Aucune destination trouvee.'
        return destination

    def correctionsFinales(self, type):
        if type == ETAPE_RECHARGE:
            self.angleDesire = 90
            self.orienter(type)
            self.deplacementArriere(5)
            self.deplacementDroit(10)
        elif type == ETAPE_TRESOR:
            if self.carte.getCible().getTresorCible().getCentre()[1] < MAX_CENTRE_TRESOR:
                self.angleDesire = 90
                self.carte.cible.conteur += 1
            else:
                self.angleDesire = 270
                self.carte.cible.conteur += 1
            self.orienter(type)
            self.deplacementArriere(7)
        elif type == ETAPE_ILE:
            arriver = self.carte.getCible().getIleCible().getCentre()
            debut = self.getPositionRobot()
            print 'arrive: ', arriver
            print 'robot: ', debut
            debut2 = self.correctionCentre(debut)
            print 'robot2: ', debut2
            self.angleDesire = self.trouverOrientationDesire(debut2, arriver)
            print self.angleDesire
            self.orienter(type)
        self.trajectoirePrevue = None

    def aligner(self, type):
        print '\n--------------------------------------------------'
        print 'Debuter l''alignement.'
        print '--------------------------------------------------'
        int = 0
        if type == ETAPE_ALIGNEMENT_ILE:
            couleur = self.carte.getCible().getIleCible().getCouleur()
            if couleur == COULEUR_VERT:
                int = 0
            elif couleur == COULEUR_BLEU:
                int = 1
            elif couleur == COULEUR_JAUNE:
                int = 2
            elif couleur == COULEUR_ROUGE:
                int = 3
        RequeteJSON(type, int)
        self.threadCommunication.signalerEnvoyerCommande()
        if not type == ETAPE_ALIGNEMENT_STATION:
            self.attendreRobot()
        print '\n--------------------------------------------------'
        print 'Allignement termine.'
        print '--------------------------------------------------'

    def trouverTrajectoirePrevu(self, destination, type):
        print '\nTrouve la trajectoire prevu...'
        self.trajectoirePrevue = self.carte.trajectoire.trouverTrajet(self.getPositionRobot(), copy.deepcopy(destination), type)
        if self.trajectoirePrevue is None:
            print 'erreur! Aucun trajet trouve.'
        else:
            print 'trajet trouve.'
        self.trajectoireReel = copy.deepcopy(self.trajectoirePrevue)

    def orienter(self, type):
        print '\nOrienter'
        conteur = 0
        while 1:
            if self.angleDesire is None:
                arriver = self.trajectoireReel[-2]
                debut = self.getPositionRobot()
                self.angleDesire = self.trouverOrientationDesire(debut, arriver)
            angle = self.trouverDeplacementOrientation()
            if -3 <= angle <= 3:
                print '\nOrientation termine.'
                break
            if conteur >= 2:
                if angle > 0:
                    angle = 1
                else:
                    angle = -1
            if angle >= 0:
                RequeteJSON("rotateClockwise", angle)
            else:
                RequeteJSON("rotateAntiClockwise", abs(angle))
            print 'Signaler que la comande est prete a envoyer.'
            self.threadCommunication.signalerEnvoyerCommande()
            time.sleep(0.5)
            self.attendreRobot()
            if type == 'deplacement':
                self.angleDesire = None
            conteur += 1
        self.angleDesire = None

    def trouverOrientationDesire(self, debut, arriver):
        deltaX = arriver[0]-debut[0]
        deltaY = -1*(arriver[1]-debut[1])
        if not deltaX == 0:
            pente = deltaY/deltaX

        if deltaY == 0 and deltaX < 0:
            angle = 180
        elif deltaY == 0 and deltaX > 0:
            angle = 0
        elif deltaX == 0 and deltaY > 0:
            angle = 90
        elif deltaX == 0 and deltaY < 0:
            angle = 270
        elif deltaX > 0 and deltaY > 0:
            angle = int(round(math.degrees(math.atan(pente))))
        elif deltaX > 0 and deltaY < 0:
            angle = 360 + int(round(math.degrees(math.atan(pente))))
        elif deltaX < 0:
            angle = 180 + int(round(math.degrees(math.atan(pente))))

        return angle

    def trouverDeplacementOrientation(self):
        angleRobot = self.getOrientationRobot()
        print 'angle du robot: ', angleRobot
        print 'angle desire: ', self.angleDesire
        depDegre = angleRobot - self.angleDesire
        if depDegre < -180:
            depDegre = depDegre + 360
        elif depDegre > 180:
            depDegre = depDegre - 360
        print 'correction: ', depDegre

        return depDegre

    def deplacer(self):
        print '\nDeplacer'
        arriver = self.trajectoireReel[-2]
        debut = self.getPositionRobot()
        dep = self.distanceADestination(debut[0], debut[1], arriver[0], arriver[1])
        dep = int(round(dep))
        print 'deplacement: ', dep
        RequeteJSON("forward", dep)
        self.threadCommunication.signalerEnvoyerCommande()
        self.attendreRobot()
        debut = self.getPositionRobot()
        dep = self.distanceAuCarre(debut[0], debut[1], arriver[0], arriver[1])
        print '\nArriver.'
        if len(self.trajectoireReel) == 2:
            self.trajectoireReel = None
        else:
            self.trajectoireReel.pop(-1)

    def deplacementDroit(self, dep):
        print '\nDeplacer'
        print 'deplacement: ', dep
        RequeteJSON("right", dep)
        self.threadCommunication.signalerEnvoyerCommande()
        self.attendreRobot()

    def deplacementArriere(self, dep):
        print '\nDeplacer'
        print 'deplacement: ', dep
        RequeteJSON("backward", dep)
        self.threadCommunication.signalerEnvoyerCommande()
        self.attendreRobot()

    def deplacementAvant(self, dep):
        print '\nDeplacer'
        print 'deplacement: ', dep
        RequeteJSON("forward", dep)
        self.threadCommunication.signalerEnvoyerCommande()
        self.attendreRobot()

    def decoderManchester(self):
        RequeteJSON("decoderManchester", 0)
        self.threadCommunication.signalerEnvoyerCommande()
        self.attendreRobot()

    def demarrerFeedVideo(self):
        self.threadVideo = FeedVideoStation()
        self.threadVideo.start()

    def demarrerConnectionTCP(self):
        self.threadCommunication = StationServeur(self)
        self.threadCommunication.start()

    def demarrerAnalyseImageWorld(self):
        self.threadAnalyseImageWorld = AnalyseImageWorld(self)
        self.threadAnalyseImageWorld.start()

    def demarrerImageVirtuelle(self):
        self.threadImageVirtuelle = ImageVirtuelle(self)
        self.threadImageVirtuelle.start()

    def initialisationTrajectoire(self):
        self.carte.getTrajectoire().initGrilleCellule(self.carte.getIles())

    def trouverTresorEtCible(self):
        self.threadCible = TrouverTresorEtCible(self)
        self.threadCible.start()

    def attendreThreadCible(self):
        while self.threadCible.isAlive():
            time.sleep(0.01)
        print 'Cible trouve'

    def attendreRobotPret(self):
        print '\nAttendre que le robot soit pret...'
        while 1:
            if self.threadCommunication.getRobotPret():
                print 'Robot est pret'
                break
            time.sleep(0.01)

    def attendreRobot(self):
        self.threadCommunication.debuteAttenteDuRobot()
        while self.threadCommunication.getAttenteDuRobot():
            time.sleep(0.01)
        time.sleep(0.1)
        print 'Robot a fini.'

    def attendreCible(self):
        while self.carte.getCible() is None:
            time.sleep(0.01)

    def attendreFinDeDetectionPrimaire(self):
        while not self.threadAnalyseImageWorld.detectionPrimaireFini:
            time.sleep(0.01)

    def attendreFeed(self):
        while self.threadVideo.captureTable is None:
            time.sleep(0.01)

    def attendreImageVirtuelle(self):
        while self.threadImageVirtuelle.imageVirtuelle is None:
            time.sleep(0.01)

    def distanceAuCarre(self, x, y, x2, y2):
        return self.carte.getTrajectoire().distanceAuCarre(x, y, x2, y2)

    def distanceADestination(self, x, y, x2, y2):
        return math.sqrt(self.distanceAuCarre(x, y, x2, y2))

    def getImage(self):
        return copy.deepcopy(self.threadVideo.captureTable)

    def setRobot(self, robot):
        self.carte.robot = robot

    def getCarte(self):
        return self.carte

    def getManchester(self):
        return self.manchester

    def setManchester(self, lettre):
        self.manchester = lettre

    def getTensionCondensateur(self):
        return self.tensionCondensateur

    def setTensionCondensateur(self, tension):
        self.tensionCondensateur = tension

    def getTrajectoirePrevue(self):
        return self.trajectoirePrevue

    def getPositionRobot(self):
        centre = self.carte.getRobotValide().getCentre()
        return centre

    def getOrientationRobot(self):
        return self.carte.getRobotValide().orientation

    def getNumTable(self):
        return self.numeroTable

    def correctionCentre(self, centre):
        xNonCorrige = centre[0]
        deltaX = xNonCorrige - self.coordonneeXMilieu
        xCorriger = int(round(self.coordonneeXMilieu + (deltaX * self.rapport)))
        yNonCorrige = centre[1]
        deltaY = yNonCorrige - self.coordonneeYMilieu
        yCorriger = int(round(self.coordonneeYMilieu + (deltaY * self.rapport)))

        return xCorriger, yCorriger