"""
FB Logic Class
Version 0.1

This file is generated by FB Devkit. Should add FB logic here.
"""
import logging
from spidr_communicator import ParamInput, ParamOutput
from robot import RobotCommSample
from status_code import StatusCode


class TemplateRoboticMoveLinear:

    def __init__(self, name: str, param_input: ParamInput, param_output: ParamOutput):
        """
        This function will be called by main.py, is the initial method of this FB
        If you have a input parameter name "newvalue", you can get it by self.param_input.newvalue
        If you want to set a output parameter name "status", you can set it by self.param_output.status = "Good"
        """
        self.name = name
        self.param_input = param_input
        self.param_output = param_output
        
        # Here is the second layer of parameter analysis, which can be customized
        self.robot = RobotCommSample()

    def run(self):
        """
        This function will be called by main.py, is the main method of this FB
        """
        logging.info("Run function start")
        logging.info("Parse input parameters")
        # For resource, normally it contains robot connection parameters such as IP, PORT etc.
        logging.info("Input resource: {}".format(self.param_input.resource))
        # For waypoints, it contains all move position, tool and reference.
        logging.info("Input waypoints: {}".format(self.param_input.waypoints))
        # For speed, it means robot move speed for this MoveLinear command
        logging.info("Input speed: {}".format(self.param_input.speed))
        # For blending, it means blending between each waypoint
        logging.info("Input blending: {}".format(self.param_input.blending))
        # check input parameters
        if (not self.param_input.resource or
                not self.param_input.waypoints or
                not self.param_input.speed or
                not self.param_input.blending):
            logging.error("Failed to parse input parameters.")
            self.param_output.status = StatusCode.BAD_INVALID_ARGUMENT.msg
            return

        # Here we can connect to the robot
        if not self.robot.connect(ip=self.param_input.resource.get("IPAddress"),
                                  port=self.param_input.resource.get("Port")):
            logging.error("Failed to connect to robot")
            self.param_output.status = StatusCode.BAD_COMMUNICATION_ERROR.msg
            return
        # Here we can move the robot
        if not self.robot.move_linear(pos=self.param_input.waypoints, speed=self.param_input.speed):
            logging.error("Failed to move robot")
            self.param_output.status = StatusCode.BAD_ROBOT_MOVE_ERROR.msg
            return
        # Here we can disconnect the robot
        self.robot.disconnect()
        # Here we can set output
        self.param_output.status = "Good"

    def is_state_turn_to_fail(self):
        """
        This method will be called by main.py
        It's a hook in running block before send machine state success, has the opportunity to set machine state as fail
        """
        return False

    def is_state_turn_to_success(self, exception):
        """
        This method will be called by main.py
        It's a hook in exception handling block before send machine state fail, has the opportunity to set machine state
        as success
        """
        return True
