#! /usr/bin/env python
import os
import json
from sys import argv

import rospy
import tf
import actionlib
import numpy as np
from naoqi import ALProxy

from masterloop import inform_masterloop_factory
from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal


_inform_masterloop = inform_masterloop_factory('calibrator')


def calibration():
    tts.say('Stand in front of camera')
    tts.say('Far enough to see your arms')
    tts.say('Then say start')
    client.send_goal(RequestSpeechGoal(['start']))
    client.wait_for_result()

    try:
        trans, q = ll.lookupTransform('odom',
                                      'Aruco_0_frame',
                                      rospy.Time(0))
        rot = tf.transformations.euler_from_quaternion(q)
        center = trans, rot

    except Exception:
        pass

    tts.say('Now stretch your arms')
    tts.say('Make sure the markers are detected')
    tts.say('Then say start')
    client.send_goal(RequestSpeechGoal(['start']))
    client.wait_for_result()

    try:
        larm, _ = ll.lookupTransform('Aruco_0_frame',
                                        'Aruco_1_frame',
                                        rospy.Time(0))
        rarm, _ = ll.lookupTransform('Aruco_0_frame',
                                     'Aruco_2_frame',
                                     rospy.Time(0))
    except Exception:
        pass

    tts.say('Now rotate to your right')
    tts.say('So that marker is still detected')
    tts.say('Then say start')
    client.send_goal(RequestSpeechGoal(['start']))
    client.wait_for_result()

    try:
        _, q = ll.lookupTransform('odom',
                                  'Aruco_0_frame',
                                  rospy.Time(0))
        RR = tf.transformations.euler_from_quaternion(q)
    except Exception:
        pass

    tts.say('Now rotate to your left')
    tts.say('So that marker is still detected')
    tts.say('Then say start')
    client.send_goal(RequestSpeechGoal(['start']))
    client.wait_for_result()

    try:
        _, q = ll.lookupTransform('odom',
                                  'Aruco_0_frame',
                                  rospy.Time(0))
        LR = tf.transformations.euler_from_quaternion(q)
    except Exception:
        pass


    tts.say('Now rotate to initial position')
    tts.say('Take a big step forward')
    tts.say('Then say start')
    client.send_goal(RequestSpeechGoal(['start']))
    client.wait_for_result()

    try:
        FW, _ = ll.lookupTransform('odom',
                                   'Aruco_0_frame',
                                   rospy.Time(0))
    except Exception:
        pass

    tts.say('Now return to the initial position')
    tts.say('Then take a big step backward')
    tts.say('Then say start')
    client.send_goal(RequestSpeechGoal(['start']))
    client.wait_for_result()

    try:
        BK, _ = ll.lookupTransform('odom',
                                   'Aruco_0_frame',
                                   rospy.Time(0))
    except Exception:
        pass

    tts.say('Now return to the initial position')
    tts.say('Then take a big step to the right')
    tts.say('Then say start')
    client.send_goal(RequestSpeechGoal(['start']))
    client.wait_for_result()

    try:
        RT, _ = ll.lookupTransform('odom',
                                   'Aruco_0_frame',
                                   rospy.Time(0))
    except Exception:
        pass

    tts.say('Now return to the initial position')
    tts.say('Then take a big step to the left')
    tts.say('Then say start')
    client.send_goal(RequestSpeechGoal(['start']))
    client.wait_for_result()

    try:
        LT, _ = ll.lookupTransform('odom',
                                   'Aruco_0_frame',
                                   rospy.Time(0))
    except Exception:
        pass

    calib = {}

    try:
        tts.say('Well done')
        calib['cr'] = [center[0][0], center[0][1], center[1][2]]
        calib['arm'] = (np.linalg.norm(np.array(larm)) +
                        np.linalg.norm(np.array(rarm))) / 2
        calib['rr'] = RR[2]
        calib['lr'] = LR[2]
        calib['fw'] = FW[0]
        calib['bk'] = BK[0]
        calib['lt'] = LT[1]
        calib['rt'] = RT[1]
        here = os.path.dirname(os.path.realpath(__file__))
        with open('{}/../config/default.yaml'.format(here), 'w') as f:
            json.dump(calib, f)

    except:
        tts.say('Something has gone very wrong')


if __name__ == '__main__':
    if len(argv) < 2:
        raise SystemExit

    rospy.init_node('calibrator', disable_signals=True)

    try:
        tts = ALProxy('ALTextToSpeech', os.environ['NAO_IP'], 9559)
        client = actionlib.SimpleActionClient('speech_server',
                                              RequestSpeechAction)
        client.wait_for_server()

        ll = tf.TransformListener()

        rospy.on_shutdown(lambda: client.cancel_goal())

        calibration()
    except KeyboardInterrupt:
        rospy.signal_shutdown('Interrupted')