from PyQt5.QtWidgets import *
from PyQt5.QtCore import *
from robotControl import *
from serialCommunication import *
from kinematics import *
import numpy as np
import sys
import ast
import os
 


 
class QLabelBuddy(QDialog) :
    def __init__(self,rc):
        self.rc=rc
        super().__init__()
        self.initUI()
 
    def initUI(self):
        self.setWindowTitle('移动到目标位置')
 
        xLabel = QLabel('x',self)#实例一个文本控件对象并设置Text
        xLineEdit = QLineEdit(self)#实例一个文本框控件对象
        # 设置伙伴控件
        xLabel.setBuddy(xLineEdit)
 
        yLabel = QLabel('y',self)#实例一个文本控件对象并设置Text
        yLineEdit = QLineEdit(self)
        # 设置伙伴控件
        yLabel.setBuddy(yLineEdit)

        zLabel = QLabel('z',self)#实例一个文本控件对象并设置Text
        zLineEdit = QLineEdit(self)
        # 设置伙伴控件
        zLabel.setBuddy(zLineEdit)

        pLabel = QLabel('pitch',self)#实例一个文本控件对象并设置Text
        pLineEdit = QLineEdit(self)
        # 设置伙伴控件
        pLabel.setBuddy(pLineEdit)

        J1Label = QLabel('J1',self)#实例一个文本控件对象并设置Text
        J1LineEdit = QLineEdit(self)#实例一个文本框控件对象
        # 设置伙伴控件
        J1Label.setBuddy(J1LineEdit)

        J2Label = QLabel('J2',self)#实例一个文本控件对象并设置Text
        J2LineEdit = QLineEdit(self)#实例一个文本框控件对象
        # 设置伙伴控件
        J2Label.setBuddy(J2LineEdit)

        J3Label = QLabel('J3',self)#实例一个文本控件对象并设置Text
        J3LineEdit = QLineEdit(self)#实例一个文本框控件对象
        # 设置伙伴控件
        J3Label.setBuddy(J3LineEdit)

        J4Label = QLabel('J4',self)#实例一个文本控件对象并设置Text
        J4LineEdit = QLineEdit(self)#实例一个文本框控件对象
        # 设置伙伴控件
        J4Label.setBuddy(J4LineEdit)

        self.label1 = QLabel('J1：%s'%0)
        self.slider1 = QSlider(Qt.Horizontal)
        self.slider1.setMinimum(-90) #设置最小值
        self.slider1.setMaximum(90) #设置最大值
        self.slider1.setValue(0)
        self.slider1.valueChanged.connect(self.valueChange1)
        self.slider1.setSingleStep(5)

        self.label2 = QLabel('J2：%s'%-90)
        self.slider2 = QSlider(Qt.Horizontal)
        self.slider2.setMinimum(-180) #设置最小值
        self.slider2.setMaximum(0) #设置最大值
        self.slider2.setValue(-90)
        self.slider2.valueChanged.connect(self.valueChange2)
        self.slider2.setSingleStep(5)

        self.label3 = QLabel('J3：%s'%0)
        self.slider3 = QSlider(Qt.Horizontal)
        self.slider3.setMinimum(-90) #设置最小值
        self.slider3.setMaximum(90) #设置最大值
        self.slider3.setValue(0)
        self.slider3.valueChanged.connect(self.valueChange3)
        self.slider3.setSingleStep(5)

        self.label4 = QLabel('J4：%s'%0)
        self.slider4 = QSlider(Qt.Horizontal)
        self.slider4.setMinimum(-90) #设置最小值
        self.slider4.setMaximum(90) #设置最大值
        self.slider4.setValue(0)
        self.slider4.valueChanged.connect(self.valueChange4)
        self.slider4.setSingleStep(5)

        self.label5 = QLabel('J5：%s'%0)
        self.slider5 = QSlider(Qt.Horizontal)
        self.slider5.setMinimum(-90) #设置最小值
        self.slider5.setMaximum(90) #设置最大值
        self.slider5.setValue(0)
        self.slider5.valueChanged.connect(self.valueChange5)
        self.slider5.setSingleStep(5)

        self.label6 = QLabel('J6：%s'%660)
        self.slider6 = QSlider(Qt.Horizontal)
        self.slider6.setMinimum(500) #设置最小值
        self.slider6.setMaximum(2500) #设置最大值
        self.slider6.setValue(660)
        self.slider6.valueChanged.connect(self.valueChange6)
        self.slider6.setSingleStep(40)
        


 
        btnOK1 = QPushButton('笛卡尔坐标移动')#实例一个按钮控件对象
        btnOK2 = QPushButton('关节坐标移动')#实例一个按钮控件对象
        btnCancel = QPushButton('关闭')#实例一个按钮控件对象
        btnclose = QPushButton('夹取')#实例一个按钮控件对象
        btnopen = QPushButton('打开')#实例一个按钮控件对象
        btnreturn = QPushButton('恢复初始点')#实例一个按钮控件对象
        btnsave = QPushButton('记录关节坐标')#实例一个按钮控件对象
        btndelete = QPushButton('删除记录坐标')#实例一个按钮控件对象
        btnmidjoint = QPushButton('移动  关节坐标')#实例一个按钮控件对象
        btnmidposition = QPushButton('移动笛卡尔坐标')#实例一个按钮控件对象
 
        mainLayout = QGridLayout(self)#实例一个网格布局器并指定父函数
        mainLayout.addWidget(xLabel,0,0)#第0行第0列，占1行1列
        mainLayout.addWidget(xLineEdit,0,1,1,1)#第0行第1列，占1行1列
 
        mainLayout.addWidget(yLabel,1,0)#第1行第0列，占1行1列
        mainLayout.addWidget(yLineEdit,1,1,1,1)#第1行第1列，占1行1列

        mainLayout.addWidget(zLabel,2,0)#第1行第0列，占1行1列
        mainLayout.addWidget(zLineEdit,2,1,1,1)#第1行第1列，占1行1列

        mainLayout.addWidget(pLabel,3,0)#第1行第0列，占1行1列
        mainLayout.addWidget(pLineEdit,3,1,1,1)#第1行第1列，占1行1列

        mainLayout.addWidget(J1Label,0,2)#第0行第0列，占1行1列
        mainLayout.addWidget(J1LineEdit,0,3,1,1)#第0行第1列，占1行1列

        mainLayout.addWidget(J2Label,1,2)#第0行第0列，占1行1列
        mainLayout.addWidget(J2LineEdit,1,3,1,1)#第0行第1列，占1行1列

        mainLayout.addWidget(J3Label,2,2)#第0行第0列，占1行1列
        mainLayout.addWidget(J3LineEdit,2,3,1,1)#第0行第1列，占1行1列

        mainLayout.addWidget(J4Label,3,2)#第0行第0列，占1行1列
        mainLayout.addWidget(J4LineEdit,3,3,1,1)#第0行第1列，占1行1列

        

        mainLayout.addWidget(self.label1,0,4)#第0行第0列，占1行1列
        mainLayout.addWidget(self.slider1,1,4,1,2) #第0行第1列，占1行2列

        mainLayout.addWidget(self.label2,2,4)#第0行第0列，占1行1列
        mainLayout.addWidget(self.slider2,3,4,1,2) #第0行第1列，占1行2列

        mainLayout.addWidget(self.label3,4,4)#第0行第0列，占1行1列
        mainLayout.addWidget(self.slider3,5,4,1,2) #第0行第1列，占1行2列

        mainLayout.addWidget(self.label4,0,6)#第0行第0列，占1行1列
        mainLayout.addWidget(self.slider4,1,6,1,2) #第0行第1列，占1行2列

        mainLayout.addWidget(self.label5,2,6)#第0行第0列，占1行1列
        mainLayout.addWidget(self.slider5,3,6,1,2) #第0行第1列，占1行2列

        mainLayout.addWidget(self.label6,4,6)#第0行第0列，占1行1列
        mainLayout.addWidget(self.slider6,5,6,1,2) #第0行第1列，占1行2列
 
        

        mainLayout.addWidget(btnOK1,4,1)#第2行第1列，占1行1列
        mainLayout.addWidget(btnOK2,4,3)#第2行第1列，占1行1列
        mainLayout.addWidget(btnclose,5,1)#第2行第1列，占1行1列
        mainLayout.addWidget(btnopen,5,3)#第2行第2列，占1行1列
        mainLayout.addWidget(btnCancel,7,1,1,3)#第2行第2列，占1行1列
        mainLayout.addWidget(btnreturn,6,1,1,3)#第2行第2列，占1行1列
        mainLayout.addWidget(btnsave,6,4)#第2行第2列，占1行1列
        mainLayout.addWidget(btndelete,6,6)#第2行第2列，占1行1列
        mainLayout.addWidget(btnmidjoint,7,4)#第2行第2列，占1行1列
        mainLayout.addWidget(btnmidposition,7,6)#第2行第2列，占1行1列
        
        btnOK1.clicked.connect(lambda:self.xyzmove(xLineEdit.text()
        +","+yLineEdit.text()+","+zLineEdit.text()+","+pLineEdit.text()))
        btnOK2.clicked.connect(lambda:self.jointmove(J1LineEdit.text()
        +","+J2LineEdit.text()+","+J3LineEdit.text()+","+J4LineEdit.text()))
        btnclose.clicked.connect(self.close1)
        btnopen.clicked.connect(self.open1)
        btnCancel.clicked.connect(self.cancelClicked)
        btnreturn.clicked.connect(self.return1)
        btnsave.clicked.connect(self.save1)
        btndelete.clicked.connect(self.delete1)
        btnmidjoint.clicked.connect(self.midjoint)
        btnmidposition.clicked.connect(self.midposition)
        
    def xyzmove(self,text):     
        print(text)

        lst = [float(x) if x !='n' else None for x in text.split(",")]
        
        if lst[3]!=None:
            lst[3] = lst[3]/57.3
        print(lst[3])
        self.rc.move_to_point(lst[:3],pitch = lst[3])
        sleep(1.5)

    def jointmove(self,text):     
        print(text)
        ik=[]
        float_list=[float(x) for x in text.split(',')]
        for i in range(len(float_list)):
            a=float(float_list[i]*np.pi/180)
            ik.append(a)
        print(ik)
        self.rc.move_to_joints(ik)
        sleep(1.5)

    def close1(self):
        self.rc.scom.Send_data('#005P' + '2500' + 'T1000!')
    def open1(self):
        self.rc.scom.Send_data('#005P' + '1500' + 'T1000!')
    def cancelClicked(self):
        self.close()
    def return1(self):
        self.rc.scom.Send_data('#004P' + '1380' + 'T1000!')
        sleep(0.5)
        ik=[0,-np.pi/2,0,0]
        self.rc.move_to_joints(ik)
        self.joints_at_present = [0,-np.pi/2,0,0,0]
        self.position_at_present = [0,0,0.266]
    
    def save1(self):
        with open('joints.txt', "a") as f:
            list =[]
            list.append([self.slider1.value()/57.3,self.slider2.value()/57.3,self.slider3.value()/57.3,self.slider4.value()/57.3])
            f.write(",".join(str(x) for x in list) + "\n")
        with open('positions.txt', "a") as f:
            list1=[]
            list =[self.slider1.value()/57.3,self.slider2.value()/57.3,self.slider3.value()/57.3,self.slider4.value()/57.3]
            Descarteslist=Geometric(list)
            list1.append(Descarteslist)
            print(list1)
            f.write(",".join(str(x) for x in list1) + "\n")

    
    def delete1(self):
        with open(r'joints.txt','a+',encoding='utf-8') as test:
            test.truncate(0)
        with open(r'positions.txt','a+',encoding='utf-8') as test:
            test.truncate(0)
    def midjoint(self):
        with open('joints.txt', "r") as f:
            data = f.read().splitlines()
        joint_s=("[" + ", ".join(data) + "]")
        list = ast.literal_eval(joint_s)
        for i in range(len(list)):
            self.rc.move_to_joints(list[i])
            sleep(1.5)
    def midposition(self):
        with open('positions.txt', "r") as f:
            data = f.read().splitlines()
        joint_s=("[" + ", ".join(data) + "]")
        list = ast.literal_eval(joint_s)
        for i in range(len(list)):
            self.rc.move_to_point(list[i])
            sleep(1.5)


   

    def valueChange1(self):
        self.label1.setText('J1：%s'%self.slider1.value())
        print('J1=%s' % self.slider1.value())
        pwm=(int(700 * (self.slider1.value()) / 90 + 1580), int(730 * (self.slider1.value()) / 90 + 1580))[self.slider1.value() > 0]
        data =''
        data += f'#000P' + '{0:0>{1}}'.format(pwm, 4) + 'T1000!'
        self.rc.scom.Send_data('{'+data+'}')
    def valueChange2(self):
        self.label2.setText('J2：%s'%self.slider2.value())
        print('J2=%s' % self.slider2.value())
        pwm=(int(1000 * (self.slider2.value()+90) / 90 + 1500),int(700 * (self.slider2.value()+90) / 90 + 1500))[(self.slider2.value())< -1.57]
        data =''
        data += f'#001P' + '{0:0>{1}}'.format(pwm, 4) + 'T1000!'
        self.rc.scom.Send_data('{'+data+'}')
    def valueChange3(self):
        self.label3.setText('J3：%s'%self.slider3.value())
        print('J3=%s' % self.slider3.value())
        pwm=(int(-660 * (self.slider3.value()) / 90 + 1540), int(-640 * (self.slider3.value()) / 90 + 1540))[self.slider3.value() > 0]
        data =''
        data += f'#002P' + '{0:0>{1}}'.format(pwm, 4) + 'T1000!'
        self.rc.scom.Send_data('{'+data+'}')
    def valueChange4(self):
        self.label4.setText('J4：%s'%self.slider4.value())
        print('J4=%s' % self.slider4.value())
        pwm=(int(-810 * (self.slider4.value()) / 90 + 1660), int(-590 * (self.slider4.value()) / 90 + 1660))[self.slider4.value() > 0]
        data =''
        data += f'#003P' + '{0:0>{1}}'.format(pwm, 4) + 'T1000!'
        self.rc.scom.Send_data('{'+data+'}')
    def valueChange5(self):
        self.label5.setText('J5：%s'%self.slider5.value())
        print('J5=%s' % self.slider5.value())
        pwm=(int(970 * (self.slider5.value()) / 90 + 1380), int(530 * (self.slider5.value()) / 90 + 1380))[self.slider5.value() > 0]
        data =''
        data += f'#004P' + '{0:0>{1}}'.format(pwm, 4) + 'T1000!'
        self.rc.scom.Send_data('{'+data+'}')
    def valueChange6(self):
        self.label6.setText('J6：%s'%self.slider6.value())
        print('J6=%s' % self.slider6.value())
        data =''
        data += f'#005P' + '{0:0>{1}}'.format(self.slider6.value(), 4) + 'T1000!'
        self.rc.scom.Send_data('{'+data+'}')
    
if __name__ == '__main__':
    app = QApplication(sys.argv)
    main = QLabelBuddy()
    main.show()
    sys.exit(app.exec_())