# -*- coding: utf-8 -*-
#
# This file is part of the bliss project
#
# Copyright (c) Beamline Control Unit, ESRF
# Distributed under the GNU LGPLv3. See LICENSE for more info.

"""
Module implementing V1 of the Symetrie hexapod protocol.
Used by shexapod.py
"""


import struct
import socket
from collections import namedtuple
from tabulate import tabulate

from bliss.controllers.motors.shexapod import (
    Pose,
    BaseHexapodError,
    BaseHexapodProtocol,
)


class HexapodV1Error(BaseHexapodError):
    pass


class TurboPmacCommand(object):
    VR_DOWNLOAD = 0x40
    VR_UPLOAD = 0xC0

    VR_PMAC_SENDLINE = 0xB0
    VR_PMAC_GETLINE = 0xB1
    VR_PMAC_FLUSH = 0xB3
    VR_PMAC_GETMEM = 0xB4
    VR_PMAC_SETMEM = 0xB5
    VR_PMAC_SETBIT = 0xBA
    VR_PMAC_SETBITS = 0xBB
    VR_PMAC_PORT = 0xBE
    VR_PMAC_GETRESPONSE = 0xBF
    VR_PMAC_READREADY = 0xC2
    VR_CTRL_RESPONSE = 0xC4
    VR_PMAC_GETBUFFER = 0xC5
    VR_PMAC_WRITEBUFFER = 0xC6
    VR_PMAC_WRITEERROR = 0xC7
    VR_FWDOWNLOAD = 0xCB
    VR_IPADDRESS = 0xE0

    def __init__(self, comm):
        self.__s = comm
        self.__data = struct.Struct("BBHHH")
        self.__read_version()

    def __call__(self, command, convtype=None):
        return self.__send(
            self.VR_DOWNLOAD, self.VR_PMAC_GETRESPONSE, command, convtype
        )

    def request(self, command, convtype=None):
        return self.__send(
            self.VR_DOWNLOAD, self.VR_PMAC_GETRESPONSE, command, convtype
        )

    def __read_version(self):
        pmactype = self.request("TYPE")
        pmacvers = self.request("VERSION")
        self.__version = "{0} - version {1}".format(pmactype, pmacvers)

    def version(self):
        return self.__version

    def __send(self, requestType, request, command, convtype):
        data = self.__data.pack(requestType, request, 0, 0, socket.htons(len(command)))
        raw = self.__s.write_readline(data + command.encode(), eol="\x06")
        if len(raw):
            raw = raw.decode()
            ans = list(map(str.strip, raw.split("\r")[:-1]))
            if convtype is not None:
                ans = list(map(convtype, ans))
            if len(ans) > 1:
                return ans
            else:
                return ans[0]
        return None


