# 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/config.py

# 
# Path: auv_control_pi/utils.py
# def micros():
#     return time.perf_counter() * 1e6
# 
# def elapsed_micros(start_time_us):
#     return (time.perf_counter() * 1e6) - start_time_us
# 
# def clamp_angle(deg):
#     """Rotate angle back to be within [0, 360]
#     """
#     n_rotations = deg // 360
#     deg -= 360 * n_rotations
#     return deg


import os
import asyncio
import logging
from ..wamp import ApplicationSession, rpc, subscribe
    from navio.lsm9ds1 import LSM9DS1
from ..config import config
from math import sqrt, atan2, asin, degrees, radians
from ..utils import micros, elapsed_micros, clamp_angle

    @rpc('ahrs.set_declination')
    def set_declination(self, val):
        self.declination = float(val)
        config.declination = self.declination
        config.save()

    @rpc('ahrs.set_board_offset')
    def set_board_offset(self, val):
        self.board_offset = float(val)
        config.board_offset = self.board_offset
        config.save()

    @subscribe('auv.update')
    def _simulate_heading(self, data):
        if SIMULATION:
            speed = data['turn_speed'] / 10
            self._simulated_heading += speed

    @property
    def heading(self):
        if SIMULATION:
            heading = self.declination + self.board_offset + self._simulated_heading
            return clamp_angle(heading)
        else:
            offset = self.declination + self.board_offset
            heading = (
                180 + degrees(radians(offset) + atan2(2.0 * (self.q[1] * self.q[2] + self.q[0] * self.q[3]),
                self.q[0] * self.q[0] + self.q[1] * self.q[1] - self.q[2] * self.q[2] - self.q[3] * self.q[3]))
            )

# 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/config.py

# 
# Path: auv_control_pi/utils.py
# def micros():
#     return time.perf_counter() * 1e6
# 
# def elapsed_micros(start_time_us):
#     return (time.perf_counter() * 1e6) - start_time_us
# 
# def clamp_angle(deg):
#     """Rotate angle back to be within [0, 360]
#     """
#     n_rotations = deg // 360
#     deg -= 360 * n_rotations
#     return deg


# Path: auv_control_pi/components/ahrs.py
import os
import asyncio
import logging
from ..wamp import ApplicationSession, rpc, subscribe
    from navio.lsm9ds1 import LSM9DS1
from ..config import config
from math import sqrt, atan2, asin, degrees, radians
from ..utils import micros, elapsed_micros, clamp_angle


    @rpc('ahrs.set_declination')
    def set_declination(self, val):
        self.declination = float(val)
        config.declination = self.declination
        config.save()

    @rpc('ahrs.set_board_offset')
    def set_board_offset(self, val):
        self.board_offset = float(val)
        config.board_offset = self.board_offset
        config.save()

    @subscribe('auv.update')
    def _simulate_heading(self, data):
        if SIMULATION:
            speed = data['turn_speed'] / 10
            self._simulated_heading += speed

    @property
    def heading(self):
        if SIMULATION:
            heading = self.declination + self.board_offset + self._simulated_heading
            return clamp_angle(heading)
        else:
            offset = self.declination + self.board_offset
            heading = (
                180 + degrees(radians(offset) + atan2(2.0 * (self.q[1] * self.q[2] + self.q[0] * self.q[3]),
                self.q[0] * self.q[0] + self.q[1] * self.q[1] - self.q[2] * self.q[2] - self.q[3] * self.q[3]))
            )

            return clamp_angle(heading)