import socket

import GCode_Interpreterdc
from controller import DummyController


class Hardware:
    def __init__(self, bind):
        self.s = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
        self.s.connect(bind)

    def probe(self, gtm):
        return

    def realize(self, gtm):
        return


class SimuHardware(Hardware):
    def __init__(self, bind):
        Hardware.__init__(self, bind)

    def probe(self, gtm):
        self.s.send("request\n".encode())
        encoder_x = int(socket.SocketIO(self.s, "rw").readline())
        encoder_y = int(socket.SocketIO(self.s, "rw").readline())
        encoder_z = int(socket.SocketIO(self.s, "rw").readline())
        print("HAS PROBED")
        last_x, last_y, last_z = gtm.current_steps
        # We are now at some position which realizes the encoder positions
        # curr_x = encoder_x + k*X_MOTOR_STEPS
        # |curr_x - last_x| <= 1/2 * X_MOTOR_STEPS
        curr_x = encoder_x + (last_x // gtm.X_MOTOR_STEPS) * gtm.X_MOTOR_STEPS
        if curr_x - last_x > .5 * gtm.X_MOTOR_STEPS:
            curr_x -= gtm.X_MOTOR_STEPS

        curr_y = encoder_y + (last_y // gtm.Y_MOTOR_STEPS) * gtm.Y_MOTOR_STEPS
        if curr_y - last_y > .5 * gtm.Y_MOTOR_STEPS:
            curr_y -= gtm.Y_MOTOR_STEPS

        curr_z = encoder_z + (last_z // gtm.Z_MOTOR_STEPS) * gtm.Z_MOTOR_STEPS
        if curr_z - last_z > .5 * gtm.Z_MOTOR_STEPS:
            curr_z -= gtm.Z_MOTOR_STEPS

        # Note that if we don't probe regularly enough, we lose track of the position
        # Really, we should be able to set_steps
        gtm.set_position(curr_x / gtm.X_STEPS_PER_MM,
                         curr_y / gtm.Y_STEPS_PER_MM,
                         curr_z / gtm.Z_STEPS_PER_MM)

    def realize(self, gtm):
        self.s.send("realize\n".encode())

        if gtm.x_throttle == 0.:
            self.s.send(str(gtm.x_direction).encode() + b" 0\n")
        else:
            self.s.send(str(gtm.x_direction).encode() + b" " + str(gtm.FAST_XY_FEEDRATE / gtm.x_throttle).encode() + b"\n")

        if gtm.y_throttle == 0.:
            self.s.send(str(gtm.y_direction).encode() + b" 0\n")
        else:
            self.s.send(str(gtm.y_direction).encode() + b" " + str(gtm.FAST_XY_FEEDRATE / gtm.y_throttle).encode() + b"\n")

        if gtm.z_throttle == 0.:
            self.s.send(str(gtm.z_direction).encode() + b" 0\n")
        else:
            self.s.send(str(gtm.z_direction).encode() + b" " + str(gtm.FAST_Z_FEEDRATE / gtm.z_throttle).encode() + b"\n")


if __name__ == '__main__':
    b = "socket.sock"
    sh = SimuHardware(b)
    ctrl = DummyController()
    gtm1 = GCode_Interpreterdc.GCodeToMotors(ctrl, sh)
    sh.probe(gtm1)
    print(gtm1.current_units)