import socket
import os
import typing
import time
import numpy as np

from sympy.abc import alpha


def parse_speed(text):
    tl = text.split(' ')
    return int(tl[0]) * float(tl[1])


class Simulator:
    def __init__(self, bind):
        self.s = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
        try:
            os.remove(bind)
        except FileNotFoundError:
            pass
        self.s.bind(bind)
        self.s.listen()


class NaiveSimulator(Simulator):
    steps = [0, 0, 0]
    pos = np.array([0., 0., 0.])
    speed = np.array([0., 0., 0.])

    time_step = 0
    last_update = 0.

    # X_STEPS_PER_INCH = 4800
    X_STEPS_PER_MM: float = 188.97
    X_MOTOR_STEPS: float = 200

    # Y_STEPS_PER_INCH = 4800
    Y_STEPS_PER_MM: int = 188.97
    Y_MOTOR_STEPS: int = 200

    # Z_STEPS_PER_INCH = 4800
    Z_STEPS_PER_MM: float = 188.97
    Z_MOTOR_STEPS: int = 200

    def __init__(self, bind):
        super().__init__(bind)
        self.last_update = time.time()
        while True:
            self.loop()

    def loop(self):
        req = socket.SocketIO(self.s, 'r').readline()
        match req:
            case 'request':
                print("At your service")
                self.request()
            case 'realize':
                print("Contemplating Life")
                self.realize()
        print("Job Done")

    def request(self):
        self.s.send(f"{self.steps[0]}".encode())
        self.s.send(f"{self.steps[1]}".encode())
        self.s.send(f"{self.steps[2]}".encode())

    def realize(self):
        print("MA VIE C'EST DE LA MERDE")
        lt = time.time()
        self.time_step = lt - self.last_update
        self.last_update = lt
        x_v = parse_speed(socket.SocketIO(self.s, 'r').readline())
        y_v = parse_speed(socket.SocketIO(self.s, 'r').readline())
        z_v = parse_speed(socket.SocketIO(self.s, 'r').readline())

        inertia = - alpha * self.speed
        gamma_command = np.array([x_v, y_v, z_v]) - self.speed
        self.speed = self.time_step * (inertia + gamma_command)
        step_increment = np.ceil(self.speed * self.time_step * np.array([1 / self.X_MOTOR_STEPS, 1 / self.Y_MOTOR_STEPS, 1 / self.Z_MOTOR_STEPS]))
        self.steps = self.steps + step_increment
        self.pos = self.pos + np.array([self.X_STEPS_PER_MM * step_increment[0], self.Y_STEPS_PER_MM * step_increment[1], self.Z_STEPS_PER_MM * step_increment[2]])
        self.s.send("Realized".encode())