# sched.add_robot('codebox2', utils.left_X_close2wall)
import utils
sched = utils.Scheduler()
# sched.add_robot('codebox1', utils.bottom_I_close2wall)

def delete_state():
    sched.stop_action(self.action)
    for i in range(1000):
        sched.stop_action(i)




import wws_lite
from IPython.display import Image
import pprint
pp = pprint.PrettyPrinter(indent=4)
import math
from time import sleep
import matplotlib.pyplot as plt
import urllib.request
import numpy as np
import orspy


class State:
    def __init__(self, sched):
        self.object_seen = False
        self.sched = sched
        self.object = None

        # self.sched.add_robot('codebox1', utils.bottom_I_close2wall)
        self.pose_detect = False
        self.img_detect = False
        self.action = 0

    def start(self):
        self.action = self.sched.goto(orspy.EulerPose(68.73, 105.66, 0, 0, 0, 0),
                                      allowed_trans_error=0.3, allowed_rot_error=0.5)
        self.topic_img = "{}/object_img".format(self.sched.wws_topic_prefix())
        self.topic_pose = "{}/object_pose".format(self.sched.wws_topic_prefix())
        wws_lite.subscribe(self.topic_img, lambda x: self.wws_detect_img_cb(x))
        wws_lite.subscribe(self.topic_pose, lambda x: self.detect_cb_pose(x))


    def clear_action(self):
        self.sched.stop_action(self.action)

    def end(self):
        wws_lite.unsubscribe(self.topic_img)
        wws_lite.unsubscribe(self.topic_pose)

    def detect_cb_pose(self, m):
        sleep(0.3)
        if not self.object_seen:
            # print('object seen, closing in...')
            self.sched.stop_action(self.action)
            self.action = self.sched.fwd(.5, maxdist=0.5)
            self.object_seen = True
            self.pose = m
            self.class_name = m['object']['payload']['objects']['detections'][0]['class_name']
            # print(f"object type- {m['object']['payload']['objects']['detections'][0]['class_name']}")
            self.pose_detect = True

    def wws_detect_img_cb(self, m):
        if self.object_seen:
            self.img = m
            detect = m['image']['url']
            f = urllib.request.urlopen(detect)
            a = plt.imread(f, 0)
            self.object_img = a
            self.img_detect = True

    def control_loop(self):
        self.start()
        while True:
            sleep(0.1)
            print('sleeping...')
            if self.img_detect and self.pose_detect:
                  break
        self.end()
#         print(self.pose)
        s = self.pose['object']['payload']['objects']['detections'][0]['bbox']
        print(f"object position- {np.mean(s[0::2]), np.mean(s[1::2])}")
        # print(self.img)
        print(f'class- {self.class_name}')
        plt.imshow(self.object_img)
        plt.show()


# state = State()
sched.goto(orspy.EulerPose(68.73, 105.66, 0, 0, 0, 0),
                                     allowed_trans_error=0.3, allowed_rot_error=0.5)
# state.control_loop()
# for i in range(100):
#     state.sched.stop_action(i)
# state.delete_state()