# Path: auv_control_pi/config.py

# 
# Path: auv_control_pi/utils.py
# def elapsed_micros(start_time_us):
# def micros():
# def clamp_angle(deg):
# def point_at_distance(distance, heading, current_point):
# def heading_to_point(point_a, point_b):
# def distance_to_point(point_a, point_b):
# def get_error_angle(target, heading):
# 
# Path: auv_control_pi/wamp.py
# class ApplicationSession(AutobahnApplicationSession):
#     name = ''
# 
#     @classmethod
#     def rpc_methods(cls, instance=None):
#         _cls = instance or cls
#         attrs = dir(_cls)
#         attrs.remove('rpc_methods')
#         methods = [getattr(_cls, attr) for attr in attrs if callable(getattr(_cls, attr))]
#         rpc_methods = [method for method in methods if getattr(method, 'is_rpc', False)]
#         return {method.rpc_uri: method for method in rpc_methods}
# 
#     @classmethod
#     def rpc_uris(cls, instance=None):
#         return cls.rpc_methods(instance).keys()
# 
#     @classmethod
#     def subcribtion_handlers(cls, instance=None):
#         _cls = instance or cls
#         attrs = dir(_cls)
#         attrs.remove('subcribtion_handlers')
#         methods = [getattr(_cls, attr) for attr in attrs if callable(getattr(_cls, attr))]
#         handlers = [method for method in methods if getattr(method, 'is_subcription', False)]
#         return {method.topic: method for method in handlers}
# 
#     def onConnect(self):
#         logger.info('Connecting to {} as {}'.format(self.config.realm, self.name))
#         self.join(realm=self.config.realm)
# 
#     async def onJoin(self, details):
#         logger.info('Joined realm as {}'.format(self.name))
#         for rpc_uri, method in self.rpc_methods(self).items():
#             logger.debug('Registering RPC: {}'.format(rpc_uri))
#             await self.register(method, rpc_uri)
# 
#         for topic, handler in self.subcribtion_handlers(self).items():
#             logger.debug('Subscribing To Topic: {}'.format(topic))
#             await self.subscribe(handler, topic)
# 
#         loop = asyncio.get_event_loop()
#         loop.create_task(self.update())
# 
# def rpc(rpc_uri=None):
#     if rpc_uri is None:
#         raise ValueError('Must provide rpc_uri')
# 
#     def outer(fnc):
#         @wraps(fnc)
#         def inner(*args, **kwargs):
#             return fnc(*args, **kwargs)
#         inner.is_rpc = True
#         inner.rpc_uri = rpc_uri
#         return inner
#     return outer
# 
# def subscribe(topic):
#     if topic is None:
#         raise ValueError('Must provide `topic `to subscribe to')
# 
#     def outer(fnc):
#         @wraps(fnc)
#         def inner(self, *args, **kwargs):
#             return fnc(self, *args, **kwargs)
#         inner.is_subcription = True
#         inner.topic = topic
#         return inner
#     return outer


import os
import asyncio
import logging
from collections import deque
from simple_pid import PID
from auv_control_pi.config import config
from auv_control_pi.utils import Point, distance_to_point, heading_to_point, get_error_angle
from auv_control_pi.wamp import ApplicationSession, rpc, subscribe


SIMULATION = os.getenv('SIMULATION', False)
logger = logging.getLogger(__name__)


class Navitgator(ApplicationSession):

    def __init__(self, *args, **kwargs):
        super().__init__(*args, **kwargs)
        self.enabled = False
        self.heading = None
        self.target_heading = None
        self.current_location = None
        self.target_waypoint = None
        self.update_frequency = 10
        self.arrived = False
        self.waypoints = deque()
        self.completed_waypoints = []
        # the pid setpoint is the error setpoint
        # and thus we always want the error to be 0 regardless of the scale
        # we use to feed into the pid.
        self.pid = PID(config.kP, config.kI, config.kD, setpoint=0, output_limits=(-100, 100))
        self.pid_output = None
        self.heading_error = None

    @subscribe('ahrs.update')
    def _update_ahrs(self, data):
        self.heading = data.get('heading', None)


# Path: auv_control_pi/config.py

