import sys
from PyQt5.QtWidgets import QLabel,QSlider,QVBoxLayout,QWidget,QApplication,QMainWindow,QGridLayout
from PyQt5.QtCore import *
from PyQt5.QtWidgets import QHBoxLayout,QPushButton
from robotControl import *
from kinematics import *
import os

class QSliderDemo(QMainWindow):
    def __init__(self,rc):
        super(QSliderDemo, self).__init__()
        self.rc=rc
        #设置窗口大小
        self.resize(400, 150)
        self.setWindowTitle("垂直移动")
        a1='X：%s'%(self.rc.position_at_present[0]*1000)
        self.label1 = QLabel(a1)
        self.slider1 = QSlider(Qt.Horizontal)
        self.slider1.setMinimum(-266) #设置最小值
        self.slider1.setMaximum(266) #设置最大值
        self.slider1.setValue(int(self.rc.position_at_present[0]*1000))
        self.slider1.setSingleStep(1) #设置滑动步长
        self.slider1.valueChanged.connect(self.valueChanged)
        self.slider1.sliderReleased.connect(self.sliderReleased)

        a2='Y：%s'%(self.rc.position_at_present[1]*1000)
        self.label2 = QLabel(a2)
        self.slider2 = QSlider(Qt.Horizontal)
        self.slider2.setMinimum(-266) #设置最小值
        self.slider2.setMaximum(266) #设置最大值
        self.slider2.setValue(int(self.rc.position_at_present[1]*1000))
        self.slider2.setSingleStep(1) #设置滑动步长
        self.slider2.valueChanged.connect(self.valueChanged)
        self.slider2.sliderReleased.connect(self.sliderReleased)

        a3='Z：%s'%(self.rc.position_at_present[2]*1000)
        self.label3 = QLabel(a3)
        self.slider3 = QSlider(Qt.Horizontal)
        self.slider3.setMinimum(0) #设置最小值
        self.slider3.setMaximum(266) #设置最大值
        self.slider3.setValue(int(self.rc.position_at_present[2]*1000))
        self.slider3.setSingleStep(1) #设置滑动步长
        self.slider3.valueChanged.connect(self.valueChanged)
        self.slider3.sliderReleased.connect(self.sliderReleased)

        #创建水平布局
        layout= QGridLayout(self)
        #layout = QVBoxLayout()
        layout.addWidget(self.label1,0,0)#第0行第0列，占1行1列
        layout.addWidget(self.slider1,1,0,1,2) #第0行第1列，占1行2列
        layout.addWidget(self.label2,2,0)
        layout.addWidget(self.slider2,3,0,1,2)
        layout.addWidget(self.label3,4,0)
        layout.addWidget(self.slider3,5,0,1,2)
       
        
        self.__btn_square=QPushButton("矩形移动")
        self.__btn_quit=QPushButton("退出")
        self.__btn_square.setParent(self)
        self.__btn_quit.setParent(self)
        self.__btn_square.clicked.connect(self.square)
        self.__btn_quit.clicked.connect(self.quit)
        layout.addWidget(self.__btn_square)
        layout.addWidget(self.__btn_quit)
        '''
        self.__btn_weizhi=QPushButton("关闭")
        self.__btn_weizhi.setParent(self)
        self.__btn_weizhi.clicked.connect(self.quit)
        layout.addWidget(self.__btn_weizhi)
        '''



        mainFrame = QWidget()
        mainFrame.setLayout(layout)
        self.setCentralWidget(mainFrame)

    def valueChanged(self):
        self.label1.setText('X：%s'%self.slider1.value())
        
        self.label2.setText('Y：%s'%self.slider2.value())
        
        self.label3.setText('Z：%s'%self.slider3.value())
        
        print('X=%s' % self.slider1.value())
        print('Y=%s' % self.slider2.value())
        print('Z=%s' % self.slider3.value())
    def sliderReleased(self):
        x1=self.slider1.value()/1000
        y1=self.slider2.value()/1000
        z1=self.slider3.value()/1000
        ik=[]
        ik.append(x1)
        ik.append(y1)
        ik.append(z1)
        print(ik)
        a=ana_ik(ik)
        print(a)
        self.rc.straight_line1(ik)
        #self.rc.move_to_joints(a)
        sleep(1.5)
        #os.system("python robotControl(1).py {} {} {}".format(self.slider1.value(),self.slider2.value(),self.slider3.value()))
    '''
    def ok(self):
        x1=self.slider1.value()/1000
        y1=self.slider2.value()/1000
        z1=self.slider3.value()/1000
        ik=[]
        ik.append(x1)
        ik.append(y1)
        ik.append(z1)
        print(ik)
        a=ana_ik(ik)
        print(a)
        
        x=x1-self.rc.position_at_present[0]
        y=y1-self.rc.position_at_present[1]
        z=z1-self.rc.position_at_present[2]
        if x!=0:
            self.rc.straight_line(ik,'x')
        if y!=0:
            self.rc.straight_line(ik,'y')
        if z!=0:
            self.rc.straight_line(ik,'z')
        
        self.rc.straight_line1(ik)
        #self.rc.move_to_joints(a)
        sleep(1.5)
    '''
    def square(self):
        self.rc.print_sqare()
    def quit(self):
        self.close()


if  __name__ == '__main__':
    app = QApplication(sys.argv)
    main = QSliderDemo()
    main.show()
    sys.exit(app.exec_())