import collections
import datetime
import inspect
import logging
import math

from threading import Thread, RLock

import numpy as np
from shapely.geometry import LineString, MultiLineString

from osgar.lib import quaternion


g_logger = logging.getLogger(__name__)


def log(*args):
    up = inspect.currentframe().f_back
    caller = up.f_code.co_name
    caller_class = up.f_locals['self'].__class__
    msg = f"{caller_class.__name__}.{caller}, " + ", ".join(str(a) for a in args)
    g_logger.info(msg)


class SimLogger:

    def __init__(self):
        self.lock = RLock()
        self.start_time = datetime.datetime.now(datetime.timezone.utc)
        self.names = ["sys"]

    def register(self, name):
        self.names.append(name)
        return len(self.names)-1

    def write(self, stream_id, data, dt=None):
        dt = datetime.datetime.now(datetime.timezone.utc) - self.start_time
        return dt


class Simulation:

    def __init__(self, bus, end_condition, world=None):
        bus.register('scan', 'rot', 'orientation', 'sim_time_sec', 'origin', 'pose2d', 'acc', 'artf', 'pose3d')
        self.bus = bus
        self.end_condition = end_condition
        self.world = world if world is not None else MultiLineString()
        self._handlers = collections.defaultdict(lambda: self.on_default)
        self.set_handler('request_origin', self.on_request_origin)
        self.set_handler('desired_speed', self.on_desired_speed)
        self.set_handler('pose2d', self.on_pose)
        self.set_handler('pose3d', self.on_pose)
        self.origin = [7.0, 3.0, 0.0]
        self.xyz = [0.0, 0.0, 0.0]
        self.orientation = quaternion.identity()
        self.speed_forward = 0.0
        self.speed_angular = 0.0
        self.time = datetime.timedelta()

    def start(self):
        self.thread = Thread(target=self.main)
        self.thread.start()

    def join(self, timeout=None):
        self.thread.join(timeout)

    def main(self):
        log("started")
        self.on_request_origin(datetime.timedelta(), "request_origin", True)
        # there is no SubT control until pose is received -> trigger it
        self.on_desired_speed(datetime.timedelta(), "desired_speed", [0, 0])

        for _ in range(500):
            packet = self.bus.listen()
            self.handle_incomming(*packet)
            if self.end_condition(self):
                return

    def set_handler(self, channel, handler):
        self._handlers[channel] = handler

    def handle_incomming(self, dt, channel, data):
        self._handlers[channel](dt, channel, data)

    def on_default(self, dt, channel, data):
        log(dt, channel, data)

    def on_request_origin(self, dt, channel, data):
        log(dt, channel, data)
        # ros_proxy_node.cc keeps sending origin data until robot arrives within 0.3m from (0,0,0)
        # we send it only once
        corrected = [rr - oo for rr, oo in zip(self.xyz, self.origin)]
        self.bus.publish('origin', [b'X100L', *corrected, *self.orientation])
        self.bus.publish('scan', self.scan())
        heading_centidegrees = round(math.degrees(quaternion.heading(self.orientation))*100)
        self.bus.publish('rot', [heading_centidegrees, 0, 0])
        self.bus.publish('orientation', self.orientation)

    def on_pose(self, dt, channel, data):
        if channel == 'pose2d':
            out = str(data)
        elif channel == 'pose3d':
            out0 = "[" + ', '.join([f"{p:.2f}" for p in data[0]]) + "]"
            out1 = " [" + ', '.join([f"{p:.2f}" for p in data[1]]) + "]"
            out = out0 + out1
        log(dt, channel, out)

    def on_desired_speed(self, dt, channel, data):
        forward = data[0]/1000.               # from millimeters per second to meters per second
        angular = math.radians(data[1]/100.)  # from centidegrees per second to radians per second
        dtime = 0.1                           # simulation step time in seconds
        max_acc_forward = 1                   # meters per sec**2
        max_acc_angular = 1                   # radians per sec**2
        acc_forward = np.clip(-max_acc_forward, (forward - self.speed_forward)/dtime, max_acc_forward)
        acc_angular = np.clip(-max_acc_angular, (angular - self.speed_angular)/dtime, max_acc_angular)
        self.speed_forward += acc_forward * dtime
        self.speed_angular += acc_angular * dtime
        log("desired", f"forward {forward:.2f} m/s", f"angular {math.degrees(angular):.2f} deg/s")
        log("current", f"forward {self.speed_forward:.2f} m/s", f"angular {math.degrees(self.speed_angular):.2f} deg/s")
        dist = dtime * self.speed_forward
        dist3d = quaternion.rotate_vector([dist, 0, 0], self.orientation)
        self.xyz = [a + b for a, b in zip(self.xyz, dist3d)]
        turn = quaternion.from_axis_angle((0,0,1), dtime * self.speed_angular)
        self.orientation = quaternion.multiply(self.orientation, turn)
        self.time += datetime.timedelta(seconds=dtime)
        heading_centidegrees = round(math.degrees(quaternion.heading(self.orientation))*100)
        x_mm, y_mm, z_mm = [round(c*1000) for c in self.xyz]
        self.bus.publish('orientation', self.orientation)
        self.bus.publish('rot', [heading_centidegrees, 0, 0])
        self.bus.publish('pose2d', [x_mm, y_mm, heading_centidegrees])
        self.bus.publish('pose3d', [[a - b for a, b, in zip(self.xyz, self.origin)], self.orientation])
        self.bus.publish('sim_time_sec', self.time.total_seconds())
        self.bus.publish('scan', self.scan())

        #log("++++", x, y, math.degrees(heading))

    def scan(self):
        from shapely.affinity import translate, rotate
        "270 degrees -> np.linspace(-135, 135, 271)"
        ret = np.zeros((271,))

        # create lidar lines
        angles = np.linspace(math.radians(-135), math.radians(135), 271)
        acos = np.cos(angles)
        asin = np.sin(angles)
        max_dist_m = 10.0
        x = acos * max_dist_m
        y = asin * max_dist_m

        # transform world to robot coordinates
        world = self.world
        world = translate(world, *[-c for c in self.xyz])
        world = rotate(world, -quaternion.heading(self.orientation), origin=(0,0), use_radians=True)

        # intersect each lidar line with the world
        for i, point in enumerate(zip(x, y)):
            cross = world.intersection(LineString([(0,0), point]))
            if cross.is_empty:
                continue
            if cross.geom_type == 'Point':
                ret[i] = math.hypot(*cross.coords)
            elif cross.geom_type == 'MultiPoint':
                # find closest intersection
                ret[i] = max_dist_m
                for item in cross:
                    dist = math.hypot(*cross.coords)
                    if ret[i] > dist:
                        ret[i] = dist
            else:
                assert False
        return ret

if __name__ == "__main__":
    from unittest.mock import MagicMock
    from shapely.geometry import MultiLineString
    bus = MagicMock()
    world = MultiLineString([[(1,-1), (1,1)], [(2,-1), (2,1)]])
    sim = Simulation(bus, lambda sim: False, world=world)
    sim.xyz = [-1.0, 0.0, 0.0]
    sim.orientation = quaternion.from_axis_angle((0, 0, 1), math.radians(90))
    scan = sim.scan()
    print(scan)