class HexapodProtocolV1(BaseHexapodProtocol):

    DEFAULT_PORT = 1025

    MACHINE_CS = 32
    USER_CS = 33

    SYSTEM_STATUS_FIELDS = (
        "error",
        "ready",
        "in_position",
        "control",
        "homing_done",
        "brake_control",
        "emergency_stop_pressed",
        "following_warning",
        "following_error",
        "actuators_out",
        "amplifier_error",
        "encoder_error",
        "phasing_error",
        "homing_error",
        "kinematic_error",
        "abort_input_error",
        "memory_error",
        "homing_virtual",
        "moving",
    )

    SystemStatus = namedtuple("SystemStatus", SYSTEM_STATUS_FIELDS)

    AXIS_STATUS_FIELDS = (
        "in_position",
        "control",
        "homing_done",
        "homing_hardware_input",
        "positive_hardware_lmit",
        "negative_hardware_limit",
        "brake_control",
        "following_warning",
        "following_error",
        "actuators_out",
        "amplifier_error",
        "encoder_error",
        "phasing_error",
    )

    AxisStatus = namedtuple("AxisStatus", AXIS_STATUS_FIELDS)

    ERROR_POSVALID = {
        1: "Homing not done",
        2: "Coordinate systems definition not realized",
        4: "Kinematic error",
        8: "Out of SYMETRIE (factory) workspace",
        16: "Out of machine workspace (see §4.4.12)",
        32: "Out of user workspace",
        64: "Actuator out of limits",
        128: "Joints out of limits",
        256: "Out of limits due to backlash compensation",
        512: "Abort” input enabled",
        1024: "Safety sensor” inputs enabled",
        63488: "Reserved",
    }

    def __init__(self, config):
        BaseHexapodProtocol.__init__(self, config)
        self.pmac = None
        self.__set_pose = None

    def connect(self):
        self.pmac = TurboPmacCommand(self.comm)
        self.__read_api_version()

    def __read_api_version(self):
        self.pmac("&2 Q20=55")
        self.wait_command()
        ans = self.pmac("&2 Q80,4,1")
        self.__api_version = "{0} [{1}]".format(*ans)
        self.__serial_number = "{2} [{3}]".format(*ans)

    @property
    def api_version(self):
        return self.__api_version

    @property
    def serial_number(self):
        return self.__serial_number

    def wait_command(self):
        ack = 1
        while ack >= 1:
            ack = self.pmac("&2 Q20", int)
        return ack

    @property
    def control(self):
        return self.system_status.control

    @control.setter
    def control(self, control):
        status = self.system_status
        curr_control = status.control
        if control and not curr_control:
            # control ON
            self.pmac("&2 Q20=3")
            err = self.wait_command()
            if err == -1:
                raise HexapodV1Error(
                    "Command ignored (emergency button engaged or ctrl in error state)"
                )
            elif err == -2:
                raise HexapodV1Error("Control of the servo motors has failed")
        elif not control and curr_control:
            # control OFF
            self.pmac("&2 Q20=4")
            self.wait_command()

    def _stop(self):
        self.pmac("&2 Q20=2")

    def _homing(self, async_=False):
        self.pmac("&2 Q20=1")
        if async_ is False:
            self.wait_command()

    def _reset(self):
        self.pmac("$$$")

    def _move(self, pose, async_=False):
        set_pose_dict = self.set_pose._asdict()
        # any coordinate which is None will be replaced by the latest set_pose
        pose_keys = list(set_pose_dict.keys())
        pose = Pose(
            *[
                set_pose_dict[pose_keys[i]] if v is None else v
                for i, v in enumerate(pose)
            ]
        )
        set_pos_cmd = "Q70=0 Q71=%f Q72=%f Q73=%f Q74=%f Q75=%f Q76=%f Q20=11" % (
            pose.tx,
            pose.ty,
            pose.tz,
            pose.rx,
            pose.ry,
            pose.rz,
        )
        self.pmac(set_pos_cmd)
        err = self.wait_command()
        if err == -1:
            raise HexapodV1Error("Command ignored. Conditions of move not met.")
        elif err == -2:
            cmd = "Q29"
            val = int(self.pmac(cmd))
            msg = f"Invalid movement command: POSVAL [{val}]"
            for i in self.ERROR_POSVALID:
                if i & val:
                    msg += f", ({i}) " + self.ERROR_POSVALID[i]
            raise HexapodV1Error(msg)

    def is_moving(self):
        pass

    def __to_status(self, status, klass):
        istatus = int(status)
        return klass(*[bool(istatus & (1 << i)) for i in range(len(klass._fields))])

    @property
    def system_status(self):
        status = self.pmac("&2 Q36", int)
        conv_status = self.__to_status(status, self.SystemStatus)
        moving = (
            conv_status.control
            and not conv_status.error
            and not conv_status.in_position
        )
        return conv_status._replace(moving=moving)

    @property
    def axis_status(self):
        status = self.pmac("&2 Q30,6,1", int)
        conv_status = [self.__to_status(x, self.AxisStatus) for x in status]
        return conv_status

    @property
    def set_pose(self):
        if self.__set_pose is None:
            self.__set_pose = self.object_pose
        return self.__set_pose

    @property
    def object_pose(self):
        pos_user = self.pmac("&2 Q53,6,1", float)
        return Pose(*pos_user)

    @property
    def platform_pose(self):
        pos_mach = self.pmac("&2 Q47,6,1", float)
        return Pose(*pos_mach)

    def _user_and_object_cs(self):
        self.pmac("&2 Q20=31")
        self.wait_command()
        all_cs = self.pmac("&2 Q80,12,1", float)
        user_cs = Pose(*all_cs[0:6])
        object_cs = Pose(*all_cs[6:12])
        return dict(user_cs=user_cs, object_cs=object_cs)

    @property
    def user_cs(self):
        return self._user_and_object_cs()["user_cs"]

    @property
    def object_cs(self):
        return self._user_and_object_cs()["object_cs"]

    def _cs_limits(self, cs):
        self.pmac("&2 Q20=%d" % cs)
        self.wait_command()
        limits = self.pmac("&2 Q80,13,1", float)
        low_limits = [limits[x] for x in range(0, 12, 2)]
        high_limits = [limits[x] for x in range(1, 12, 2)]
        low_pose = Pose(*low_limits)
        high_pose = Pose(*high_limits)
        return dict(low_limits=low_pose, high_limits=high_pose)

    @property
    def user_limits(self):
        return self._cs_limits(self.USER_CS)

    @property
    def machine_limits(self):
        return self._cs_limits(self.MACHINE_CS)

    def _cs_limits_enabled(self, cs):
        self.pmac("&2 Q20=%d" % cs)
        self.wait_command()
        enabled = self.pmac("&2 Q80", bool)
        if cs == self.MACHINE_CS:
            return enabled == 1 or enabled == 3
        else:
            return enabled == 2 or enabled == 3

    @property
    def user_limits_enabled(self):
        return self._cs_limits_enabled(self.USER_CS)

    @property
    def machine_limits_enabled(self):
        return self._cs_limits_enabled(self.MACHINE_CS)

    def speeds(self):
        self.pmac("&2 Q20 35")
        self.wait_command()
        speeds = self.pmac("&2 Q86,6,1", float)
        return dict(
            tspeed=speeds[0],
            rspeed=speeds[1],
            min_tspeed=speeds[2],
            max_tspeed=speeds[4],
            min_rspeed=speeds[3],
            max_rspeed=speeds[5],
        )

    @property
    def tspeed(self):
        return self.speeds()["tspeed"]

    @property
    def rspeed(self):
        return self.speeds()["rspeed"]

    def virtual_home(self, pose):
        set_pos_cmd = "&2 Q71=%f Q72=%f Q73=%f Q74=%f Q75=%f Q76=%f Q20=42" % (
            pose.tx,
            pose.ty,
            pose.tz,
            pose.rx,
            pose.ry,
            pose.rz,
        )
        self.pmac(set_pos_cmd)
        err = self.wait_command()
        if err != 0:
            raise HexapodV1Error("Virtual homing failed")

    DUMP_STATUS_TEMPLATE = """\
Symetrie Hexapode
API version   : {api_version}
Serial Number : {serial_number}

{system_status}

{axis_status}
"""

    def dump_status(self):
        infos = {}
        infos["api_version"] = self.api_version
        infos["serial_number"] = self.serial_number

        system_status = self.system_status
        rows = [
            (field, str(getattr(system_status, field)))
            for field in system_status._fields
        ]
        heads = ["System Status", "Value"]
        infos["system_status"] = tabulate(rows, headers=heads)

        axis_status = self.axis_status
        rows = [
            [field] + [str(getattr(one_axis, field)) for one_axis in axis_status[0:6]]
            for field in axis_status[0]._fields
        ]
        heads = Pose._fields
        infos["axis_status"] = tabulate(rows, headers=heads)

        return self.DUMP_STATUS_TEMPLATE.format(**infos)

    DUMP_POSITION_TEMPLATE = """\
{positions}

Translation Speed : {speeds[tspeed]} mm/s (min= {speeds[min_tspeed]}; max= {speeds[max_rspeed]})
Rotation Speed    : {speeds[rspeed]} deg/s (min= {speeds[min_rspeed]}; max= {speeds[max_rspeed]})
"""

    def dump_position(self):
        user_cs = self.user_cs
        object_cs = self.object_cs
        object_pose = self.object_pose
        platform_pose = self.platform_pose
        user_limits = self.user_limits
        machine_limits = self.machine_limits

        heads = (
            ["Hexapode"]
            + ["{0} (mm)".format(i) for i in self.Pose._fields[:3]]
            + ["{0} (deg)".format(i) for i in self.Pose._fields[3:]]
        )

        rows = [
            ["User coordinate system"] + list(user_cs),
            ["Object coordinate system"] + list(object_cs),
            ["Object pose"] + list(object_pose),
            ["Platform pose"] + list(platform_pose),
            ["High user limits"] + list(user_limits["high_limits"]),
            ["Low user limits"] + list(user_limits["low_limits"]),
            ["High machine limits"] + list(machine_limits["high_limits"]),
            ["Low machine limits"] + list(machine_limits["low_limits"]),
        ]

        positions = tabulate(rows, headers=heads)

        speeds = self.speeds()

        return self.DUMP_POSITION_TEMPLATE.format(positions=positions, speeds=speeds)

    def dump(self):
        return self.dump_status() + "\n" + self.dump_position()
