"""
RB Logic Class
Version 0.1

This file is generated by Devkit. Should add resource block logic here.
"""
import logging
import json
from copy import copy
from enum import IntEnum
from .impl.gripper_comm import GripperComm


class CommandId(IntEnum):
    CONNECT = 0,
    DISCONNECT = 1,
    CALIBRATION = 2
    GRASP = 3,
    RELEASE = 4,
    GET_WIDTH = 5


class GripperTemplate:
    """
    This class is the main RB logic class, and will instantiation by RA system for one resource GripperTemplate.
    """
    def __init__(self, device_info: dict):
        """
        This function will be called by RA system, is the initial method of this RB
        All parameters for this device, such as IP, port and type etc. are in device_info,
        So device_info parameter should be parsed and then create the long connection in ths class.
        """
        self.device_info = device_info

        # Here is the second layer of parameter analysis, which can be customized
        self.gripper = GripperComm(self.device_info)

    def run_command(self, request):
        """
        This function will be called by RA system while RA got one command request for this device.
        In this function, following steps should be done:
        1. interpret request
        2. execute command
        3. return response
        """
        response = {}
        logging.debug("Run command start")

        # Interpret request
        command = request.get("cmd_id")

        # Execute command
        if command == CommandId.CONNECT:
            response = self.do_connect(request)
        elif command == CommandId.DISCONNECT:
            response = self.do_disconnect(request)
        elif command == CommandId.CALIBRATION:
            response = self.do_calibration(request)
        elif command == CommandId.GRASP:
            response = self.do_grasp(request)
        elif command == CommandId.RELEASE:
            response = self.do_release(request)
        elif command == CommandId.GET_WIDTH:
            response = self.do_get_width(request)
        else:
            response = self.compose_response(request, 1, "Unknown command id: {}".format(command))

        # Return response
        return json.dumps(response)

    def do_connect(self, request):
        if self.gripper.connect():
            return self.compose_response(request)
        else:
            logging.error("Failed to connect to gripper")
            return self.compose_response(request, -1, "connect failed")

    def do_disconnect(self, request):
        self.gripper.disconnect()
        return self.compose_response(request)

    def do_calibration(self, request):
        self.gripper.calibration()
        return self.compose_response(request)

    def do_grasp(self, request):
        if not self.gripper.is_connected():
            self.do_connect(request)
        params = request.get("cmd_param")
        rst = self.gripper.grasp(position=params.get("position"),
                                    speed=params.get("speed"),
                                    force=params.get("force"))
        if rst:
            return self.compose_response(request)
        else:
            logging.error("Failed to execute gripper grasp command")
            return self.compose_response(request, -2, "grasp failed")

    def do_release(self, request):
        if not self.gripper.is_connected():
            self.do_connect(request)
        params = request.get("cmd_param")
        rst = self.gripper.release(position=params.get("position"),
                                 speed=params.get("speed"),
                                 force=params.get("force"))
        if rst:
            return self.compose_response(request)
        else:
            logging.error("Failed to execute gripper release command")
            return self.compose_response(request, -3, "release failed")

    def do_get_width(self, request):
        if not self.gripper.is_connected():
            self.do_connect(request)
        width = self.gripper.get_width()
        response = self.compose_response(request)
        response["data"]["width"] = width
        return response

    def compose_response(self, request, code: int = 0, message: str = ""):
        """
        Simply compose the response message, will return the same command id, request id, AGV name as the request
        """
        response = copy(request)
        response["error_code"] = {"value": code, "description": message}
        response["data"] = {}
        return response