# 
# Path: auv_control_pi/utils.py
# def elapsed_micros(start_time_us):
# def micros():
# def clamp_angle(deg):
# def point_at_distance(distance, heading, current_point):
# def heading_to_point(point_a, point_b):
# def distance_to_point(point_a, point_b):
# def get_error_angle(target, heading):
# 
# Path: auv_control_pi/wamp.py
# class ApplicationSession(AutobahnApplicationSession):
#     name = ''
# 
#     @classmethod
#     def rpc_methods(cls, instance=None):
#         _cls = instance or cls
#         attrs = dir(_cls)
#         attrs.remove('rpc_methods')
#         methods = [getattr(_cls, attr) for attr in attrs if callable(getattr(_cls, attr))]
#         rpc_methods = [method for method in methods if getattr(method, 'is_rpc', False)]
#         return {method.rpc_uri: method for method in rpc_methods}
# 
#     @classmethod
#     def rpc_uris(cls, instance=None):
#         return cls.rpc_methods(instance).keys()
# 
#     @classmethod
#     def subcribtion_handlers(cls, instance=None):
#         _cls = instance or cls
#         attrs = dir(_cls)
#         attrs.remove('subcribtion_handlers')
#         methods = [getattr(_cls, attr) for attr in attrs if callable(getattr(_cls, attr))]
#         handlers = [method for method in methods if getattr(method, 'is_subcription', False)]
#         return {method.topic: method for method in handlers}
# 
#     def onConnect(self):
#         logger.info('Connecting to {} as {}'.format(self.config.realm, self.name))
#         self.join(realm=self.config.realm)
# 
#     async def onJoin(self, details):
#         logger.info('Joined realm as {}'.format(self.name))
#         for rpc_uri, method in self.rpc_methods(self).items():
#             logger.debug('Registering RPC: {}'.format(rpc_uri))
#             await self.register(method, rpc_uri)
# 
#         for topic, handler in self.subcribtion_handlers(self).items():
#             logger.debug('Subscribing To Topic: {}'.format(topic))
#             await self.subscribe(handler, topic)
# 
#         loop = asyncio.get_event_loop()
#         loop.create_task(self.update())
# 
# def rpc(rpc_uri=None):
#     if rpc_uri is None:
#         raise ValueError('Must provide rpc_uri')
# 
#     def outer(fnc):
#         @wraps(fnc)
#         def inner(*args, **kwargs):
#             return fnc(*args, **kwargs)
#         inner.is_rpc = True
#         inner.rpc_uri = rpc_uri
#         return inner
#     return outer
# 
# def subscribe(topic):
#     if topic is None:
#         raise ValueError('Must provide `topic `to subscribe to')
# 
#     def outer(fnc):
#         @wraps(fnc)
#         def inner(self, *args, **kwargs):
#             return fnc(self, *args, **kwargs)
#         inner.is_subcription = True
#         inner.topic = topic
#         return inner
#     return outer


# Path: auv_control_pi/components/navigation.py
import os
import asyncio
import logging
from collections import deque
from simple_pid import PID
from auv_control_pi.config import config
from auv_control_pi.utils import Point, distance_to_point, heading_to_point, get_error_angle
from auv_control_pi.wamp import ApplicationSession, rpc, subscribe



SIMULATION = os.getenv('SIMULATION', False)
logger = logging.getLogger(__name__)


class Navitgator(ApplicationSession):

    def __init__(self, *args, **kwargs):
        super().__init__(*args, **kwargs)
        self.enabled = False
        self.heading = None
        self.target_heading = None
        self.current_location = None
        self.target_waypoint = None
        self.update_frequency = 10
        self.arrived = False
        self.waypoints = deque()
        self.completed_waypoints = []
        # the pid setpoint is the error setpoint
        # and thus we always want the error to be 0 regardless of the scale
        # we use to feed into the pid.
        self.pid = PID(config.kP, config.kI, config.kD, setpoint=0, output_limits=(-100, 100))
        self.pid_output = None
        self.heading_error = None

    @subscribe('ahrs.update')
    def _update_ahrs(self, data):
        self.heading = data.get('heading', None)


    @subscribe('gps.update')