#!/usr/bin/python
import rospy
import yaml
import sys
import os
from python_qt_binding.QtWidgets import QApplication, QWidget, QVBoxLayout,QHBoxLayout,QGridLayout, QLabel, QSlider, QLineEdit, QPushButton
from python_qt_binding.QtCore import Signal, Qt,  pyqtSlot
from python_qt_binding.QtGui import QFont
from threading import Thread
import signal
from geometry_msgs.msg import Quaternion
from functools import reduce
from numpy import pi
from tf.transformations import quaternion_from_euler

RANGE = 1000

def rsetattr(obj, attr, val):
    pre, _, post = attr.rpartition('.')
    if pre:
        return setattr(rgetattr(obj, pre), post, val)
    # convert to expected field type
    return setattr(obj, post, type(getattr(obj, post))(val))

def rgetattr(obj, attr, *args):
    if attr == '':
        return obj
    def _getattr(obj, attr):
        return getattr(obj, attr, *args)
    return reduce(_getattr, [obj] + attr.split('.'))

def split_field(key):
    if '.' in key:
        return key.rsplit('.', 1)
    return '', key

def isRPY(key, msg):
    is_axis = False
    field, axis = split_field(key)
    if axis in ('roll', 'pitch', 'yaw'):
        return type(rgetattr(msg, field)) == Quaternion
    return False

def robust_eval(val):
    if type(val) in (list,tuple):
        return [robust_eval(v) for v in val]
    if type(val) == str:
        val_expr = val.strip().lower()
        
        # check for  Pi fractions 
        for sign, sign_rep in ((1, ''), (-1, '-')):
            if val_expr == sign_rep + 'pi':
                return sign*pi
            
            for denom in range(2, 13):
                if val_expr == sign_rep + 'pi/' + str(denom):
                    return sign * pi/denom
        return val
    
    return float(val)

def key_tag(topic, key):
    return topic + '/' + key

class Publisher:
    def __init__(self, topic, msg, info):
        self.topic = topic
        self.msg = msg()
        self.pub = rospy.Publisher(topic, msg, queue_size=1)
        self.rpy = {}
            
        # init map from GUI to message
        self.map = {}
        to_remove = []
        for key in info:
            tag = key_tag(topic, key)
            if type(info[key]) == dict:
                if isRPY(info[key]['to'], self.msg):
                    field, axis = split_field(info[key]['to'])
                    if field not in self.rpy:
                        self.rpy[field] = {'roll': 0, 'pitch': 0, 'yaw': 0}
                self.map[tag] = info[key]['to']
                info[key].pop('to')
            else:
                if key != 'type':
                    # init non-zero defaults
                    if isRPY(key, self.msg):
                        field, axis = split_field(key)
                        if field not in self.rpy:
                            self.rpy[field] = {'roll': 0, 'pitch': 0, 'yaw': 0}
                        self.rpy[field][axis]  = robust_eval(info[key])
                    else:
                        self.write(key, robust_eval(info[key]))
                to_remove.append(key)
        for rm in to_remove:
            info.pop(rm)
                           
    def write(self, key, val):
        
        field, axis = split_field(key)
        if field in self.rpy:
            self.rpy[field][axis] = val
        elif '[' in key:        
            field, idx = key[:-1].split('[')
            idx = int(idx)
            current = rgetattr(self.msg, field)
            if len(current) <= idx:
                if 'name' in field:
                    current += ['' for i in range(idx +1 - len(current))]
                else:
                    current += [0 for i in range(idx +1 - len(current))]
            current[idx] = val
            rsetattr(self.msg, field, current)
        else:
            rsetattr(self.msg, key, val)
        
    def update(self, values):
        for tag in self.map:
            self.write(self.map[tag], values[tag]['val'])
        # write RPY's to Quaternions
        for field in self.rpy:
            q = quaternion_from_euler(self.rpy[field]['roll'],self.rpy[field]['pitch'], self.rpy[field]['yaw'])
            for idx, axis in enumerate(('x','y','z','w')):
                if field:
                    rsetattr(self.msg, field + '.' + axis, q[idx])
                else:
                    setattr(self.msg, axis, q[idx])
        # update time if stamped msg
        if hasattr(self.msg, "header"):
            self.write('header.stamp', rospy.Time.now())
        self.pub.publish(self.msg)


