from allen import Allen, Sensors, Motors
from module import Space, Module
from numpy import random as rng
from math import atan2
from math import pi

class PlanExecutor(Module):
    
    def __init__(self):
        self.achieved = True
        super().__init__("Go",0.1)
        
    def setGoal(self,x,y):
        self.goalX = x
        self.goalY = y
        self.achieved = False

    def get(self):
        return Sensors.x, Sensors.y
            
    def isGoalAchieved(self):
        return self.achieved
    
    def behavior(self):
        if self.achieved:
            return
        vX = self.goalX-Sensors.x
        vY = self.goalY-Sensors.y
        if abs(vX) < 0.2 and abs(vY) < 0.2:
            # stop
            Motors.forward = 0.0 
            Motors.turn = 0
            self.achieved = True
        else:
            true_angle = Sensors.compass
            goal_angle = atan2(-vY,vX)
            goal_angle = 2*pi + goal_angle if goal_angle < 0 else goal_angle
            goal_angle *= 180/pi
            goal_angle = 360 - goal_angle
            diff_angle = goal_angle - true_angle
            if goal_angle > 180:
                goal_angle -= 360
            elif goal_angle < -180:
                goal_angle += 360
            if abs(diff_angle) < 8: # we lowered this value so that the robot follows its path more exactly
                # go forward
                Motors.forward = 1.0 
                Motors.turn = 0
            else: 
                # turn 
                Motors.forward = 0.0
                Motors.turn = -1 if diff_angle < 0 else 1
        Motors.apply()

if __name__ == "__main__":

    robot = Allen.getInstance()
    robot.open()
    go = PlanExecutor()
    go.setGoal(2.6,0.0)
    
    input()
    go.stop()
    robot.close()