#!/usr/bin/env python
# encoding: utf-8

from PyQt5.QtWidgets import *
from PyQt5.QtGui import *
from PyQt5.QtCore import *
from pyqtgraph import GraphicsLayoutWidget
import pyqtgraph.opengl as gl
from pyqtgraph.opengl import GLViewWidget
import pyqtgraph as pg
pg.setConfigOption('background', '#E3E6E3')
pg.setConfigOption('foreground', 'k')
from win32api import GetSystemMetrics
import numpy as np
import re
SIZE_X = 400
SIZE_Y = 400
SIZE_Z = 400

class RayCanvasWidget(GLViewWidget):
    def __init__(self):
        super(RayCanvasWidget, self).__init__()
        # w = GetSystemMetrics (0)
        # h = GetSystemMetrics (1)
        # self.setMinimumWidth(w/3)
        self.opts['distance'] = 40
        self.opts['elevation'] = 0
        self.opts['azimuth'] = 0
        self.gx = gl.GLGridItem()
        self.gx.rotate(90, 0, 1, 0)
        self.gx.translate(-10, 0, 0)
        # self.addItem(self.gx)
        self.gy = gl.GLGridItem()
        self.gy.rotate(90, 1, 0, 0)
        self.gy.translate(0, -10, 0)
        # self.addItem(self.gy)
        self.gz = gl.GLGridItem()
        self.gz.translate(0, 0, -10)
        # self.addItem(self.gz)
        # set the scale
        self.setScale(SIZE_X,SIZE_Y,SIZE_Z)
        self.angle = 0
        # 参数设置
        # self.updateItems()
        # 动态移动点
        self.points = [[0,0,0]]
        self.sp_move = gl.GLScatterPlotItem(pos=np.array(self.points), size=0.4, color=(0.8, 0.7, 0.0, 1), pxMode=False)
        self.addItem(self.sp_move)
        # 路线移动点
        self.sp_road_move = gl.GLScatterPlotItem(pos=np.array(self.points), size=0.4, color=(1.0, 0.0, 0.0, 1), pxMode=False)
        self.addItem(self.sp_road_move)

        self.timer = QTimer()
        self.timer.timeout.connect(self.TimerMove)
        self.timer_cnt = 0
        self.timer.start(100)

        self.updateItems(angle=0,
                         ssd=-100,
                         raySize_x=10,
                         raySize_y=10,
                         raySizeEdge_x=0,
                         raySizeEdge_y=0,
                         deepth=0
                         )

    def setScale(self, size_x, size_y, size_z):
        '''设置实际坐标转换opengl坐标系数'''
        # axis range ---> x,y,z
        self.x_min = -20
        self.x_max = 20
        self.y_min = -20
        self.y_max = 20
        self.z_min = -20
        self.z_max = 20
        # set real range
        self.x_range = size_x
        self.y_range = size_y
        self.z_range = size_z
        # get transform scale
        self.x_scale = (self.x_max-self.x_min)/self.x_range
        self.y_scale = (self.y_max-self.y_min)/self.y_range
        self.z_scale = (self.z_max-self.z_min)/self.z_range

    def axisToGL(self,x,y,z):
        '''按比例返回opengl中xyz坐标'''
        x = x*self.x_scale/2
        y = y*self.y_scale/2
        z = (self.z_range/2-z)*self.z_scale/2
        return x,y,z

    def drawWaterTank(self):
        '''画水箱'''
        size = 21
        # z = np.array([[0 for i in range(size)] for i in range(size)])
        # p1 = gl.GLSurfacePlotItem(z=z, shader='shaded', color=(0.5, 0.5, 1, 0.1))
        # p1.translate(-10, -10, -10)
        # self.addItem(p1)
        # p2 = gl.GLSurfacePlotItem(z=z, shader='shaded', color=(0.5, 0.5, 1, 0.1))
        # p2.translate(-10, -10, -10)
        # p2.rotate(90, 1, 0, 0)
        # self.addItem(p2)
        # p3 = gl.GLSurfacePlotItem(z=z, shader='shaded', color=(0.5, 0.5, 1, 1))
        # p3.translate(-10, -10, -10)
        # p3.rotate(90, 0, 1, 0)
        # self.addItem(p3)
        # p4 = gl.GLSurfacePlotItem(z=z, shader='shaded', color=(0.5, 0.5, 1, 1))
        # p4.translate(-10, -10, 10)
        # p4.rotate(90, 1, 0, 0)
        # self.addItem(p4)

    def drawRayDirection(self):
        """加速器方向绘画"""
        clr = pg.glColor(0,0,200)

        # 中心点位置
        zero_x = -0
        zero_y = -0
        zero_z = 20

        zero = [zero_x,zero_y,zero_z]
        self.zero_ray_direction= zero

        x = [zero_x+5,zero_y,zero_z]
        y = [zero_x,zero_y+5,zero_z]
        z = [zero_x,zero_y,zero_z-5]

        # X 箭头坐标
        line_x = gl.GLLinePlotItem(pos=np.array([zero,x]), color=clr, width=3, antialias=True)
        arrow_left = [zero_x+4,zero_y,zero_z-1]
        arrow_right = [zero_x+4,zero_y,zero_z+1]
        line_x_1 = gl.GLLinePlotItem(pos=np.array([x,arrow_left]), color=clr, width=3, antialias=True)
        line_x_2 = gl.GLLinePlotItem(pos=np.array([x,arrow_right]), color=clr, width=3, antialias=True)
        self.addItem(line_x)
        self.addItem(line_x_1)
        self.addItem(line_x_2)
        # Y 箭头坐标
        line_y = gl.GLLinePlotItem(pos=np.array([zero,y]), color=clr, width=3, antialias=True)
        arrow_left = [zero_x,zero_y+4,zero_z-1]
        arrow_right = [zero_x,zero_y+4,zero_z+1]
        line_y_1 = gl.GLLinePlotItem(pos=np.array([y,arrow_left]), color=clr, width=3, antialias=True)
        line_y_2 = gl.GLLinePlotItem(pos=np.array([y,arrow_right]), color=clr, width=3, antialias=True)
        self.addItem(line_y)
        self.addItem(line_y_1)
        self.addItem(line_y_2)
        # Z
        arrow_left = [zero_x,zero_y-1,zero_z-4]
        arrow_right = [zero_x,zero_y+1,zero_z-4]
        line_z = gl.GLLinePlotItem(pos=np.array([zero,z]), color=clr, width=3, antialias=True)
        line_z_1 = gl.GLLinePlotItem(pos=np.array([z,arrow_left]), color=clr, width=3, antialias=True)
        line_z_2 = gl.GLLinePlotItem(pos=np.array([z,arrow_right]), color=clr, width=3, antialias=True)
        self.addItem(line_z)
        self.addItem(line_z_1)
        self.addItem(line_z_2)

    def drawBoxDirection(self):
        '''水箱方向'''
        # draw direction A->B
        self.vLine = gl.GLLinePlotItem(pos=np.array([[self.x_min/2,0,self.z_min/2],[self.x_max/2,0,self.z_min/2]]), color=pg.glColor(0,250,255), width=1, antialias=True)
        self.addItem(self.vLine)
        # draw direction G->T
        self.hLine = gl.GLLinePlotItem(pos=np.array([[0,self.y_min/2,self.z_min/2],[0,self.y_max/2,self.z_min/2]]), color=pg.glColor(0,250,255), width=1, antialias=True)
        self.addItem(self.hLine)
        # draw direction Z
        self.zLine = gl.GLLinePlotItem(pos=np.array([[0,0,self.z_min/2],[0,0,self.z_max/2]]), color=pg.glColor(0,250,255), width=1, antialias=True)
        self.addItem(self.zLine)



        # 中心点位置
        zero_x = -10
        zero_y = 0
        zero_z = 10

        zero = [zero_x,zero_y,zero_z]
        self.zero_box_direction= zero

        x = [zero_x+5,zero_y,zero_z]
        y = [zero_x,zero_y+5,zero_z]
        z = [zero_x,zero_y,zero_z-5]

        clr = pg.glColor(200,0,0)
        # X 箭头坐标
        x1 = [10,-10,10]
        x2 = [-10,-10,10]
        arrow_left = [x2[0]+4,x2[1],x2[2]-1]
        arrow_right = [x2[0]+4,x2[1],x2[2]+1]
        line_x = gl.GLLinePlotItem(pos=np.array([x1,x2]), color=clr, width=1, antialias=True)
        line_x_1 = gl.GLLinePlotItem(pos=np.array([x2,arrow_left]), color=clr, width=1, antialias=True)
        line_x_2 = gl.GLLinePlotItem(pos=np.array([x2,arrow_right]), color=clr, width=1, antialias=True)
        self.addItem(line_x)
        self.addItem(line_x_1)
        self.addItem(line_x_2)
        # Y 箭头坐标
        y1 = [-10,-10,10]
        y2 = [-10,10,10]
        arrow_left = [y2[0],y2[1]-4,y2[2]-1]
        arrow_right = [y2[0],y2[1]-4,y2[2]+1]
        line_y = gl.GLLinePlotItem(pos=np.array([y1,y2]), color=clr, width=1, antialias=True)
        line_y_1 = gl.GLLinePlotItem(pos=np.array([y2,arrow_left]), color=clr, width=1, antialias=True)
        line_y_2 = gl.GLLinePlotItem(pos=np.array([y2,arrow_right]), color=clr, width=1, antialias=True)
        self.addItem(line_y)
        self.addItem(line_y_1)
        self.addItem(line_y_2)
        # Z
        z1 = [-10,10,10]
        z2 = [-10,10,-10]
        arrow_left = [z2[0],z2[1]-1,z2[2]+4]
        arrow_right = [z2[0],z2[1]+1,z2[2]+4]
        line_z = gl.GLLinePlotItem(pos=np.array([z1,z2]), color=clr, width=1, antialias=True)
        line_z_1 = gl.GLLinePlotItem(pos=np.array([z2,arrow_left]), color=clr, width=1, antialias=True)
        line_z_2 = gl.GLLinePlotItem(pos=np.array([z2,arrow_right]), color=clr, width=1, antialias=True)
        self.addItem(line_z)
        self.addItem(line_z_1)
        self.addItem(line_z_2)

    def drawXYZ(self):
        # line.rotate(90, 0, 1, 0)
        pass

    def drawRayQuad(self,x1,y1,x2,y2,z):
        '''画射野四边形'''
        x1,y1,d = self.axisToGL(x1,y1,z)
        x2,y2,d = self.axisToGL(x2,y2,z)
        axis = [[x1,y1,d],
                [x2,y1,d],
                [x2,y2,d],
                [x1,y2,d],
                [x1,y1,d],]
        plt = gl.GLLinePlotItem(pos=np.array(axis), color=pg.glColor(255,255,255), width=1, antialias=True)
        self.addItem(plt)

    def drawRayEdgeQuad(self,x1,y1,x2,y2,z):
        '''画射野边缘四边形'''
        x1,y1,d = self.axisToGL(x1,y1,z)
        x2,y2,d = self.axisToGL(x2,y2,z)
        axis = [[x1,y1,d],
                [x2,y1,d],
                [x2,y2,d],
                [x1,y2,d],
                [x1,y1,d],]
        plt = gl.GLLinePlotItem(pos=np.array(axis), color=pg.glColor(0,150,225), width=1, antialias=True)
        self.addItem(plt)

    def drawLight(self,x1,y1,x2,y2,z):
        '''画光线示意图'''
        x1 = x1*(abs(self.z_range)+abs(self.SSD))/(abs(self.SSD)+abs(z))
        x2 = x2*(abs(self.z_range)+abs(self.SSD))/(abs(self.SSD)+abs(z))
        y1 = y1*(abs(self.z_range)+abs(self.SSD))/(abs(self.SSD)+abs(z))
        y2 = y2*(abs(self.z_range)+abs(self.SSD))/(abs(self.SSD)+abs(z))
        z = self.z_range
        x1,y1,d = self.axisToGL(x1,y1,z)
        x2,y2,d = self.axisToGL(x2,y2,z)
        x_l,y_l,ssd = self.axisToGL(0,0,self.SSD)
        axis = [[x1,y1,d],
                [0,0,ssd],
                [x2,y1,d],
                [0,0,ssd],
                [x2,y2,d],
                [0,0,ssd],
                [x1,y2,d],
                [0,0,ssd],
                [x1,y1,d],
                ]
        plt = gl.GLLinePlotItem(pos=np.array(axis), color=pg.glColor(255,255,255), width=1, antialias=True)
        self.addItem(plt)
        axis = [[x1,y1,d],
                [x2,y1,d],
                [x2,y2,d],
                [x1,y2,d],
                [x1,y1,d],]
        plt = gl.GLLinePlotItem(pos=np.array(axis), color=pg.glColor(150,0,5), width=2, antialias=True)
        self.addItem(plt)

    def drawLightPoint(self,ssd):
        x,y,z = self.axisToGL(0,0,ssd)
        sp_light = gl.GLScatterPlotItem(pos=np.array([[0,0,z]]), size=3.0, color=(1.0, 0, 0.0, 1), pxMode=False)
        self.addItem(sp_light)

    def drawRoad(self,road):
        # if self.angle == 45:
            # road = [[r[0],r[1],r[2]] for r in road]
        # if self.angle == 135:
            # road = [[r[1],-r[0],r[2]] for r in road]
        # if self.angle == 225:
            # road = [[-r[0],-r[1],r[2]] for r in road]
        # if self.angle == 315:
            # road = [[-r[1],r[0],r[2]] for r in road]
        # 下位机更换了x轴走位方向, 将x取反
        self.points = [[-r[0],r[1],r[2]] for r in road]
        self.addItem(self.sp_move)
        self.addItem(self.sp_road_move)
        new_road = []
        for r in road:
            x,y,z = self.axisToGL(-r[0],r[1],r[2])
            new_road.append([x,y,z])
        sp_road = gl.GLScatterPlotItem(pos=np.array(new_road), size=0.1, color=(0.0, 1.0, 0.0, 1), pxMode=False)
        self.addItem(sp_road)

        # road_line = gl.GLLinePlotItem(pos=np.array(new_road), color=pg.glColor(250,255,5), width=1, antialias=True)
        # road_line = gl.GLScatterPlotItem(pos=np.array([new_road[0],new_road[-1]]), size=3.0, color=(1.0, 0, 0.0, 1), pxMode=False)
        # self.addItem(road_line)

    def updateItems(self, angle=0, ssd=-100,raySize_x=10,raySize_y=10,raySizeEdge_x=0,raySizeEdge_y=0,deepth=0,road=[[0,0,0]]):
        # self.opts['elevation'] = 30
        # self.opts['azimuth'] = 90
        # angle=kwargs['angle']
        # ssd=kwargs['ssd']
        # raySize_x=kwargs['raySize_x']
        # raySize_y=kwargs['raySize_y']
        # raySizeEdge_x=kwargs['raySizeEdge_x']
        # raySizeEdge_y=kwargs['raySizeEdge_y']
        # deepth=kwargs['deepth']
        # road=kwargs['road']
        # self.SSD = ssd*10
        self.SSD = ssd*2
        self.raySize_x = raySize_x*10
        self.raySize_y = raySize_y*10
        self.raySizeEdge_x = raySize_x*10/2+raySizeEdge_x
        self.raySizeEdge_y = raySize_y*10/2+raySizeEdge_y
        self.deepth = deepth
        self.angle = angle
        # 清空重载
        self.items = []
        self.addItem(self.gx)
        # self.addItem(self.gy)
        self.addItem(self.gz)
        self.drawWaterTank()
        self.drawBoxDirection()
        # self.drawRayDirection()
        self.drawXYZ()
        self.drawRayEdgeQuad(-self.raySizeEdge_x,-self.raySizeEdge_y,self.raySizeEdge_x,self.raySizeEdge_y,self.deepth)
        self.drawRayQuad(-self.raySize_x/2,-self.raySize_y/2,self.raySize_x/2,self.raySize_y/2,self.deepth)
        self.drawLight(-self.raySize_x/2,-self.raySize_y/2,self.raySize_x/2,self.raySize_y/2,self.deepth)
        self.drawLightPoint(self.SSD)
        self.drawRoad(road)
        # 添加移动

        # 根据角度选择相应的度数,不管旋转了多少圈，
        # 只需取出一圈360的余值进行计算
        azimuth = self.opts['azimuth']
        azimuth = azimuth%360
        err = 0
        ag = angle + 25
        if azimuth<=ag:
            err = ag-azimuth
        if ag<azimuth:
            err = -(azimuth-ag)
        self.opts['azimuth'] += err

        elevation = self.opts['elevation']
        self.opts['elevation'] -= elevation%360
        self.opts['elevation'] += 30

        self.update()

    def load_config(self, conf):
        if conf['type'] == 'OAR':
            num = re.compile('^([0-9]+).*$') #regex for num
            road = conf['road']
            ssd = float(num.match(conf['ssd']).group(1))
            raySizeX = float(num.match(conf['ray_size_x']).group(1))
            raySizeY = float(num.match(conf['ray_size_y']).group(1))
            raySizeEdgeX = float(num.match(conf['ray_size_edge_x']).group(1))
            raySizeEdgeY = float(num.match(conf['ray_size_edge_y']).group(1))
            deepth = float(num.match(conf['deepth']).group(1))
            #  angle = float(conf['angle'])
            angle = float(conf['angle']) if 'angle' in conf else 0
            self.updateItems(angle=angle,ssd=-ssd,raySize_x=raySizeX,raySize_y=raySizeX,raySizeEdge_x=raySizeEdgeX,raySizeEdge_y=raySizeEdgeY,deepth=deepth,road=road)
        if conf['type'] == 'PDD':
            num = re.compile('^([0-9]+).*$') #regex for num
            road = conf['road']
            ssd = float(num.match(conf['ssd']).group(1))
            raySizeX = float(num.match(conf['ray_size_x']).group(1))
            raySizeY = float(num.match(conf['ray_size_y']).group(1))
            deepth = float(num.match(conf['deepth']).group(1))
            #  angle = float(conf['angle'])
            angle = float(conf['angle']) if 'angle' in conf else 0
            self.updateItems(angle=angle,ssd=-ssd,raySize_x=raySizeX,raySize_y=raySizeX,road=road)
        if conf['type'] == 'SCAN':
            num = re.compile('^([0-9]+).*$') #regex for num
            road = conf['road']
            ssd = float(num.match(conf['ssd']).group(1))
            raySizeX = float(num.match(conf['ray_size_x']).group(1))
            raySizeY = float(num.match(conf['ray_size_y']).group(1))
            deepth = float(num.match(conf['deepth']).group(1))
            angle = float(conf['angle']) if 'angle' in conf else 0
            self.updateItems(angle=angle,ssd=-ssd,raySize_x=raySizeX,raySize_y=raySizeX,deepth=deepth,road=road)

    def updatePos(self, x, y, z):
        x, y, z = self.axisToGL(-x,y,z)
        self.sp_road_move.setData(pos = np.array([[x,y,z]]))
        # self.xline.setData(pos = np.array([[x,y,z],[x,self.y_min/2,z]]))
        # self.yline.setData(pos=np.array([[x,y,z],[self.x_min/2,y,z]]))
        # self.zline.setData(pos=np.array([[x,y,z],[x,y,self.z_min/2]]))

    def paintGL(self, *args, **kwds):
        GLViewWidget.paintGL(self, *args, **kwds)
        textFont = QFont()
        textFont.setPixelSize(10)
        self.qglColor(Qt.white)
        for i in range(self.x_min,self.x_max+1):
            if i%4 == 0:
                # x axis
                self.renderText(i/2, self.y_max/2+2, self.z_min/2, '%.0f'%(-i/self.x_scale), textFont)
                # y axis
                self.renderText(self.x_max/2+2, i/2, self.z_min/2, '%.0f'%(i/self.y_scale), textFont)
                # z axis
                self.renderText(self.x_min/2, self.y_max/2+1, i/2, '%.0f'%(self.z_range/2-i/self.z_scale), textFont)
        # self.renderText(0, 0, self.z_max/2+5, '扫描矩阵示意图')

        self.qglColor(Qt.blue)
        self.renderText(self.x_min/2, self.y_max/2, 11, 'd(mm)', textFont)
        self.renderText(0, self.y_max/2+3, self.z_min/2, 'x(mm)', textFont)
        self.renderText(self.x_max/2+3, 0, self.z_min/2, 'y(mm)', textFont)

        self.qglColor(Qt.red)
        textFont.setPixelSize(40)
        self.renderText(0, -10, 10, 'x', textFont)
        self.renderText(-10, 0, 10, 'y', textFont)
        self.renderText(-10, 10, 0, 'z', textFont)
        # draw direction text
        self.qglColor(Qt.yellow)
        self.renderText(0, 0, -self.z_min/2, 'O')
        if self.angle == 0:
            self.renderText(self.x_min/2, 0, self.z_min/2, 'B')
            self.renderText(self.x_max/2, 0, self.z_min/2, 'A')
            self.renderText(0, self.y_min/2, self.z_min/2, 'G')
            self.renderText(0, self.y_max/2, self.z_min/2, 'T')
        if self.angle == 90:
            self.renderText(self.x_min/2, 0, self.z_min/2, 'T')
            self.renderText(self.x_max/2, 0, self.z_min/2, 'G')
            self.renderText(0, self.y_min/2, self.z_min/2, 'B')
            self.renderText(0, self.y_max/2, self.z_min/2, 'A')
        if self.angle == 180:
            self.renderText(self.x_min/2, 0, self.z_min/2, 'A')
            self.renderText(self.x_max/2, 0, self.z_min/2, 'B')
            self.renderText(0, self.y_min/2, self.z_min/2, 'T')
            self.renderText(0, self.y_max/2, self.z_min/2, 'G')
        if self.angle == 270:
            self.renderText(self.x_min/2, 0, self.z_min/2, 'G')
            self.renderText(self.x_max/2, 0, self.z_min/2, 'T')
            self.renderText(0, self.y_min/2, self.z_min/2, 'A')
            self.renderText(0, self.y_max/2, self.z_min/2, 'B')

    def updateSurface(self, x, y, z, size):
        try:
            self.setScale(size*10*2,size*10*2,SIZE_Z)
            x = np.array(x)*self.x_scale/2
            y = np.array(y)*self.y_scale/2
            z = (self.z_range/2-np.array(z))*self.z_scale/2
            self.items = []
            self.addItem(self.gx)
            self.addItem(self.gy)
            self.addItem(self.gz)
            self.addItem(self.vLine)
            self.addItem(self.hLine)
            self.addItem(self.sp1)
            for i in range(0,len(x)):
                pts = [[a,b,c] for a,b,c in zip(x[i],y[i],z[i])]
                sp_points = gl.GLScatterPlotItem(pos=np.array(pts), size=3/len(x), color=(0.0, 1.0, 0.0, 0.5), pxMode=False)
                self.addItem(sp_points)
                plt = gl.GLLinePlotItem(pos=np.array(pts), color=pg.glColor(0,0,255), width=1, antialias=True)
                self.addItem(plt)
            z = np.array(z).T
            for i in range(0,len(x)):
                pts = [[b,a,c] for a,b,c in zip(x[i],y[i],z[i])]
                plt = gl.GLLinePlotItem(pos=np.array(pts), color=pg.glColor(0,0,255), width=1, antialias=True)
                self.sp1.setData(pos = np.array(pts))
                self.addItem(plt)
        except:
            pass

    def updateScanRange(self,size,step,direction,deepth):
        try:
            self.raySize = size
            self.scanStep = step
            self.scanPoints = []
            s = self.raySize*10
            res = int(s) % int(self.scanStep)
            # 分割x,y
            x = np.arange(-s/2,s/2+1,step)
            y = np.arange(-s/2,s/2+1,step)
            if not res == 0:
                x = np.append(x, s/2)
                y = np.append(y, s/2)
            if direction == 'A->B':
                x, y = np.meshgrid(x, y)    # x-y 平面的网格
            if direction == 'G->T':
                y, x = np.meshgrid(x, y)    # x-y 平面的网格
            z = x*0 + deepth
            self.points = [x,y,z]
            self.updateSurface(x,y,z,size)
        except:
            pass

    def TimerMove(self):
        self.timer_cnt += 1
        try:
            if self.points:
                if self.timer_cnt >= len(self.points):
                    self.timer_cnt = 0
                i = self.timer_cnt
                x,y,z = self.axisToGL(self.points[i][0],self.points[i][1],self.points[i][2])
                self.sp_move.setData(pos = np.array([[x,y,z]]))
        except:
            pass