class SliderPublisher(QWidget):
    def __init__(self, content):
        super(SliderPublisher, self).__init__()
        
        content = content.replace('\t', '    ')
        
        # get message types
        self.publishers = {}
        self.values = {}
        pkgs = []
        
        # to keep track of key ordering in the yaml file
        order = []
        old = []
         
        for topic, info in yaml.safe_load(content).items():
            pkg,msg = info['type'].split('/')
            pkgs.append(__import__(pkg, globals(), locals(), ['msg']))
            self.publishers[topic] = Publisher(topic, getattr(pkgs[-1].msg, msg), info)
            order.append((topic,[]))
            for key in info:
                tag = key_tag(topic,key)
                self.values[tag] = info[key]
                order[-1][1].append((content.find(' ' + key + ':'), key))
                old.append((content.find(' ' + key + ':'), key))
                for bound in ['min', 'max']:
                    self.values[tag][bound] = robust_eval(self.values[tag][bound])
                self.values[tag]['val'] = 0
            order[-1][1].sort()
        order.sort(key = lambda x: x[1][0][0])
        # build sliders - thanks joint_state_publisher
        sliderUpdateTrigger = Signal()
        self.vlayout = QVBoxLayout(self)
        self.gridlayout = QGridLayout()
        font = QFont("Helvetica", 9, QFont.Bold)
        topic_font = QFont("Helvetica", 10, QFont.Bold)
        
        sliders = []
        self.key_map = {}
        y = 0
        for topic,keys in order:
            topic_layout = QVBoxLayout()
            label = QLabel(topic)
            label.setFont(topic_font)
            topic_layout.addWidget(label)
            self.gridlayout.addLayout(topic_layout, *(y, 0))
            y+=1
            for idx,key in keys:
                tag = key_tag(topic,key)
                key_layout = QVBoxLayout()
                row_layout = QHBoxLayout()
                label = QLabel(key)
                label.setFont(font)
                row_layout.addWidget(label)
                
                display = QLineEdit("0.00")
                display.setAlignment(Qt.AlignRight)
                display.setFont(font)
                display.setReadOnly(True)
                
                row_layout.addWidget(display)    
                key_layout.addLayout(row_layout)
                
                slider = QSlider(Qt.Horizontal)
                slider.setFont(font)
                slider.setRange(0, RANGE)
                slider.setValue(int(RANGE/2))
                
                key_layout.addWidget(slider)
            
                self.key_map[tag] = {'slidervalue': 0, 'display': display, 'slider': slider}
                slider.valueChanged.connect(self.onValueChanged)
                self.gridlayout.addLayout(key_layout, *(y,0))
                y+=1
                #sliders.append(key_layout)
        
            # Generate positions in grid and place sliders there
            #self.positions = [(y,0) for y in range(len(sliders))]
            #for item, pos in zip(sliders, self.positions):
            #    self.gridlayout.addLayout(item, *pos)
            
        self.vlayout.addLayout(self.gridlayout)            
        
        self.ctrbutton = QPushButton('Center', self)
        self.ctrbutton.clicked.connect(self.center)
        self.vlayout.addWidget(self.ctrbutton)
            
        self.center(1)
        
    def sliderToValue(self, slider, tag):
        val = self.values[tag]
        return val['min'] + slider*(val['max'] - val['min'])/RANGE            
        
    @pyqtSlot(int)
    def onValueChanged(self, event):
        # A slider value was changed, but we need to change the joint_info metadata.
        for key, key_info in self.key_map.items():
            key_info['slidervalue'] = key_info['slider'].value()
            # build corresponding value                        
            self.values[key]['val'] = self.sliderToValue(key_info['slidervalue'], key)    
            key_info['display'].setText("%.2f" % self.values[key]['val'])        
            
    def center(self, event):
        for key, key_info in self.key_map.items():
            key_info['slider'].setValue(int(RANGE/2))
        self.onValueChanged(event)
            
        
    def loop(self):        
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            for pub in self.publishers:
                self.publishers[pub].update(self.values)
            rate.sleep()
            
if __name__ == "__main__":
    
    rospy.init_node('slider_publisher')
    
    # read passed param
    filename = sys.argv[-1]
    if not os.path.exists(filename):
        if not rospy.has_param("~file"):
            rospy.logerr("Pass a yaml file (~file param or argument)")
            sys.exit(0)
        filename = rospy.get_param("~file")
        
    
    # also get order from file
    with open(filename) as f:
        content = f.read()
                        
    # build GUI
    title = rospy.get_name().split('/')[-1]
    app = QApplication([title])    
    sp = SliderPublisher(content)
    #pause
    Thread(target=sp.loop).start()
    signal.signal(signal.SIGINT, signal.SIG_DFL)
    sp.show()
    sys.exit(app.exec_())
    
