from datetime import datetime
import traceback
import time
import PLCInterface
import math
from HardwarePositions import Point
import ctypes

# digital outputs
PIN_WATCHDOG = 8

def millis():
    dt = datetime.now()
    return int(dt.microsecond/10)

class RobotController(object):
    def __init__(self, arm, hardwarePositions):
        self.alive = True
        self._arm = arm
        self._hardware_positions = hardwarePositions
        self._tcp_speed = 50
        self._tcp_acc = 100
        self._angle_speed = 20
        self._angle_acc = 500
        self._vars = {}
        self._funcs = {}
        self._robot_init()

    # Robot init
    def _robot_init(self):
        self._arm.clean_warn()
        self._arm.clean_error()
        self._arm.motion_enable(True)
        self._arm.set_mode(0)
        self._arm.set_state(0)
        time.sleep(1)
        self._arm.register_error_warn_changed_callback(self._error_warn_changed_callback)
        self._arm.register_state_changed_callback(self._state_changed_callback)
        if hasattr(self._arm, 'register_count_changed_callback'):
            self._arm.register_count_changed_callback(self._count_changed_callback)

    # Register error/warn changed callback
    def _error_warn_changed_callback(self, data):
        if data and data['error_code'] != 0:
            self.alive = False
            self.pprint('err={}, quit'.format(data['error_code']))
            self._arm.release_error_warn_changed_callback(self._error_warn_changed_callback)

    # Register state changed callback
    def _state_changed_callback(self, data):
        if data and data['state'] == 4:
            self.alive = False
            self.pprint('state=4, quit')
            self._arm.release_state_changed_callback(self._state_changed_callback)

    # Register count changed callback
    def _count_changed_callback(self, data):
        if self.is_alive:
            self.pprint('counter val: {}'.format(data['count']))

    def _check_code(self, code, label):
        if not self.is_alive or code != 0:
            self.alive = False
            ret1 = self._arm.get_state()
            ret2 = self._arm.get_err_warn_code()
            self.pprint('{}, code={}, connected={}, state={}, error={}, ret1={}. ret2={}'.format(label, code, self._arm.connected, self._arm.state, self._arm.error_code, ret1, ret2))
        return self.is_alive

    lastWatchdogTimestamp = millis()
    watchdogState = 0
    
    def _handle_watchdog(self):
        
        #if (millis() - self.lastWatchdogTimestamp) > 500:
        self.watchdogState = not self.watchdogState
        self.digitalWrite(PIN_WATCHDOG, self.watchdogState)
            
            #self.lastWatchdogTimestamp = millis()

    @staticmethod
    def pprint(*args, **kwargs):
        try:
            stack_tuple = traceback.extract_stack(limit=2)[0]
            print('[{}][{}] {}'.format(time.strftime('%Y-%m-%d %H:%M:%S', time.localtime(time.time())), stack_tuple[1], ' '.join(map(str, args))))
        except:
            print(*args, **kwargs)

    @property
    def arm(self):
        return self._arm

    @property
    def VARS(self):
        return self._vars

    @property
    def FUNCS(self):
        return self._funcs

    @property
    def is_alive(self):
        if self.alive and self._arm.connected and self._arm.error_code == 0:
            if self._arm.state == 5:
                cnt = 0
                while self._arm.state == 5 and cnt < 5:
                    cnt += 1
                    time.sleep(0.1)
            return self._arm.state < 4
        else:
            return False
        
    def digitalRead(self, pin):
        return not self._arm.get_cgpio_digital(pin)[1]

    def digitalWrite(self, pin, state):
        self._arm.set_cgpio_digital(pin, state)

    def init(self, interface):
        self._interface = interface

    def run(self):
        try:
            MOVE_STATE_XY = 0
            MOVE_STATE_Z = 1
            moveState = MOVE_STATE_XY
            moveAscend = 100

            PICK_STATE_DESCENDING = 0
            PICK_STATE_SUCK = 1
            PICK_STATE_ASCENDING = 2
            pickState = PICK_STATE_DESCENDING
            pickUpDescend = 15
            putDownDescend = 10

            NIXIE_TYPE_R = 0
            NIXIE_TYPE_F = 1
            nixieType = NIXIE_TYPE_R

            calibrationMode = 0

            # init hand by alinging end effector and moving it up a bit
            current_angle = self._arm.angles
            angle = -(current_angle[1] + current_angle[2])
            self._arm.set_servo_angle(servo_id=4, angle=angle)
            self._arm.set_position(z=10, radius=-1, speed=self._tcp_speed, mvacc=self._tcp_acc, relative=True, wait=True)

            # main run loop
            while self.is_alive:
                t1 = time.monotonic()

                # periodic tasks
                self._handle_watchdog()
                self._interface.handle()

                # command execution start
                if self._interface.commandReceived():
                    self._interface.ackCommandReceived()
                    command = self._interface.getCommand()
                    print("got command" + str(command))

                    if command[0] == PLCInterface.COMMAND_MOVE:
                        print("move")
                        (point, tilt) = self._hardware_positions.getPosition(command[1], command[2])
                        print(point.str())
                        if point == Point(0,0,0):
                            print("error accessing hardware array: " + str(command[1]) + ", " + str(command[2]))
                        else:
                            moveState = MOVE_STATE_XY
                            self._arm.set_position(x=point.x, y=point.y, z=point.z+moveAscend, yaw=math.degrees(tilt), radius=-1, speed=self._tcp_speed, mvacc=self._tcp_acc, relative=False, wait=False)

                    elif command[0] == PLCInterface.COMMAND_ROTATE:
                        angle = ctypes.c_int8(command[1]).value
                        print("rotate by " + str(angle))
                        self._arm.set_position(yaw=angle, radius=-1, speed=self._tcp_speed, mvacc=self._tcp_acc, relative=True, wait=False)

                    elif command[0] == PLCInterface.COMMAND_PICK_UP:
                        print("picking up nixie")
                        pickState = PICK_STATE_DESCENDING
                        self._arm.set_vacuum_gripper(1, wait=False, timeout=3, delay_sec=None)
                        self._arm.set_position(z=-pickUpDescend, radius=-1, speed=self._tcp_speed, mvacc=self._tcp_acc, relative=True, wait=False)

                    elif command[0] == PLCInterface.COMMAND_PUT_DOWN:
                        print("putting down nixie")
                        pickState = PICK_STATE_DESCENDING
                        self._arm.set_position(z=-putDownDescend, radius=-1, speed=self._tcp_speed, mvacc=self._tcp_acc, relative=True, wait=False)

                    elif command[0] == PLCInterface.COMMAND_CHANGE_NIXIE_TYPE:
                        print("changing nixie type")
                        nixieType = command[1]
                        self._interface.commandExecuted()

                    elif command[0] == PLCInterface.COMMAND_START_CALIBRATION:
                        print("starting calibration")
                        calibrationMode = 1
                        self._interface.commandExecuted()

                    elif command[0] == PLCInterface.COMMAND_STOP_CALIBRATION:
                        print("stopping calibration")
                        calibrationMode = 0
                        self._interface.commandExecuted()

                    elif command[0] == PLCInterface.COMMAND_CHANGE_CALIBRATION:
                        print("changing calibration value")
                        

                # command executed check
                if self._interface.executingCommand():
                    command = self._interface.getCommand()

                    if command[0] == PLCInterface.COMMAND_MOVE:
                        if moveState == MOVE_STATE_XY:
                            if not self._arm.get_is_moving():
                                self._arm.set_position(z=-moveAscend, radius=-1, speed=self._tcp_speed, mvacc=self._tcp_acc, relative=True, wait=False)
                                moveState = MOVE_STATE_Z
                        elif moveState == MOVE_STATE_Z:
                            if not self._arm.get_is_moving():
                                self._interface.commandExecuted()

                    # simple move commands are done when the arm stops moving
                    elif command[0] == PLCInterface.COMMAND_ROTATE or command[0] == PLCInterface.COMMAND_CHANGE_CALIBRATION:
                        if not self._arm.get_is_moving():
                            self._interface.commandExecuted()

                    # pick up command goes down, sucks the nixie and raises up again
                    elif command[0] == PLCInterface.COMMAND_PICK_UP:
                        # pick up state machine
                        if pickState == PICK_STATE_DESCENDING:
                            if not self._arm.get_is_moving():

                                pickState = PICK_STATE_SUCK
                        elif pickState == PICK_STATE_SUCK:
                            self._arm.set_position(z=pickUpDescend+moveAscend, radius=-1, speed=self._tcp_speed, mvacc=self._tcp_acc, relative=True, wait=False)
                            pickState = PICK_STATE_ASCENDING
                        elif pickState == PICK_STATE_ASCENDING:
                            if not self._arm.get_is_moving():
                                self._interface.commandExecuted()

                    # put down command goes down, releases the nixie and raises up again
                    elif command[0] == PLCInterface.COMMAND_PUT_DOWN:
                        # put down state machine
                        if pickState == PICK_STATE_DESCENDING:
                            if not self._arm.get_is_moving():
                                pickState = PICK_STATE_SUCK
                        elif pickState == PICK_STATE_SUCK:
                            self._arm.set_vacuum_gripper(0, wait=False, timeout=3, delay_sec=None)
                            self._arm.set_position(z=putDownDescend, radius=-1, speed=self._tcp_speed, mvacc=self._tcp_acc, relative=True, wait=False)
                            pickState = PICK_STATE_ASCENDING
                        elif pickState == PICK_STATE_ASCENDING:
                            if not self._arm.get_is_moving():
                                self._interface.commandExecuted()

                interval = time.monotonic() - t1
                if interval < 0.01:
                    time.sleep(0.01 - interval)
            self._tcp_acc = 100
        except Exception as e:
            self.pprint('MainException: {}'.format(e))
        self.alive = False
        self._arm.release_error_warn_changed_callback(self._error_warn_changed_callback)
        self._arm.release_state_changed_callback(self._state_changed_callback)
        if hasattr(self._arm, 'release_count_changed_callback'):
            self._arm.release_count_changed_callback(self._count_changed_callback)