#!/usr/bin/env python3

from collections import defaultdict
from struct import unpack
from trabant.math import vec3,quat,almosteq
from math import cos,sin

floors   = []
sector_linedefs = []
boxes = []
box_angles = {}
hole_delimitors = {}
column_count = 150
eps = 0.01
cutlimit = 50

for l in open('level.py'):
    k,v = l.split(' = ')
    # print(k, v)
    # print()
    globals()[k] = eval(v)

def verify_base():
    # print(linedefs)
    # print(vertexes)
    # print(sectors)
    # print()
    # s = (40,)
    # for v1,v2,rs,ls in linedefs:
        # if rs in s or ls in s:
            # print(v1,v2,rs,ls)
    # print()
    for v1,v2,rs,ls in linedefs:
        assert v1 >= 0 and v1 < len(vertexes) and v2 >= 0 and v2 < len(vertexes)
        assert rs >= -1 and rs < len(sectors) and ls >= -1 and ls < len(sectors)

def compress():
    global linedefs, sidedefs, sectors
    # s = list(set(sectors))
    # vd = []
    # for sidx in sidedefs:
        # sector = sectors[sidx]
        # new_idx = s.index(sector)
        # vd += [new_idx]
    # sidedefs = vd
    # sectors = s
    for i,l in enumerate(linedefs):
        v1,v2,rside,lside = l
        rsect = -1 if rside < 0 else sidedefs[rside]
        lsect = -1 if lside < 0 else sidedefs[lside]
        linedefs[i] = (v1,v2,rsect,lsect)

def create_seg_floors():
    global floors,sector_linedefs
    fs = []
    for si,sd in enumerate(sectors):
        lo,hi = sd
        lds = [(v1,v2,rs,ls) for v1,v2,rs,ls in linedefs if rs==si or ls==si]
        sector_linedefs += [lds]
        v_lds = defaultdict(list)
        for v1,v2,rs,ls in lds:
            v_lds[v1] += [(v1,v2,rs,ls)]
            v_lds[v2] += [(v1,v2,rs,ls)]
        head = min(lds, key=lambda ld: min(ld[0],ld[1]))
        ld = head
        head_vidx = min(head[0],head[1])
        vidx = max(head[0],head[1])
        v = [head_vidx]
        #d = []
        while vidx != head_vidx:
            v += [vidx]
            #d += [ld]
            adjoining = v_lds[vidx]
            ld = adjoining[0] if adjoining[0] != ld else adjoining[1]
            vidx = ld[0] if ld[0] != vidx else ld[1]
        fs += [(lo,v)]
    for s,f in enumerate(fs):
        fl = []
        z,v = f
        for vidx in v:
            x,y = vertexes[vidx]
            fl += [(x,y,z)]
        floors += [fl]
    #flip_floors()

def find_holes():
    holes = []
    for sidx,lds in enumerate(sector_linedefs):
        print(sidx, lds)
        other_sidxes = [(rs if ls==sidx else ls) for _,_,rs,ls in lds]
        sidx2 = other_sidxes[0]
        if len(set(other_sidxes)) == 1 and sidx2 >= 0:
            print('checking for hole (or pedistal):', sidx, sidx2)
            lo1,hi1 = sectors[sidx]
            lo2,hi2 = sectors[sidx2]
            if lo1 < lo2:
                print('adding hole!')
                holes += [(sidx,sidx2)]
    return holes

def create_floor_holes():
    global floors,box_angles,sector_linedefs,sectors
    sidxes = find_holes()
    print(sidxes)
    print('Adding %i holes.' % len(sidxes))
    for lo_sidx,hi_sidx in sidxes:
        lo,hi = sectors[hi_sidx]
        floor = [(v[0],v[1],lo) for v in floors[lo_sidx]]
        center = sum((vec3(*f) for f in floors[lo_sidx]), vec3()) / len(floors[lo_sidx])
        # Create convex polygons emanating from the hole.
        up = vec3(0,0,1)
        for i in range(0,len(floor)):
            f1,f2 = floor[i-1],floor[i]
            p1,p2 = vec3(*f1),vec3(*f2)
            out = (p2-p1).cross(up)
            if out*((p1+p2)/2-center) < 0:
                out = -out
            box_angles[len(floors)] = out.angle_z(vec3(0,1,0))
            hole_delimitors[len(floors)] = True
            fvs = [vec3(*f) for f in floors[hi_sidx]]
            including = [f for f,p in zip(floors[hi_sidx],fvs) if (p-p1)*out>0]
            floors += [including+[f2,f1]]    # Reverse f order!
            sector_linedefs += [sector_linedefs[lo_sidx]]
            sector_linedefs[hi_sidx] += [(0,0,len(sector_linedefs)-1,-1)]
            sectors += [sectors[lo_sidx]]
            # Drop used vertices.
            drop = including
            if len(drop) > 3:
                if i == 0:  # Exclude extremities on first split.
                    drop.remove(max([(vec3(*f),f) for f in drop], key=lambda vf:(vf[0]-p1).length2())[1])
                    drop.remove(max([(vec3(*f),f) for f in drop], key=lambda vf:(vf[0]-p2).length2())[1])
                for d in drop:
                    floors[hi_sidx].remove(d)
        if len(floors[hi_sidx]) < 3:
            assert False
            floors[hi_sidx] = floors[-1]
            floors = floors[:-1]
            box_angles[hi_sidx] = box_angles[len(floors)]
            del box_angles[len(floors)]

def _get_longest_edges(f):
    lengths,straight_lengths = [],[]
    right,fwd = vec3(1,0,0),vec3(0,1,0)
    for i in range(0,len(f)):
        f1,f2 = f[i-1],f[i]
        p1,p2 = vec3(*f1),vec3(*f2)
        v = p2-p1
        lengths += [(v.length2(), v)]
        if not v*right and not v*fwd:
            straight_lengths += [(v.length2(), v)]
    sl = sorted(straight_lengths, key=lambda e:e[0])[-1][1] if straight_lengths else None
    return sorted(lengths, key=lambda e:e[0])[-1][1], sl

def find_rotated_boxes():
    global floors,box_angles
    for i,f in enumerate(floors):
        if i in box_angles:
            continue
        longest_edge,longest_straight_edge = _get_longest_edges(f)
        if longest_edge*vec3(1,0,0) and longest_edge*vec3(0,1,0) and \
          (longest_straight_edge == None or longest_edge.length() > 1.4*longest_straight_edge.length()):
            #if abs(longest_edge.normalize()*vec3(1,0,0)) < 0.9 and abs(longest_edge.normalize()*vec3(0,1,0)) < 0.9:
            a = longest_edge.angle_z(vec3(1,0,0))
            print('Box %i is rotated %g degrees.' % (i,a*180/3.14159))
            box_angles[i] = a

def _vsplice(vs, i):
    def v(a,b):
        b = b if b <= len(vs) else b-len(vs)
        if a < 0:
                return vs[a:]+vs[:b]
        if b < a:
                return vs[a:]+vs[:b]
        return vs[a:b]
    l = len(vs)//2
    l2 = l + (len(vs)&1)
    #print('_vsplice:', i, i+l+1, i-l2, i+1)
    return v(i,i+l+1), v(i-l2, i+1)

def convexify():
    global floors,sector_linedefs
    i = 0
    while i < len(floors):
        f = floors[i]
        for j in range(0,len(f)):
            f1,f2,f3 = f[j-2],f[j-1],f[j]
            p1,p2,p3 = vec3(*f1),vec3(*f2),vec3(*f3)
            if (p2-p1).cross((p3-p2)).z < 0:
                assert len(f) > 3
                a,b = _vsplice(f, j-1)
                floors[i] = a
                floors += [b]
                sector_linedefs += [sector_linedefs[i]]
                assert len(floors[i]) >= 3 and len(floors[-1]) >= 3
                _flip(i, a)
                _flip(len(floors)-1, b)
                if i in box_angles:
                    box_angles[len(floors)-1] = box_angles[i]
                if i in hole_delimitors:
                    hole_delimitors[len(floors)-1] = True
                i -= 1
                break
        i += 1

def _flip(i, f):
        ps = [vec3(*v) for v in f]
        z = 0
        for j in range(0,len(ps)):
            p1,p2,p3 = ps[j-2],ps[j-1],ps[j]
            z += -1 if (p2-p1).cross(p3-p2).z < 0 else +1
        if z < 0:
            print('flipping floor %i' % i)
            floors[i] = list(reversed(f))

def flip_floors():
    global floors
    # Have all vertices in counter-clockwise order.
    for i,f in enumerate(floors):
        _flip(i, f)

def lowest_neighbor(fidx, m):
    for _,_,rs,ls in sector_linedefs[fidx]:
        if rs >= 0:
            lo,hi = sectors[rs]
            m = min(m,lo)
        if ls >= 0:
            lo,hi = sectors[ls]
            m = min(m,lo)
    return m

def boxify():
    global boxes
    gx,gy,gz = lambda p:p.x, lambda p:p.y, lambda p:p.z
    for i,f in enumerate(floors):
        q,iq = quat(),quat()
        if i in box_angles:
            q  =  q.rotate_z(+box_angles[i])
            iq = iq.rotate_z(-box_angles[i])
        # Rotate and calculate size.
        sps = [q*vec3(*v) for v in f]
        g = lambda f,l: l(f(sps,key=l))
        minx,miny,minz,maxx,maxy,maxz = g(min,gx), g(min,gy), g(min,gz), g(max,gx), g(max,gy), g(max,gz)
        s = maxx-minx, maxy-miny, maxz-minz
        c = iq * vec3((maxx+minx)/2, (maxy+miny)/2, (maxz+minz)/2)
        #print(c,s)
        boxes += [(iq,c,s)]

def _boxcoords(c,s,q=None):
    s1,s2,s3,s4 = -s,s.with_y(-s.y),s,s.with_x(-s.x)
    if q != None:
        s1,s2,s3,s4 = q*s1,q*s2,q*s3,q*s4
    return c+s1,c+s2,c+s3,c+s4

def _midcoords(p1,p2):
    z = (p1.z+p2.z)/2
    s,t = vec3(p1).with_z(z),vec3(p2).with_z(z)
    return s.with_x((s.x+t.x)/2), s.with_y((s.y+t.y)/2), t.with_x((s.x+t.x)/2), t.with_y((s.y+t.y)/2)

def mask_aabb_enclosed(crds, p1,p2):
    mask = 0
    for i,c in enumerate(crds):
        if c.x >= p1.x-eps and c.x <= p2.x+eps and \
           c.y >= p1.y-eps and c.y <= p2.y+eps and \
           c.z >= p1.z-eps and c.z <= p2.z+eps:
            mask |= 1<<i
    return mask

def remove_redundant_boxes():
    global boxes,box_angles,hole_delimitors
    drop = defaultdict(int)
    for i,c in enumerate(boxes):
        ciq,cc,cs = c
        c1,c2,c3,c4 = _boxcoords(vec3(*cc),(vec3(*cs)/2).with_z(0))
        c5,c6,c7,c8 = _midcoords(c1,c3)
        for j,b in enumerate(boxes):
            biq,bc,bs = b
            if i == j or drop[j] == 0xff or ciq.q[0] != 1 or biq.q[0] != 1:
                continue
            b1,_,b2,_ = _boxcoords(vec3(*bc),vec3(*bs)/2)
            mask = mask_aabb_enclosed([c1,c2,c3,c4,c5,c6,c7,c8], b1,b2)
            if mask:
                drop[i] |= mask
    for i,mask in sorted(drop.items(), reverse=True):
        if mask == 0xff:
            print('dropping box', i)
            for j,a in list(box_angles.items()):
                if j > i:
                    del box_angles[j]
                    box_angles[j-1] = a
            for j in list(hole_delimitors.keys()):
                if j > i:
                    del hole_delimitors[j]
                    hole_delimitors[j-1] = True
            del boxes[i]
            del floors[i]

def shrink_rotated_boxes():
    global boxes
    shortened_count = 0
    drop = defaultdict(int)
    for i,c in enumerate(boxes):
        ciq,cc,cs = c
        if ciq.q[0] == 1:
            continue
        #cq = ciq.inverse()
        cc,cs = vec3(*cc),vec3(*cs)
        c1,c2,c3,c4 = _boxcoords(cc,(cs/2).with_z(0), ciq)
        ## cminx,cmaxx = min([c1,c2,c3,c4], key=lambda c:c.x), max([c1,c2,c3,c4], key=lambda c:c.x)
        cminy,cmaxy = min([c1,c2,c3,c4], key=lambda c:c.y), max([c1,c2,c3,c4], key=lambda c:c.y)
        for j,b in enumerate(boxes):
            biq,bc,bs = b
            bc,bs = vec3(*bc),vec3(*bs)/2
            if biq.q[0] != 1 or not almosteq(cc.z+cs.z/2,bc.z+bs.z):
                continue
            bmin,bmax = bc-bs,bc+bs
            ## if cminx.x<bmin.x and cmaxx.x>bmax.x:
                ## bminy,bmaxy = min(bmin.y,bmax.y),max(bmin.y,bmax.y)
                ## mincloser = (abs(cminx.x-bc.x) < abs(cmaxx.x-bc.x))
                ## print('OUT HERE!', i, cminx, cmaxx, bmin, bmax, mincloser, abs(cmaxx.y-bminy), abs(cmaxx.y-bmaxy), (not mincloser and abs(cmaxx.y-bminy)<cutlimit and abs(cmaxx.y-bmaxy)<cutlimit))
                ## if mincloser and abs(cminx.y-bminy)<cutlimit and abs(cminx.y-bmaxy)<cutlimit:
                    ## d = bmin.x-cminx.x
                    ## if d > 0.1:
                        ## print('HERE MIN X!', d)
                        ## if cs.x > cs.y:
                            ## a = vec3(1,0,0).angle_z(cq*vec3(1,0,0))
                            ## v = cq*vec3(d/cos(a),0,0)
                            ## print('vector is:', v, v.length(), cq*vec3(1,0,0))
                            ## cs.x -= v.length()
                            ## cc -= v/2
                            ## print('SHOTENED 1!', i, boxes[i])
                            ## boxes[i] = ciq,tuple(cc),tuple(cs)
                            ## print('SHOTENED 2!', i, boxes[i])
                            ## shortened_count += 1
                        ## break
                ## elif not mincloser and abs(cmaxx.y-bminy)<cutlimit and abs(cmaxx.y-bmaxy)<cutlimit:
                    ## d = cmaxx.x-bmax.x
                    ## if d > 0.1:
                        ## print('HERE MAX X!', d)
                        ## if cs.x > cs.y:
                            ## a = vec3(1,0,0).angle_z(cq*vec3(1,0,0))
                            ## v = cq*vec3(d/cos(a),0,0)
                            ## print('vector is:', v, v.length(), cq*vec3(1,0,0))
                            ## cs.x -= v.length()
                            ## cc -= v/2
                            ## print('SHOTENED 3!', i, boxes[i])
                            ## boxes[i] = ciq,tuple(cc),tuple(cs)
                            ## print('SHOTENED 4!', i, boxes[i])
                            ## print('shortener was', j)
                            ## shortened_count += 1
                        ## break
            if cminy.y<bmin.y and cmaxy.y>bmax.y:
                bminx,bmaxx = min(bmin.x,bmax.x),max(bmin.x,bmax.x)
                mincloser = (abs(cminy.y-bc.y) < abs(cmaxy.y-bc.y))
                print('OUT HERE!', i, j, cminy, cmaxy, bmin, bmax, mincloser, abs(cmaxy.x-bminx), abs(cmaxy.x-bmaxx), (not mincloser and abs(cmaxy.x-bminx)<cutlimit and abs(cmaxy.x-bmaxx)<cutlimit))
                if mincloser and cminy.x-bminx>-cutlimit and cminy.x-bmaxx<cutlimit:
                    # Don't cut on the wrong side of a hole-delimitor.
                    fy1,fy2 = vec3(*floors[i][-1]),vec3(*floors[i][-2])
                    if i in hole_delimitors and ((cminy-fy1).length2()<(bmin-fy1).length2() or (cminy-fy2).length2()<(bmin-fy2).length2()):
                        print('SKIPPING %i due to hole_delimitor.' % i)
                        continue
                    print('NOT HOLE DELIMITOR:', hole_delimitors, fy1,fy2)
                    d = bmin.y-cminy.y
                    if d > 0.1:
                        print('HERE MIN Y!', d)
                        if cs.x > cs.y:
                            a = vec3(0,1,0).angle_z(ciq*vec3(1,0,0))
                            v = ciq*vec3(d/cos(a),0,0)
                            print('vector is:', v, v.length(), ciq*vec3(1,0,0), d, a, cos(a))
                            cs.x -= v.length()
                            cc += v/2
                            print('SHOTENED 1!', i, boxes[i])
                            boxes[i] = ciq,tuple(cc),tuple(cs)
                            print('SHOTENED 2!', i, boxes[i])
                            print('shortener was', j)
                            shortened_count += 1
                        break
                ## elif not mincloser and cmaxy.x-bminx>-cutlimit and cmaxy.x-bmaxx<cutlimit:
                    ## d = cmaxy.y-bmax.y
                    ## if d > 0.1:
                        ## print('HERE MAX Y!', d)
                        ## if cs.x > cs.y:
                            ## a = vec3(0,1,0).angle_z(ciq*vec3(1,0,0))
                            ## v = ciq*vec3(d/cos(a),0,0)
                            ## print('vector is:', v, v.length(), ciq*vec3(1,0,0))
                            ## cs.x -= v.length()
                            ## cc -= v/2
                            ## print('SHOTENED 3!', i, boxes[i])
                            ## boxes[i] = ciq,tuple(cc),tuple(cs)
                            ## print('SHOTENED 4!', i, boxes[i])
                            ## print('shortener was', j)
                            ## shortened_count += 1
                        ## break
    return shortened_count

def set_box_heights():
    global boxes
    gx,gy,gz = lambda p:p.x, lambda p:p.y, lambda p:p.z
    for i,b in enumerate(boxes):
        q,c,s = b
        c,s = vec3(*c),vec3(*s)
        before = minz = c.z-s.z
        minz = lowest_neighbor(i, minz)
        if minz == before:
            minz -= 32
        s.z += before-minz
        c.z -= (before-minz)/2
        boxes[i] = (q,tuple(c),tuple(s))

def output():
    strs = [('%g'%f) for q,c,s in boxes for f in list(c)+list(s)]
    qstrs = [('%g'%f) for q,c,s in boxes for f in q.q]
    scnt = defaultdict(int)
    def e(s):
        scnt[s] += 1
    [e(s) for s in strs+qstrs]
    sidx = [k for k,v in sorted(scnt.items(),key=lambda j:-j[1])]
    s2idx = {k:i for i,kv in enumerate(sorted(scnt.items(),key=lambda j:-j[1])) for k,v in [kv]}
    print(sidx)
    print(s2idx)
    for s in strs:
        #print(s, sidx[s2idx[s]])
        assert s == sidx[s2idx[s]]
    l = ''
    b = ''
    r = ''
    t = 'floor_lookup = ['
    for s in sidx:
        t += '%s,'%s
        if len(t) > column_count:
            l += t+'\n'
            t = ''
    l += t
    t = 'floor_boxes = ['
    for i in strs:
        t += '%s,'%s2idx[i]
        if len(t) > column_count:
            b += t+'\n'
            t = ''
    b += t
    t = 'box_quats = {'
    for i in range(0,len(qstrs),4):
        if qstrs[i] == '1':
            continue
        s = [('%s'%s2idx[qs]) for qs in qstrs[i:i+4]]
        s = ','.join(s)
        t += '%i:(%s),' % (i/4,s)
        if len(t) > column_count:
            r += t+'\n'
            t = ''
    r += t
    print(l+']')
    print(b+']')
    print(r+'}')

assert _vsplice([1,2,3,4], -1) == ([4,1,2],[2,3,4])
assert _vsplice([1,2,3,4], +0) == ([1,2,3],[3,4,1])
assert _vsplice([1,2,3,4], +1) == ([2,3,4],[4,1,2])
assert _vsplice([1,2,3,4], +2) == ([3,4,1],[1,2,3])
assert _vsplice([1,2,3,4,5], -1) == ([5,1,2],[2,3,4,5])
assert _vsplice([1,2,3,4,5], +0) == ([1,2,3],[3,4,5,1])
assert _vsplice([1,2,3,4,5], +1) == ([2,3,4],[4,5,1,2])
assert _vsplice([1,2,3,4,5], +2) == ([3,4,5],[5,1,2,3])
assert _vsplice([1,2,3,4,5], +3) == ([4,5,1],[1,2,3,4])

compress()
verify_base()
create_seg_floors()
create_floor_holes()
flip_floors()
convexify()
find_rotated_boxes()
#flip_floors()
boxify()
set_box_heights()
remove_redundant_boxes()
while shrink_rotated_boxes():
    pass
output()


#! /usr/bin/env python3

import rospy
from std_msgs.msg import Bool, Empty
import rospkg
import subprocess
import os

#### DEFINE NEEDED CALLBACK FUNCTIONS

def callback_vizualization_map_building(msg):

	global proc_map_run, proc_viz_run, proc_map, proc_viz, last_viz_map_value, map_destination_path

	#on rising edge
	if(msg.data and not last_viz_map_value):

		command_slam = "exec roslaunch  {0} {1}".format(slam_algorithm_launchfile_package, slam_algorithm_launchfile_name)

		command_viz = "exec roslaunch  {0} {1}".format("slam_algorithms", "launch_map_visualization.launch")

		if(not proc_viz_run):
			proc_viz = subprocess.Popen(command_viz, shell=True)
			proc_viz_run = True

			state = proc_viz.poll()
			if state is None:
				rospy.loginfo("Process is running fine")
			elif state < 0:
				rospy.loginfo("Process terminated with error")
				proc_viz_run = False
			elif state > 0:
				rospy.loginfo("Process terminated without error")

		rospy.sleep(2.0)	#give some time to start visualization

		if(not proc_map_run):
			proc_map = subprocess.Popen(command_slam, shell=True)
			proc_map_run = True

			state = proc_map.poll()
			if state is None:
				rospy.loginfo("Process is running fine")
			elif state < 0:
				rospy.loginfo("Process terminated with error")
				proc_map_run = False
			elif state > 0:
				rospy.loginfo("Process terminated without error")

	#on falling edge
	if(not msg.data and last_viz_map_value):

		if(proc_viz_run):
			proc_viz.terminate()
			rospy.sleep(2.5)	#give time to close process
			proc_viz.kill()
			proc_viz_run = False

			state = proc_viz.poll()
			if state is None:
				rospy.loginfo("process is running fine, but should been terminated!")
				proc_viz_run = True
			elif state < 0:
				rospy.loginfo("Process terminated with error")
			elif state > 0:
				rospy.loginfo("Process terminated without error")

		if(proc_map_run):

			#the following code saves the generated map
			command_save = "rosrun  map_server map_saver -f {0} map:={1} __name:=map_saver_gui".format("initial_gui_generated_map", used_map_topic)

			proc_save = subprocess.Popen(command_save, shell=True, cwd=map_destination_path)

			state = proc_save.poll()
			if state is None:
				rospy.loginfo("Process is running fine")
			elif state < 0:
				rospy.loginfo("Process terminated with error")
			elif state > 0:
				rospy.loginfo("Process terminated without error")

			rospy.sleep(1.5)	#time to save map

			proc_map.terminate()
			rospy.sleep(2.5)	#give time to close process
			proc_map.kill()
			proc_map_run = False

			state = proc_map.poll()
			if state is None:
				rospy.loginfo("process is running fine, but should been terminated!")
				proc_map_run = True
			elif state < 0:
				rospy.loginfo("Process terminated with error")
			elif state > 0:
				rospy.loginfo("Process terminated without error")

			rospy.sleep(1.5)	#again time to process

	last_viz_map_value = msg.data

def callback_stop_map_generation(msg):

	global proc_map, proc_map_run, map_destination_path

	if(proc_map_run):

		#the following code saves the generated map
		command_save = "rosrun  map_server map_saver -f {0} map:={1} __name:=map_saver_gui".format("initial_gui_generated_map", used_map_topic)

		proc_save = subprocess.Popen(command_save, shell=True, cwd=map_destination_path)

		state = proc_save.poll()
		if state is None:
			rospy.loginfo("Process is running fine")
		elif state < 0:
			rospy.loginfo("Process terminated with error")
		elif state > 0:
			rospy.loginfo("Process terminated without error")

		rospy.sleep(1.5)	#time to save map

		#now we kill the running mapping process

		proc_map.terminate()
		rospy.sleep(2.5)	#give time to close process
		proc_map.kill()
		rospy.sleep(1.0)	#give time to close process
		proc_map_run = False

		state = proc_map.poll()
		if state is None:
			rospy.loginfo("process is running fine, but should been terminated!")
			proc_map_run = True
		elif state < 0:
			rospy.loginfo("Process terminated with error")
		elif state > 0:
			rospy.loginfo("Process terminated without error")

#### DEFINE GENERAL FUNCTIONS WHICH ARE NEEDED
def directory_check_and_generate(path):
	if(not (os.path.isdir(path))):
		os.mkdir(path)
		print("Created directory ", path, " because it was not present! (Is needed for filehandling!)")

#### FOLLOWING THE MAIN CODE

if __name__ == '__main__':

	#initate node
	rospy.init_node('map_builder_start')

	#get the name of the node if started from a .launch file
	node_name = rospy.get_name()

	#get params started from a .launch file
	map_destination_package = rospy.get_param(node_name + "/map_destination_package", "navigation")
	map_destination_folder = rospy.get_param(node_name + "/map_destination_folder", "/maps/test")
	slam_algorithm_launchfile_package = rospy.get_param(node_name + "/slam_algorithm_launchfile_package", "slam_algorithms")
	slam_algorithm_launchfile_name = rospy.get_param(node_name + "/slam_algorithm_launchfile_name", "karto_slam_real_robot.launch")
	used_map_topic = rospy.get_param(node_name + "/used_map_topic", "/map_karto")

	#get full paths to directories with the help of RosPack
	rospack = rospkg.RosPack()
	global map_destination_path
	map_destination_path = rospack.get_path(map_destination_package)
	map_destination_path = (map_destination_path + map_destination_folder)

	#checking if needed directory exists, otherwise we generate it
	directory_check_and_generate(map_destination_path)

	#definde global needed variables
	global proc_viz_run		#variable to see if subprocess for visualization is running
	global proc_map_run	#variable to see if subprocess for mapping is running

	proc_viz_run = False	#visualiuation process not running
	proc_map_run = False	#mapping process not running

	global last_viz_map_value	#safe last value on to vizualise_map topic for edge detection
	last_viz_map_value = False	#set to false

	#define a subscriber to a boolean msg which tells us if we should start the mapping phase
	sub_right_loc = rospy.Subscriber("vizualise_map", Bool, callback_vizualization_map_building)

	#define a subscriber which tells us if the
	sub_right_loc = rospy.Subscriber("end_mapping", Empty, callback_stop_map_generation)

	rospy.spin()


# model-related
from django.db.models import Q
from django.utils import timezone
from django.conf import settings
from . import models as m

# functionality
import operator as op
import datetime, random, re, traceback
from autotapta.analyze.Cluster import clusterRangeVars
from autotapta.input.Template import translateCapability
from autotapta.analyze.Analyze import extractEpisodes, extractTriggerCases, extractRevertCases
from autotapta.analyze.Rank import checkIfEpisodeCovered
from autotap.util import update_trace_in_cache
from autotap.variable import update_separation_count

# st-related
from . import capmap
from . import st_util

import json
import webcolors
from celery_workers import tasks

######################################
######################################
## INDEX                            ##
## FES :: FRONTEND / SELECTORS      ##
## STV :: ST-END VIEWS              ##
## RC  :: RULE CREATION             ##
## SPC :: SAFETY PROPERTY CREATION  ##
## SLM :: STATELOG MANAGEMENT       ##
## DM  :: DEVICE MANAGEMENT         ##
######################################
######################################

################################################################################
## [FES] FRONTEND / SELECTORS
################################################################################

def get_or_make_user(code,mode):
    try:
        user = m.User_ICSE19.objects.get(code=code)
    except m.User_ICSE19.DoesNotExist:
        user = m.User_ICSE19(code=code,mode=mode)
        user.save()
    return user

### @TO_DELETE: Potentially dead code
# return id:name dict of all devices
# def all_devs(user):
#     json_resp = {}
#     for dev in m.Device.objects.filter(users__contains=user):
#         json_resp[dev.id] = dev.name
#     return json_resp

### @TO_DELETE: Potentially dead code
# def user_devs(user):
#     if isinstance(user, m.User):
#         mydevs = m.Device.objects.filter(users__contains=user)
#     else:
#         mydevs = m.Device.objects.filter(owner=user)
#     pubdevs = m.Device.objects.filter(public=True)
#     return mydevs.union(pubdevs)

# return id:name dict of all channels
def valid_chans(user,is_trigger):
    json_resp = {'chans' : []}
    # for integration of user and user_icse19
    if isinstance(user, m.User):
        chans = m.Channel.objects.filter(Q(device__users__in=[user])).distinct().order_by('name')
        devs = m.Device.objects.filter(Q(users__in=[user]))
    else:
        chans = m.Channel.objects.filter(Q(capability__device__public=True)).distinct().order_by('name')
        devs = m.Device.objects.filter(Q(public=True))
    chans = filter(lambda x : chan_has_valid_cap(x,devs,is_trigger),chans)
    for chan in chans:
        json_resp['chans'].append({'id' : chan.id, 'name' : chan.name, 'icon' : chan.icon})
    return json_resp

def dev_has_valid_cap(dev,channel,is_trigger):
    poss_caps = dev.caps.all().intersection(m.Capability.objects.filter(channels=channel))
    if is_trigger:
        return any(map(lambda x : x.readable,poss_caps))
    else:
        return any(map(lambda x : x.writeable,poss_caps))

def dev_has_valid_cap_no_channel(dev, is_trigger):
    poss_caps = dev.caps.all()
    if is_trigger:
        return any(map(lambda x : x.readable,poss_caps))
    else:
        return any(map(lambda x : x.writeable,poss_caps))

def chan_has_valid_cap(chan,devs,is_trigger):
    dev_caps = m.Capability.objects.none()
    for dev in devs:
        dev_caps = dev_caps.union(dev.caps.all())
    poss_caps = m.Capability.objects.filter(channels=chan).intersection(dev_caps)
    if is_trigger:
        return any(map(lambda x : x.readable,poss_caps))
    else:
        return any(map(lambda x : x.writeable,poss_caps))

def rwfilter_caps(caps,is_trigger):
    if is_trigger:
        return filter((lambda x : x.readable),caps)
    else:
        return filter((lambda x : x.writeable),caps)

def map_labels(caps,is_trigger,is_event):
    if is_trigger:
        if is_event:
            return list(map((lambda x : (x.id,x.name,x.eventlabel)),caps))
        else:
            return list(map((lambda x : (x.id,x.name,x.statelabel)),caps))
    else:
        return list(map((lambda x : (x.id,x.name,x.commandlabel)),caps))

def filtermap_caps(caps,is_trigger,is_event):
    return map_labels(rwfilter_caps(caps,is_trigger),is_trigger,is_event)

###############################################################################
## [STV] ST-END VIEWS
###############################################################################

def is_valid_value(param,val):
    # not case sensitive
    val = val.lower()

    if param.type == 'set':
        try:
            m.SetParamOpt.objects.get(param_id=param.id,value=val)
            return True
        except m.SetParamOpt.DoesNotExist:
            return False
    elif param.type == 'range':
        try:
            floval = float(val)
        except ValueError:
            return False
        return (param.rangeparam.min <= floval <= param.rangeparam.max)
    elif param.type == 'bin':
        # not case sensitive
        return (param.binparam.tval.lower() == val or param.binparam.fval.lower() == val)
    elif param.type == 'input':
        if param.inputparam.inputtype == 'int':
            try:
                intval = int(val)
            except ValueError:
                return False
            else:
                return True
        else:
            return True
    else:
        #this shouldn't happen
        return False

def test_cond(cond,value):
    # for solving the case sensitive problem,
    # probably there's a better way to enforce this
    processed_value = value
    processed_cond_value = cond.val
    if cond.par.type == 'range':
        processed_value = float(processed_value)
        processed_cond_value = float(processed_cond_value)

    # timer support begin   -- by Bo Wang
    if cond.par.type == 'time':
        processed_value = processed_value.replace(':', '')
        processed_cond_value = processed_cond_value.replace(':', '')
    # timer support end

    if isinstance(value, str) and isinstance(cond.val, str):
        processed_value = value.lower()
        processed_cond_value = cond.val.lower()

    if cond.comp == '=':
        return op.eq(processed_value, processed_cond_value)
    elif cond.comp == '!=':
        return op.neq(processed_value, processed_cond_value)
    elif cond.comp == '<':
        return op.lt(processed_value, processed_cond_value)
    elif cond.comp == 'lte':
        return op.lte(processed_value, processed_cond_value)
    elif cond.comp == '>':
        return op.gt(processed_value, processed_cond_value)
    elif cond.comp == 'gte':
        return op.gte(processed_value, processed_cond_value)
    else:
        return None

def test_cond_dict(cond_par, cond_val, value):
    # for solving the case sensitive problem,
    # probably there's a better way to enforce this
    processed_value = value
    processed_cond_value = cond_val['value']
    if cond_par['type'] == 'range':
        processed_value = float(processed_value)
        processed_cond_value = float(processed_cond_value)

    # timer support begin   -- by Bo Wang
    if cond_par['type'] == 'time':
        processed_value = processed_value.replace(':', '')
        processed_cond_value = processed_cond_value.replace(':', '')
    # timer support end

    if isinstance(value, str) and isinstance(cond_val['value'], str):
        processed_value = value.lower()
        processed_cond_value = cond_val['value'].lower()

    if cond_val['comparator'] == '=':
        return op.eq(processed_value, processed_cond_value)
    elif cond_val['comparator'] == '!=':
        return op.neq(processed_value, processed_cond_value)
    elif cond_val['comparator'] == '<':
        return op.lt(processed_value, processed_cond_value)
    elif cond_val['comparator'] == 'lte':
        return op.lte(processed_value, processed_cond_value)
    elif cond_val['comparator'] == '>':
        return op.gt(processed_value, processed_cond_value)
    elif cond_val['comparator'] == 'gte':
        return op.gte(processed_value, processed_cond_value)
    else:
        return None

### @TO_DELETE: Potentially dead code
# determine if all Conditions of a Trigger are currently satisfied
# def check_trigger(trigger):
#     state = m.State.objects.get(cap=trigger_cap,dev=trigger_dev)
#     for cond in m.Condition.objects.filter(trigger=trigger):
#         try:
#             pv = m.ParVal.objects.get(state=state,par=cond_par)
#             if not test_cond(cond,pv.val):
#                 return False
#         except m.ParVal.DoesNotExist:
#             return False

#     return True

def update_state(cap,dev,param,val):
    '''
    Update/create PV corresponding to state of a given cap/dev pair. \n
    The state here stands for the current state of the device. \n
    Input:
        cap:    the related capability
        dev:    the related device
        param:  the parameter that should be updated
        val:    the new value of the parameter
    Output:
        pv:     the updated/created ParVal object
    '''
    # when is_action is False, the state is the state of a device's capability
    state = get_or_make_state(cap, dev, val, is_action=False)
    # ParVal specifies that for one specific state, what param/value it currently has
    try:
        pv = m.ParVal.objects.get(state=state,par=param)
        if param.type == 'color':
            hue, saturation = pv.val.split(',')
            target_hue, target_saturation = val.split(',')
            hue = hue if target_hue == '*' else target_hue
            saturation = saturation if target_saturation == '*' else target_saturation
            val = '%s,%s' % (hue, saturation)
        pv.val = val
        pv.save()
    except m.ParVal.DoesNotExist:
        pv = m.ParVal.objects.create(state=state,par=param,val=val)

    return pv

# @TODO: Get rid of `is_current` or don't use `State` as current state of the device
def update_log(cap, dev, parvals):
    '''
    Log the new event in StateLog and inactivate the old state.\n
    Input:
        cap     : the capability to be logged
        dev     : the device to be logged
        parvals : a list of ParVal objects
    Output:
        None
    '''
    # mark old log inactive (if one exists), and create new log
    for parval in parvals:
        is_superifttt = False
        # if there is a state that is triggered by superifttt but not confirmed yet
        # if it's a to-occur state, it has to be within 2 minutes
        old_entries = m.StateLog.objects.filter(
            timestamp__gte=get_now() - datetime.timedelta(seconds=120),
            status=m.StateLog.TO_OCCUR,
            cap=cap,
            dev=dev,
            loc=dev.location,
            param=parval.par,
            value=parval.val,
            is_superifttt=True
        ).all()
        if len(old_entries) > 0:
            # if there are TO_OCCUR StateLog, then delete them all and set is_superifttt to True
            old_entries.delete()
            is_superifttt = True

        # search for the current value of the param, change all of them to HAPPENED
        entries = m.StateLog.objects.filter(
            status = m.StateLog.CURRENT,
            cap = cap,
            dev = dev,
            loc = dev.location,
            param = parval.par
        ).all()
        # set all the found entries (although there should either be only one or none) to HAPPENED
        for entry in entries:
            entry.status = m.StateLog.HAPPENED
            entry.save()

        # if the old state is inactivated or cannot find a matching statelog,
        # then create a new entry
        state_log = m.StateLog.objects.create(
            status=m.StateLog.CURRENT,
            cap=cap,
            dev=dev,
            loc=dev.location,
            param=parval.par,
            value=parval.val,
            is_superifttt=is_superifttt
        )

        update_separation_count(state_log)
        update_trace_in_cache(state_log)
        schedule_history_triggered(state_log)
    return

def stval_to_val(param: m.Parameter, st_val):
    if param.type in ('bin', 'set'):
        try:
            return m.ParValMapping.objects.get(param=param, st_val=st_val).val
        except m.ParValMapping.DoesNotExist:
            err_msg = "cannot find error mapping for %s of parameter %s" % (st_val, param.name)
            m.ErrorLog.objects.create(err=err_msg)
            raise Exception(err_msg)
    elif param.type in ('range', 'color'):
        return st_val
    else:
        err_msg = "param type %s not supported yet" % param.type
        m.ErrorLog.objects.create(err=err_msg)
        raise Exception(err_msg)

def val_to_stval(param: m.Parameter, val):
    if param.type in ('bin', 'set'):
        try:
            return m.ParValMapping.objects.get(param=param, val=val).st_val
        except m.ParValMapping.DoesNotExist:
            err_msg = "cannot find error mapping for %s of parameter %s" % (val, param.name)
            m.ErrorLog.objects.create(err=err_msg)
            raise Exception(err_msg)
    elif param.type in ('range', 'color'):
        return val
    else:
        err_msg = "param type %s not supported yet" % param.type
        m.ErrorLog.objects.create(err=err_msg)
        raise Exception(err_msg)

def check_s_triggers(s_trigger_list):
    for s_trig in s_trigger_list:
        # state = m.State.objects.get(cap=s_trig.cap, dev=s_trig.dev)
        # conditions = m.Condition.objects.filter(trigger=s_trig)
        # for cond in conditions:
        #     try:
        #         val = m.ParVal.objects.get(par=cond.par, state=state).val
        #         if not test_cond(cond, val):
        #             return False
        #     except m.ParVal.DoesNotExist:
        #         err_msg = "unable to get current state of %s" % str(state)
        #         m.ErrorLog.objects.create(err=err_msg)
        #         raise Exception(err_msg)
        check_trig_func = check_history_s_trig if check_trigger_is_history(s_trig) else check_regular_s_trig
        if not check_trig_func(s_trig):
            return False

    return True

def check_trigger_is_history(trig):
    cap = trig.cap
    # TODO: currently use hard-coding. later we should
    # add another field to indicate history trigger
    if cap.name == 'Time Since State':
        return True
    else:
        return False

def check_regular_s_trig(s_trig):
    state = m.State.objects.get(cap=s_trig.cap, dev=s_trig.dev)
    conditions = m.Condition.objects.filter(trigger=s_trig)
    for cond in conditions:
        try:
            val = m.ParVal.objects.get(par=cond.par, state=state).val
            if not test_cond(cond, val):
                return False
        except m.ParVal.DoesNotExist:
            err_msg = "unable to get current state of %s" % str(state)
            m.ErrorLog.objects.create(err=err_msg)
            raise Exception(err_msg)
    return True

def check_history_s_trig(s_trig):
    conditions = list(m.Condition.objects.filter(trigger=s_trig))  # should have a time condition and a trigger condition
    if len(conditions) != 2:
        err_msg = "Time since state should have 2 conditions!"
        raise Exception(err_msg)

    time_in_secs = None
    comparator = None
    trigger_clause = None
    for cond in conditions:
        if cond.par.type == "duration":
            time_clause = json.loads(cond.val.replace('\'', '\"'))
            time_in_secs = time_to_int(time_clause)
            comparator = cond.comp
        elif cond.par.type == "meta":
            trigger_clause = json.loads(cond.val.replace('\'', '\"'))

    trigger_parameters = trigger_clause['parameters']
    trigger_parameterVals = trigger_clause['parameterVals']
    trigger_cap_id = trigger_clause['capability']['id']
    trigger_dev_id = trigger_clause['device']['id']

    if len(trigger_parameters) != 1 or len(trigger_parameterVals) != 1:
        err_msg = "Trigger in history channel should only have one parameter"
        m.ErrorLog.objects.create(err=err_msg)
        raise Exception(err_msg)

    trigger_parameter = trigger_parameters[0]
    trigger_parameterVal = trigger_parameterVals[0]

    current_time_utc = datetime.datetime.utcnow()
    start_time = current_time_utc - datetime.timedelta(seconds=time_in_secs)

    # check current status
    state = m.State.objects.get(cap__id=trigger_cap_id, dev__id=trigger_dev_id)
    current_parval = m.ParVal.objects.get(par__id=trigger_parameter['id'], state=state)

    if not test_cond_dict(trigger_parameter, trigger_parameterVal, current_parval.val):
        return False

    # check history
    # if comp == '>', state should always be true from start_time to current_time_utc
    # if comp == '<', state should once be false from start_time to current_time_utc, but finally is true
    post_state_logs = m.StateLog.objects.filter(dev__id=trigger_clause['device']['id'],
                                                cap__id=trigger_clause['capability']['id'],
                                                timestamp__gte=start_time,
                                                status__in=(m.StateLog.CURRENT, m.StateLog.HAPPENED))

    if_state_switch = False
    for state_log in post_state_logs:
        sl_param = state_log.param
        sl_value = state_log.value
        for cond_par, cond_val in zip(trigger_parameters, trigger_parameterVals):
            if test_cond_dict(cond_par, cond_val, sl_value):
                # such thing will neglect the condition when comp == '>'
                if_state_switch = True
                break

    if if_state_switch == (comparator == '<'):
        return True
    else:
        return False

def check_if_rule_triggered(cap, dev, pvs, esrule):
    pvs_dict = dict(pvs)

    # check if Etrigger is met
    if cap != esrule.Etrigger.cap or dev != esrule.Etrigger.dev:
        return False
    conditions = m.Condition.objects.filter(trigger=esrule.Etrigger)
    for cond in conditions:
        val = pvs_dict[cond.par]
        if not test_cond(cond, val):
            return False

    # check if Striggers are met
    return check_s_triggers(esrule.Striggers.all())

def schedule_history_triggered(log_obj: m.StateLog):
    '''
    should be called by update_log, to check if history trigger activated.
    '''
    history_cap = capmap.lookup('Time Since State', 'vd')
    assert len(history_cap) == 1
    history_cap = history_cap[0]
    dev_obj = log_obj.dev
    loc_obj = dev_obj.location
    st_installed_app_id = loc_obj.st_installed_app_id
    history_esrules = m.ESRule.objects.filter(Etrigger__cap=history_cap, st_installed_app_id=st_installed_app_id)
    # log_obj .cap  .dev  .param   .value   .valuetype

    schedule_time_list = list()
    for history_rule in history_esrules:
        conds = m.Condition.objects.filter(trigger=history_rule.Etrigger)
        assert len(conds) == 2 # Etrigger should have only one cond.
        history_trigger_pvs = dict()

        for cond in conds:
            history_trigger_pvs[cond.par.name] = cond.val

        # trigger_pvs should be this now:
        # {"trigger": "...<trigger in json>...", "time": <duration value> }
        event_trigger_obj = json.loads(history_trigger_pvs['trigger'].replace('\'', '\"'))  # ugly fix of json format
        event_trigger_parameters = event_trigger_obj['parameters']
        event_trigger_parameterVals = event_trigger_obj['parameterVals']
        event_trigger_cap_id = event_trigger_obj['capability']['id']
        event_trigger_dev_id = event_trigger_obj['device']['id']
        if log_obj.cap.id != event_trigger_cap_id:
            continue # this rule is not what we want
        if log_obj.dev.id != event_trigger_dev_id:
            continue

        current_rule_match = True
        # param = log_obj.param
        value = log_obj.value
        for cond_par, cond_val in zip(event_trigger_parameters, event_trigger_parameterVals):
            if not test_cond_dict(cond_par, cond_val, value):
                current_rule_match = False
                break

        if current_rule_match:
            time_clause = json.loads(history_trigger_pvs['time'].replace('\'', '\"'))
            count_down_time = time_to_int(time_clause) # translate this into seconds
            if count_down_time not in schedule_time_list:
                schedule_time_list.append(count_down_time)

    # schedule time events
    for count_down_time in schedule_time_list:
        print("# [HISTORY] schedule_history_triggered schedule:" + str((log_obj.id, count_down_time)))
        tasks.triggering_history_event.apply_async((log_obj.id, count_down_time), countdown=count_down_time)

def st_process_update(kwargs):
    '''
    Update states and find out what action should be triggered.\n
    Input:
        - kwargs: a dict of the new event occurred
        {
            "st_installed_app_id": the id of the installed SmartThings app instance,
            "capid": capability id in this project,
            "deviceid": device id in this project,
            "values": { <st_param_id>: <parval_value>, ... }
        }
    Output:
        - json_resp: a dict of actions that should be triggered
        {
            "actions": [{
                    "capability": "<commandname>",
                    "devid": "<device proj id>",
                    "values": { "<st_param_id>": "<parval_value>", ...}
                }, ...],
            "errors": [{"msg": "<error_details>"}, ...]
        }
    Note: Only devices that are not fake will be returned
    '''
    json_resp = {"actions": [], "errors": []}

    cap = capmap.lookup(kwargs['capid'],'st')
    if len(cap) > 0:
        cap = cap[0] # currently let's assume there's only one capability...
    else:
        m.ErrorLog.objects.create(err="st_process_update > capmap: Cannot find the corresponding capability: " + kwargs['capid'])
        return json_resp
    try:
        dev = m.Device.objects.get(id=kwargs['deviceid'])
        pvdict = kwargs['values'] #{st_param_id : value}
        if kwargs['capid'] == 'colorControl':
            hue = pvdict['hue'] if 'hue' in pvdict else '*'
            saturation = pvdict['saturation'] if 'saturation' in pvdict else '*'
            color_val = '%s,%s' % (hue, saturation)
            pvdict = {'color': color_val}
        pvs = [(m.Parameter.objects.get(sysname=pv_id), stval_to_val(m.Parameter.objects.get(sysname=pv_id), pvdict[pv_id])) for pv_id in pvdict]
    except m.Device.DoesNotExist:
        err_detail = "Cannot find the device: {}.".format(kwargs['deviceid'])
        json_resp["errors"].append(get_error_msg("st_process_update", err_detail, status=404))
        return json_resp
    except m.Parameter.DoesNotExist:
        err_detail = "Cannot find a parameter that's corresponding to '{}'.".format(pv_id)
        json_resp["errors"].append(get_error_msg("st_process_update", err_detail, status=500))
        return json_resp

    for param, val in pvs:
        # update the related par-val pair (trigger)
        pv = update_state(cap,dev,param,val)
        # assume that superifttt will only be installed once per home
        update_log(cap, dev, [pv])

    # ------------------------------------------------------------------------
    # --------- below: consider using check_actions_and_get_json -------------
    st_installed_app_id = kwargs['st_installed_app_id']
    json_resp = check_actions_and_get_json(st_installed_app_id, cap, dev, pvs)
    return json_resp
    # -------- before: consider using check_actions_and_get_json -------------
    # ------------------------------------------------------------------------

# def get_state_parval(cap, dev, param) (in branch production-dev-timer. useless now.)

def vd_process_update(kwargs):
    '''
    Update states of virtual devices and find out what action should be triggered.\n
    Input:
        - kwargs: a dict of the new event occurred
        {
            "st_installed_app_id": the id of the installed SmartThings app instance,
            "capid": capability id in this project,
            "deviceid": device id in this project,
            "values": { <st_param_id>: <parval_value>, ... }
        }
    Output:
        - json_resp: a dict of actions that should be triggered
        {
            "actions": [{
                    "capability": "<commandname>",
                    "devid": "<device proj id>",
                    "values": { "<st_param_id>": "<parval_value>", ...}
                }, ...],
            "errors": [{"msg": "<error_details>"}, ...]
        }
    '''
    json_resp = {"actions": [], "errors": []}

    cap = capmap.lookup(kwargs['capid'],'vd')
    if len(cap) > 0:
        cap = cap[0] # currently let's assume there's only one capability...
    else:
        m.ErrorLog.objects.create(err="vd_process_update > capmap: Cannot find the corresponding capability." + kwargs['capid'])
        return json_resp
    try:
        dev = m.Device.objects.get(id=kwargs['deviceid'])
        pvdict = kwargs['values'] #{st_param_id : value}
        pvs = [(m.Parameter.objects.get(name=pv_id, cap=cap), pvdict[pv_id]) for pv_id in pvdict]
    except m.Device.DoesNotExist:
        err_detail = "Cannot find the device: {}.".format(kwargs['deviceid'])
        json_resp["errors"].append(get_error_msg("vd_process_update", err_detail, status=404))
        return json_resp
    except m.Parameter.DoesNotExist:
        err_detail = "Cannot find a parameter that's corresponding to '{}'.".format(pv_id)
        json_resp["errors"].append(get_error_msg("vd_process_update", err_detail, status=500))
        return json_resp

    for param, val in pvs:
        # update the related par-val pair (trigger)
        pv = update_state(cap,dev,param,val)
        # assume that superifttt will only be installed once per home
        update_log(cap, dev, [pv])

    # ------------------------------------------------------------------------
    # --------- below: consider using check_actions_and_get_json -------------
    st_installed_app_id = kwargs['st_installed_app_id']
    json_resp = check_actions_and_get_json(st_installed_app_id, cap, dev, pvs)

    if json_resp['actions']:
        st_util.execute_commands_on_core_vd(dev.location, json_resp)
    return json_resp
    # -------- before: consider using check_actions_and_get_json -------------
    # ------------------------------------------------------------------------

def vd_process_update_history(log: m.StateLog, delay_seconds: int):
    # for history events, just check if any rules are triggered. no need to update the state log/state
    device = log.dev
    location = device.location

    json_resp = check_history_actions_and_get_json(log, delay_seconds)

    if json_resp['actions']:
        st_util.execute_commands_on_core_vd(location, json_resp)
    return json_resp

def apply_esrule_action(esrule: m.ESRule):
    # trigger the action
    action = esrule.action # an action
    # exclude devices that are not real
    if action.dev.dev_type == m.Device.FAKEDEV:
        return None

    action_dict = {
        'capability' : capmap.reverse_lookup(action.cap, 'st').name,
        'devid' : action.dev.id,
        'values': {}
    }
    st_cap = capmap.reverse_lookup(action.cap, "st")

    action_pvs = {}
    # find the corresponding Par-Val pair for this action
    action_conditions = m.ActionCondition.objects.filter(action=action)
    for ac in action_conditions:
        action_pvs[ac.par.sysname] = val_to_stval(ac.par, ac.val)
        # create a new TO_OCCUR log and mark it as issued by superifttt
        m.StateLog.objects.create(
            status = m.StateLog.TO_OCCUR,
            cap = action.cap,
            dev = action.dev,
            param = ac.par,
            value = ac.val,
            is_superifttt = True
        )

    action_dict = st_util.st_action_handler[st_cap.command_type](action_pvs)
    action_dict = {**action_dict, **{"capability": st_cap.name, "devid": action.dev.id}}
    return action_dict

def check_actions_and_get_json(st_installed_app_id, cap, dev, pvs, old_pvs=None):
    '''
    add **olf_pvs**. in case that some Etrigger need last time information.
    '''
    json_resp = {"actions": [], "errors": []}
    rule_queryset = m.ESRule.objects.filter(st_installed_app_id=st_installed_app_id)
    for esrule in rule_queryset:
        if check_if_rule_triggered(cap, dev, pvs, esrule):
            # trigger the action
            action_dict = apply_esrule_action(esrule)
            if action_dict:
                json_resp['actions'].append(action_dict)

    return json_resp

def check_history_actions_and_get_json(state_log: m.StateLog, delay_seconds: int):
    # when events corresponding to history channel happens, check whether any rules triggered
    history_cap = capmap.lookup('Time Since State', 'vd')[0]  # should be only one history cap
    device = state_log.dev
    location = device.location
    st_installed_app_id = location.st_installed_app_id

    json_resp = {"actions": [], "errors": []}

    history_esrules = m.ESRule.objects.filter(Etrigger__cap=history_cap, st_installed_app_id=st_installed_app_id)
    for history_rule in history_esrules:
        conds = m.Condition.objects.filter(trigger=history_rule.Etrigger)
        assert len(conds) == 2 # Etrigger should have only one cond.
        history_trigger_pvs = dict()

        for cond in conds:
            history_trigger_pvs[cond.par.name] = cond.val

        event_trigger_obj = json.loads(history_trigger_pvs['trigger'].replace('\'', '\"'))  # ugly fix of json format
        event_trigger_time = json.loads(history_trigger_pvs['time'].replace('\'', '\"'))
        event_trigger_time = time_to_int(event_trigger_time)
        event_trigger_parameters = event_trigger_obj['parameters']
        event_trigger_parameterVals = event_trigger_obj['parameterVals']
        event_trigger_cap_id = event_trigger_obj['capability']['id']
        event_trigger_dev_id = event_trigger_obj['device']['id']

        if state_log.cap.id != event_trigger_cap_id:
            continue # this rule is not what we want
        if state_log.dev.id != event_trigger_dev_id:
            continue
        if event_trigger_time != delay_seconds:
            continue

        rule_triggered = True
        # First check if event trigger is triggered
        post_state_logs = m.StateLog.objects.filter(dev=device, cap__id=state_log.cap.id,
                                                    timestamp__gte=state_log.timestamp,
                                                    status__in=(m.StateLog.CURRENT, m.StateLog.HAPPENED))
        for post_sl in post_state_logs:
            post_sl_param = post_sl.param
            post_sl_value = post_sl.value
            for cond_par, cond_val in zip(event_trigger_parameters, event_trigger_parameterVals):
                if not test_cond_dict(cond_par, cond_val, post_sl_value):
                    rule_triggered = False
                    break

            if not rule_triggered:
                break

        # Next check if state triggers are triggered
        if rule_triggered:
             rule_triggered = check_s_triggers(history_rule.Striggers.all())

        if rule_triggered:
            # trigger the rule
            action_dict = apply_esrule_action(history_rule)
            if action_dict:
                json_resp['actions'].append(action_dict)

    return json_resp

###############################################################################
## [RC] RULE CREATION
###############################################################################

def get_or_make_state(cap, dev, value, is_action=False):
    '''
    Get the state of a cap/dev pair, or create one if none exists
    '''
    state, created = m.State.objects.get_or_create(cap=cap,dev=dev,action=is_action)
    state.text = "{}".format(value)
    state.save()
    return state

# @TO_DELETE: Potentially dead code
# def update_pv(state,par_id,val):
#     try:
#         pv = m.ParVal.objects.get(state=state,par_id=par_id)
#         pv.val = val
#         pv.save()
#     except m.ParVal.DoesNotExist:
#         pv = m.ParVal(state=state,par_id=par_id,val=val)
#         pv.save()

###############################################################################
## [SPC] SAFETY PROPERTY CREATION
###############################################################################

def time_to_int(time):
    return time['seconds'] + time['minutes']*60 + time['hours']*3600

def int_to_time(secs):
    time = {}
    time['hours'] = secs // 3600
    time['minutes'] = (secs // 60) % 60
    time['seconds'] = secs % 60
    return time

def clause_to_trigger(clause):
    if 'channel' in clause and 'id' in clause['channel']:
        t = m.Trigger(chan=m.Channel.objects.get(id=clause['channel']['id']),
                    dev=m.Device.objects.get(id=clause['device']['id']),
                    cap=m.Capability.objects.get(id=clause['capability']['id']),
                    pos=clause['id'],
                    text=clause['text'])
    else:
        t = m.Trigger(dev=m.Device.objects.get(id=clause['device']['id']),
                    cap=m.Capability.objects.get(id=clause['capability']['id']),
                    pos=clause['id'],
                    text=clause['text'])
    t.save()

    try:
        pars = clause['parameters']
        vals = clause['parameterVals']
        for par,val in zip(pars,vals):
            cond = m.Condition(par=m.Parameter.objects.get(id=par['id']),
                               val=val['value'],
                               comp=val['comparator'],
                               trigger=t)
            cond.save()
    except KeyError:
        pass

    return t

def clause_to_action(clause):
    if 'channel' in clause and 'id' in clause['channel']:
        a = m.Action(chan=m.Channel.objects.get(id=clause['channel']['id']),
                     dev=m.Device.objects.get(id=clause['device']['id']),
                     cap=m.Capability.objects.get(id=clause['capability']['id']),
                     text=clause['text'])
    else:
        a = m.Action(dev=m.Device.objects.get(id=clause['device']['id']),
                     cap=m.Capability.objects.get(id=clause['capability']['id']),
                     text=clause['text'])
    a.save()

    try:
        pars = clause['parameters']
        vals = clause['parameterVals']
        for par,val in zip(pars,vals):
            cond = m.ActionCondition(par=m.Parameter.objects.get(id=par['id']),
                                     val=val['value'],
                                     action=a)
            cond.save()
    except KeyError:
        pass

    return a

def trigger_to_clause(trigger,is_event):
    chan = {'id' : trigger.chan.id, 'name' : trigger.chan.name, 'icon' : trigger.chan.icon} if trigger.chan else {}
    c = {'channel' : chan,
         'device' : {'id' : trigger.dev.id,
                     'name' : trigger.dev.name,
                     'label': trigger.dev.label},
         'capability' : {'id' : trigger.cap.id,
                         'name' : trigger.cap.name,
                         'label' : (trigger.cap.eventlabel if is_event else trigger.cap.statelabel)},
         'text' : trigger.text,
         'id' : trigger.pos}
    conds = m.Condition.objects.filter(trigger=trigger).order_by('id')
    if conds != []:
        c['parameters'] = []
        c['parameterVals'] = []

        for cond in conds:
            if cond.par.type == 'bin':
                values = [cond.par.binparam.tval, cond.par.binparam.fval]
            else:
                values = []
            val = json.loads(cond.val.replace('\'', '\"')) if cond.par.type in ('meta', 'duration') else cond.val
            c['parameters'].append({'id' : cond.par.id,
                                    'name' : cond.par.name,
                                    'type' : cond.par.type,
                                    'values': values})
            c['parameterVals'].append({'comparator' : cond.comp,
                                       'value' : val})
    return c

def action_to_clause(action):
    chan = {'id' : action.chan.id, 'name' : action.chan.name, 'icon' : action.chan.icon} if action.chan else {}
    c = {'channel' : chan,
         'device' : {'id' : action.dev.id,
                     'name' : action.dev.name,
                     'label': action.dev.label},
         'capability' : {'id' : action.cap.id,
                         'name' : action.cap.name,
                         'label' : action.cap.commandlabel},
         'text' : action.text}
    action_conds = m.ActionCondition.objects.filter(action=action).order_by('id')
    if action_conds != []:
        c['parameters'] = []
        c['parameterVals'] = []

        for cond in action_conds:
            if cond.par.type == 'bin':
                values = [cond.par.binparam.tval, cond.par.binparam.fval]
            else:
                values = []
            val = json.loads(cond.val.replace('\'', '\"')) if cond.par.type in ('meta', 'duration') else cond.val
            c['parameters'].append({'id' : cond.par.id,
                                    'name' : cond.par.name,
                                    'type' : cond.par.type,
                                    'values': values})
            c['parameterVals'].append({'comparator' : '=',
                                       'value' : val})
    return c

def state_to_clause(state):
    chan = {'id' : state.chan.id, 'name' : state.chan.name, 'icon' : state.chan.icon} if state.chan else {}
    c = {'channel' : chan,
         'device' : {'id' : state.dev.id,
                     'name' : state.dev.name,
                     'label': state.dev.label},
         'capability' : {'id' : state.cap.id,
                         'name' : state.cap.name,
                         'label' : state.cap.commandlabel},
         'text' : state.text}

    # pvs = m.ParVal.objects.filter(state=state).order_by('id')
    # if pvs != []:
    #     c['parameters'] = []
    #     c['parameterVals'] = []
    #
    #     for pv in pvs:
    #         c['parameters'].append({'id' : pv.par.id,
    #                                 'name' : pv.par.name,
    #                                 'type' : pv.par.type})
    #         c['parameterVals'].append({'comparator' : '=',
    #                                    'value' : pv.val})
    # escapeFunc = lambda x:re.sub(r'([\.\\\+\*\?\[\^\]\$\(\)\{\}\!\<\>\|\:\-])', r'\\\1', x)
    cap = m.Capability.objects.get(id=state.cap.id)
    par_list = m.Parameter.objects.filter(cap_id=cap.id)
    par_dict = dict()
    for par in par_list:
        values = []
        par_c = dict()
        par_c['id'] = par.id
        par_c['type'] = par.type
        par_c['name'] = par.name
        if par.type == 'bin':
            bin_par = m.BinParam.objects.get(id=par.id)
            par_c['value_list'] = [bin_par.fval, bin_par.tval]
            values = [bin_par.tval, bin_par.fval]
            t_template = r'\{%s/T\|(?P<value>[\w &\-]+)\}' % par.name
            f_template = r'\{%s/F\|(?P<value>[\w &\-]+)\}' % par.name
            t_val = re.search(t_template, cap.commandlabel)
            f_val = re.search(f_template, cap.commandlabel)
            if t_val and f_val:
                par_c['value_list_in_statement'] = [f_val.group('value'), t_val.group('value')]
            else:
                par_c['value_list_in_statement'] = par_c['value_list'] = [bin_par.fval, bin_par.tval]
        elif par.type == 'range':
            par_c['value_list'] = []
        elif par.type == 'set':
            par_c['value_list'] = [opt.value for opt in m.SetParamOpt.objects.filter(param_id=par.id)]
        elif par.type == 'color':
            par_c['value_list'] = [webcolors.CSS3_NAMES_TO_HEX.keys()]
        else:
            raise Exception('var type %s not supported' % par.type)
        par_c['values'] = values

        par_dict[par.name] = par_c

    if state.dev.label is not None and state.dev.label:
        template_text = re.sub(r'\{DEVICE\}', state.dev.label, cap.commandlabel)
    else:
        template_text = re.sub(r'\{DEVICE\}', state.dev.name, cap.commandlabel)
    template_text = re.sub(r'\{(\w+)/(T|F)\|[\w &\-]+\}\{\1/(T|F)\|[\w &\-]+\}', r'(?P<\1>[\w &\-]+)', template_text)
    template_text = re.sub(r'\{(\w+)\}', r'(?P<\1>[\w &\-]+)', template_text)
    re_mat = re.match(template_text, state.text)

    for par, par_c in par_dict.items():
        par_dict[par_c['name']]['value'] = re_mat.group(par_c['name'])

    c['parameters'] = [{'type': par_c['type'], 'name': par_c['name'], 'id': par_c['id'], 'values': par_c['values']}
                       for par, par_c in sorted(par_dict.items())]
    c['parameterVals'] = list()
    for par, par_c in sorted(par_dict.items()):
        if par_c['type'] == 'bin':
            value = par_c['value_list'][par_c['value_list_in_statement'].index(par_c['value'])]
        else:
            value = par_c['value']
        c['parameterVals'].append({'comparator': '=', 'value': value})

    return c

def display_trigger(trigger):
    return {'channel' : {'icon' : trigger.chan.icon}, 'text' : trigger.text}

###############################################################################
## [SLM] STATE & LOG MANAGEMENT
###############################################################################

# @TO_DELETE: Potentially dead code
# create and save new StateLog entry in database
# def log_state(newstate):
#     pvs = m.ParVal.objects.filter(state=newstate)
#     for pv in pvs:
#         try:
#             oldlog = m.StateLog.objects.get(
#                     is_current=True,
#                     cap=newstate.cap,
#                     dev=newstate.dev,
#                     param=pv.par
#                     )
#             # if no update, continue
#             if oldlog.val == pv.val:
#                 continue

#             oldlog.is_current=False
#             oldlog.save()
#         except m.StateLog.DoesNotExist:
#             pass

#         newlog = m.StateLog(
#                 is_current=True,
#                 cap=newstate.cap,
#                 dev=newstate.dev,
#                 param=pv.par,
#                 val=pv.val
#                 )
#         newlog.save()

# get current state of dev/cap pair
def current_state(dev,cap):
    try:
        curstate = m.State.objects.get(cap=cap,dev=dev,action=False)
        return curstate
    except m.State.DoesNotExist:
        return None

# get value of state of dev/cap pair at a given time
def historical_state(dev,cap,targettime):
    state = m.State.objects.get(dev=dev,cap=cap,action=False)
    out = []
    for pv in m.ParVal.objects.filter(state=state):
        try:
            logs = m.StateLog.objects.filter(
                    status__in=[m.StateLog.HAPPENED, m.StateLog.CURRENT],
                    cap=state.cap,
                    dev=state.dev,
                    param=pv.par,
                    timestamp__lte=targettime
                )
            if logs:
                lastlog = max(logs,key=lambda log : log.timestamp)
                out.append((lastlog.param,lastlog.value))
            else:
                out.append((pv.par.id,"N/A"))
        except m.StateLog.DoesNotExist:
            out.append((pv.par.id,"N/A"))

    return out

# get record of all logged changes to a dev/cap pair
def device_cap_history(dev,cap):
    qset = m.StateLog.objects.filter(status=m.StateLog.HAPPENED,dev=dev,cap=cap)
    states = []
    for entry in sorted(qset,key=lambda entry : entry.timestamp,reverse=True):
        states.append((entry.timestamp.ctime(),entry.param.id,entry.value))
    return states

###############################################################################
## [DM] DEVICE MANAGEMENT
###############################################################################

def prep_command(action, group):
    '''
    Preparing the commands that will be sent to IoTCore

    Input:

    Output:
    '''
    if group == 'st':
        cap = action.cap
        pvs = m.ActionParVal.objects.filter(state=action)
        pars = [pv.par for pv in pvs]
        enumcommands = filter(lambda x: pv.par.is_command,pvs) #this shouldn't return >1
        if enumcommands:
            commname = enumcommands[0].par.sysname
            commvals = []
        else:
            commname = None #cap.commandname
            commvals = [{'name' : pv.par.name, 'value' : pv.val} for pv in pvs]
        return {'command'   : {'name' : commname,
                               'vals' : commvals}}
    else:
        return None

def send_command(command, group):
    if group == 'st':
        signed_command = {'signature' : 'superifttt',
                          'command' : command['command']}

def get_param_vals(param):
    if param.type == 'bin':
        param_val_list =  [param.binparam.tval, param.binparam.fval]
    elif param.type == 'set':
        options = m.SetParamOpt.objects.filter(param=param)
        param_val_list = [opt.value for opt in options]
    else:
        # TODO: should try to handle range/color
        # maybe we could use kmeans to cluster the values based on the past events?
        param_val_list = []
    return param_val_list

def get_param_vals_range(dev, cap, param):
    range_seps = m.RangeCounter.objects.filter(dev=dev, cap=cap, param=param, count__gt=0)
    return [rs.representative for rs in range_seps]

def get_param_vals_color(dev, cap, param):
    color_counts = m.ColorCounter.objects.filter(dev=dev, cap=cap, param=param, count__gt=0)
    return [cc.name for cc in color_counts]

def get_dev_commands(dev, trace, template_dict, boolean_map, orig_tap_list, plain_revert=False):
    """
    return a list of tuple (cap, param, value, count)
    """
    result = list()
    for cap in dev.caps.all():
        if cap.name == 'Color Temperature':
            continue
        if cap.writeable:
            # this capability has commands
            # should iterate through all possible commands
            params = list(m.Parameter.objects.filter(cap=cap))
            if len(params) == 1:
                param = params[0]
                if param.type in ('set', 'bin'):
                    param_val_list = get_param_vals(param)
                elif param.type == 'range':
                    param_val_list = get_param_vals_range(dev, cap, param)
                elif param.type == 'color':
                    param_val_list = get_param_vals_color(dev, cap, param)
                else:
                    param_val_list = []
                for val in param_val_list:
                    entry = {
                        'device_name': dev.name if dev.dev_type == 'v' else dev.label,
                        'capability': cap.name,
                        'attribute': param.name,
                        'current_value': val
                    }
                    dev_name, cap_name, value = translateCapability(entry, template_dict=template_dict, boolean_map=boolean_map)
                    if type(value) == int or type(value) == float:
                        value = str(value)
                    elif type(value) == bool:
                        value = 'true' if value else 'false'
                    else:
                        value = value
                    target_action = dev_name + '.' + cap_name + '=' + str(value)
                    tap_list = [tap for tap in orig_tap_list if tap.action == target_action]
                    episode_list = extractEpisodes(trace, target_action, post_time_span=datetime.timedelta(minutes=2))
                    if not plain_revert:
                        _, revert_list = extractTriggerCases(trace, target_action, tap_list=tap_list)
                    else:
                        revert_time_list = extractRevertCases(trace, target_action)
                    count = len(episode_list)
                    covered = 0
                    for episode in episode_list:
                        if checkIfEpisodeCovered(episode, tap_list, target_action):
                            covered += 1
                    reverted = sum(revert_list) if not plain_revert else len(revert_time_list)
                    result.append((cap, param, val, count, covered, reverted))
            else:
                pass

    return result

#######################
# Permission Checking #
#######################

def is_error(status_code):
    return status_code >=400

def has_access_to_location(user, loc_id):
    if user.is_superuser:
        return True
    else:
        return m.Location.objects.filter(pk=loc_id, users=user).exists()

def check_access_to_location(request, loc_id: int):
    '''
    Given a request (which contains the user) and a location id, check:
        1. if the user is logged in
        2. if the location exists
        3. if the user has access to the location
    Input:
        request: Django's request object
        loc_id:  the id of the location、
    Output:
        err_json: (dict) {"err": "xxxxx"}, defaults to {}, which means no error detected
        status:   (int) the status number, defaults to 200, which means no error detected
    '''
    err_msg = {}
    status = 200
    if not request.user.is_authenticated:
        err_msg["err"] = "Please log in!"
        status = 401
    if not m.Location.objects.filter(pk=loc_id).exists():
        err_msg["err"] = "The requested location doesn't exist."
        status = 404
    if not has_access_to_location(request.user, loc_id):
        err_msg["err"] = "You don't have access to this location."
        status = 403

    return err_msg, status

############
# Recorder #
############
def record_request(req_dict, typ, location):
    req_str = json.dumps(req_dict)
    return m.Record.objects.create(request=req_str, response="", typ=typ, location=location)

def record_response(res_dict, typ, location):
    res_str = json.dumps(res_dict)
    m.Record.objects.create(request="", response=res_str, typ=typ, location=location)

def get_location_from_user(user):
    locations = m.Location.objects.all()
    for loc in locations:
        if user in list(loc.users.all()):
            return loc
    return None

##########
# Helper #
##########

def get_error_msg(func_name, err, status=500):
    err_msg = "{}: {}\n{}".format(func_name, err, traceback.format_exc())
    m.ErrorLog.objects.create(err=err_msg)
    return {"msg": "{}".format(err)}

def get_now():
    return timezone.now() if settings.USE_TZ else datetime.datetime.now()


from pyglet.gl import *
import pyglet
from pyglet import app, graphics
from pyglet.window import Window, key
import numpy as np
from pyglet import clock
import math

#dx, dy = 25, 15 # Размеры прямоугольника
#dx2 = dx / 2
#dy2 = dy / 2
size = 20
a = int(size/2) #ширина
wx, wy = 1.5 * size, 1.5 * size # Параметры области визуализации
w, h = int(20 * wx), int(20 * wy) # Размеры окна вывода
#----------------------------------------------------
window = Window(visible = True, width = w, height = h,
                resizable = True, caption = 'А-05-19 Лабораторная работа № 4 Абросимова')
window.set_location(0, 50)
glClearColor(0.15, 0.15, 0.15, 1.0)
glClear(GL_COLOR_BUFFER_BIT)
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL)
#----------------------------------------------------

angle = -3*math.pi/4
alfa = math.pi/4

tc = 1 # Число повторов текстуры
t_coords = ((0, 0), (0, tc), (tc, tc), (tc, 0)) # Координаты текстуры
n_rot = 0
color = 0

def texInit():
    r = GL_RGB
    p = GL_TEXTURE_2D
    vp = np.full((w, w, 3), 0, dtype='uint8')
    k = w // 4
    vp[:w, :k, 0] = 255
    vp[:w, k:2*k, :] = 255
    vp[:w, 2*k:3 * k, 1] = 255

    vp = vp.flatten()
    vp = (GLubyte * (w * w * 3))(*vp)

    gl.glTexParameterf(p, GL_TEXTURE_WRAP_S, GL_REPEAT)
    gl.glTexParameterf(p, GL_TEXTURE_WRAP_T, GL_REPEAT)
    gl.glTexParameterf(p, GL_TEXTURE_MAG_FILTER, GL_LINEAR)
    gl.glTexParameterf(p, GL_TEXTURE_MIN_FILTER, GL_LINEAR)
    gl.glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_DECAL)
    gl.glTexImage2D(p, 0, r, w, w, 0, r, GL_UNSIGNED_BYTE, vp)
    gl.glEnable(p)

param1 = 1
param2 = 1

def update(dt):
    global n_rot, color, tc, param1, param2
    if n_rot >= 360:
        n_rot = 0
    if tc > 4 or tc < 1:
        param1 *= -1
    tc += param1 * 0.1
    n_rot += 1
    if color < 0 or color > 1:
        param2 *=-1
    color += param2 * 0.009

clock.schedule_interval(update, 0.01)
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glEnable(GL_DEPTH_TEST)
texInit()

@window.event
def on_draw():
    global n_rot, angle, alfa, color
    window.clear()

    glEnable(GL_CULL_FACE)
    glCullFace(GL_BACK)
    glMatrixMode(GL_PROJECTION)
    glLoadIdentity()
    glRotatef(angle * 180 / math.pi, 1, 0, 0)
    glRotatef(-alfa * 180 / math.pi, 0, 1, 0)
    glRotatef(angle, 0, 1, 0)
    glOrtho(-wx, wx, -wx, wx, -wx, wx)

    glMatrixMode(gl.GL_MODELVIEW)
    glLoadIdentity()
    glRotatef(n_rot, 0, 1, 0)

    glDisable(GL_TEXTURE_2D)

    glBegin(GL_QUADS)
    glColor3f(color, 0, 1-color)

    glVertex3f(0, -a, 0)
    glVertex3f(-a, -a, -a)
    glVertex3f(-a, a, -a)
    glVertex3f(0, a, 0)

    glVertex3f(a, -a, -a)
    glVertex3f(0, -a, 0)
    glVertex3f(0, a, 0)
    glVertex3f(a, a, -a)
    glEnd()
    glEnable(GL_TEXTURE_2D)  # Включение текстуры

    glBegin(GL_QUADS)
    glTexCoord2f(0, 0)
    glVertex3f(-a, -a, -a)
    glTexCoord2f(tc * 0.75, 0)
    glVertex3f(0, -a, 0)
    glTexCoord2f(tc * 0.75, tc)
    glVertex3f(0, a, 0)
    glTexCoord2f(0, tc)
    glVertex3f(-a, a, -a)

    glTexCoord2f(0, 0)
    glVertex3f(0, -a, 0)
    glTexCoord2f(tc * 0.75, 0)
    glVertex3f(a, -a, -a)
    glTexCoord2f(tc * 0.75, tc)
    glVertex3f(a, a, -a)
    glTexCoord2f(0, tc)
    glVertex3f(0, a, 0)
    glEnd()

app.run()


import random
import numpy as np
from DataLoader import TestDataSet
from testings.DataTypes import INT, FLOAT, DataSlice, DataLoader, Ndarray, Resolution, ERR_GROUP
from testings.UtilsFuncs import calib_np_to_phi, map_range
from typing import Optional, Callable, Tuple
import logging
FORMAT = '[%(asctime)s [%(name)s][%(levelname)s]: %(message)s'
logging.basicConfig(format=FORMAT, datefmt='%Y-%m-%d %H:%M:%S')
LOG = logging.getLogger('UtilsClass')

# Decorator for checking data is loaded
def _checkDataLoaded(f: Callable) -> Callable:
    def checkDataWrapper(self, *args, **kwargs):
        if self.dataset is None:
            LOG.error("Dataset is not loaded. Use loadData() to load data first!")
            raise AssertionError("Dataset is not loaded. Use loadData() to load data first!")
        # call normal function
        return f(self, *args, **kwargs)

    return checkDataWrapper

# Class Tester
class Loader(object):

    def __init__(self):
        self.dataset: Optional[TestDataSet] = None
        self.currentSlice: DataSlice = DataSlice()
        self.currentLoader: Optional[DataLoader] = None
        self.currentLoaderIndex: int = 0
        self.pointer: int = 0
        self.min_idx: int = 0
        self.max_idx: int = 0
        return

    def loadData(self, mode: str = "test"):
        """Load dataset"""
        # cfg = {'iter': 1000, 'show': True, 'log_interval': 10, 'preview_interval': 200, 'train_samples': 10}
        # 'save_root': "/home/shanwu/Projects/autocalibration_project/results/2039_21102020/",}
        self.dataset = TestDataSet(mode=mode, err_dist="uniform", err_config=ERR_GROUP[4], crop=False)
        self.max_idx = len(self.dataset)
        LOG.debug("Data loaded.")
        return

    @_checkDataLoaded
    def getSlice(self, idx: Optional[int] = None) -> DataSlice:
        """
        Get a sample from the dataset, dataset must be loaded
        :param idx: int, optional, index of the sample
        :return: A DataSlice sample
        """
        if idx is None or not (idx is not None and 0 <= idx < self.max_idx):
            LOG.warning("Random idx generated in getSlice().")
            idx = random.randint(self.min_idx, self.max_idx)

        # set pointer
        self.pointer = idx

        # load data
        d_stereo, p_lidar, T, P0, rot, trans, shape, cam2 = self.dataset[idx]
        p_lidar = p_lidar[p_lidar[:, 3] > 0, :]
        p_lidar[:, 3] = 1
        self.currentLoader = self.dataset.current_loader
        self.currentLoaderIndex = self.dataset.current_pointer

        # structured data
        rot, trans = rot.numpy(), trans.numpy()
        self.currentSlice = DataSlice(d_stereo, p_lidar, T, P0, rot, trans, shape, cam2)
        return self.currentSlice

    def __len__(self):
        return len(self.dataset)

class CameraInfo(object):
    def __init__(self, projection_matrix: Ndarray, resolution: Optional[Resolution] = None):
        if not self.validation_check(projection_matrix):
            raise ValueError("Input is not a projection matrix.")
        # Projection/camera matrix (for stereo camera system, KITTI has four)
        #     [fx'  0  cx' Tx]
        # P = [ 0  fy' cy' Ty]
        #     [ 0   0   1   0]
        self.P: Ndarray = projection_matrix
        self.fx: FLOAT = projection_matrix[0, 0]
        self.fy: FLOAT = projection_matrix[1, 1]
        self.cx: FLOAT = projection_matrix[0, 2]
        self.cy: FLOAT = projection_matrix[1, 2]
        self.tx: FLOAT = projection_matrix[0, 3]
        self.ty: FLOAT = projection_matrix[1, 3]
        self.Pinv: Ndarray = self.calc_inverse_projection_matrix(projection_matrix)
        self.resolution: Optional[Resolution] = resolution
        return

    @staticmethod
    def calc_inverse_projection_matrix(projection_matrix: Ndarray) -> Optional[Ndarray]:
        if projection_matrix.shape == (3, 4):
            return np.linalg.inv(np.vstack([projection_matrix,
                                            np.array([0, 0, 0, 1], dtype=np.float32)[np.newaxis, :]]))
        elif projection_matrix.shape == (4, 4):
            return np.linalg.inv(projection_matrix)
        else:
            LOG.error("Projection matrix must be a shape of (3, 4) or (4, 4)!")
            return

    @staticmethod
    def validation_check(projection_matrix) -> bool:
        if projection_matrix.shape == (3, 4):
            if not np.all(projection_matrix[2, :3] == np.array([0, 0, 1])):
                LOG.error("Input is not a projection matrix.")
                return False
            else:
                return True
        elif projection_matrix.shape == (4, 4):
            if not np.all(projection_matrix[3] == np.array([0, 0, 0, 1])):
                LOG.error("Input is not a projection matrix.")
                return False
            else:
                return True
        else:
            LOG.error("Projection matrix must be a shape of (3, 4) or (4, 4)!")
            return False

class Projector(object):

    def __init__(self, cam: CameraInfo):
        self.cam: CameraInfo = cam
        return

    @staticmethod
    def transform(pc: Ndarray, transform: Ndarray) -> Ndarray:
        return (transform @ pc.T).T

    def convert_to_uvd(self, pc: Ndarray, transform: Optional[Ndarray] = None, min_dist: FLOAT = 2.0,
                       return_raw_3d: bool = False) -> Tuple[Ndarray, Optional[Ndarray]]:
        """
        Convert point cloud to uvd pixels
        :param pc: shape (N, 4), columns are (x, y, z, 1)
        :param transform: shape (4, 4), optional, transformation from lidar frame to camera frame, change origin
        :param min_dist: float, minimum valid distance for lidar depth
        :param return_raw_3d: bool, if True, return corresponding raw point cloud along with the uvd pixels.
        :return: Tuple[uvd, Optional[raw3d]]
        """
        # transform to camera's coordinate system
        if transform is not None:
            pc = self.transform(pc, transform)

        # extract the depth
        # (x axis in Lidar's frame, but z axis in reference camera's frame after transformation)
        depths = pc[:, 2]

        # project camera intrinsic matrix and normalize on third dimension to get coordinates
        pts2d_cam = self.cam.P @ pc.T
        pts2d = pts2d_cam / pts2d_cam[2, :]

        # sift points and only keep points in the camera view and keep 1 pixel margin
        mask = np.ones(depths.shape[0], dtype=np.bool)
        mask = np.logical_and(mask, depths > min_dist)
        if self.cam.resolution is not None:  # sift points based on camera's view
            resolution = self.cam.resolution
            xmin, ymin, xmax, ymax = 0, 0, resolution[1], resolution[0]
            mask = np.logical_and(mask, pts2d[0, :] > xmin + 1)
            mask = np.logical_and(mask, pts2d[0, :] < xmax - 1)
            mask = np.logical_and(mask, pts2d[1, :] > ymin + 1)
            mask = np.logical_and(mask, pts2d[1, :] < ymax - 1)
        pts2d = pts2d[:, mask]
        depths = depths[mask]

        # transpose the coordinates back to normal shape (that I prefer) and convert to int (for slicing)
        pts2d = pts2d.T[:, :2]
        pts2d = pts2d.round()

        # if return raw corresponding 3D points
        if return_raw_3d:
            # return (u, v, 1, 1/d), (x, y, z)
            return np.concatenate([pts2d, np.ones(pts2d.shape[0], dtype=np.float32)[:, np.newaxis],
                                   1 / depths[:, np.newaxis]], axis=1), \
                   pc[mask, :]

        # return (u, v, 1, 1/d), None
        return np.concatenate([pts2d, np.ones(pts2d.shape[0], dtype=np.float32)[:, np.newaxis],
                               1 / depths[:, np.newaxis]], axis=1), \
               None

    def convert_from_uvd(self, uvd: Ndarray) -> Ndarray:
        """
        Convert depth pixels to point cloud
        :param uvd: shape (N, 4), columns are u, v, 1, 1/z, where u=x direction idx, v=y direction idx, z=depth
        :return: Numpy point cloud, shape (N, 4)
        """
        # the third column is 1
        assert np.all(uvd[:, 2]), LOG.error("The format of an uvd point is (u, v, 1, 1/z)!")
        # shape (N, 4)
        assert uvd.shape[1] == 4, LOG.error("The shape of an uvd set must be (N, 4)!")
        pts3d = self.cam.Pinv @ uvd.T
        return (pts3d / pts3d[3, :]).T

    def test_conversion(self, pc: Ndarray, transform: Ndarray) -> FLOAT:
        """Testing the conversion from point cloud to and from depth map"""
        print("Testing the conversion from point cloud -> 2D depth pixel -> point cloud (all in camera frame)")
        uvd, pts3d = self.convert_to_uvd(pc, transform, return_raw_3d=True)
        pts3d_new = self.convert_from_uvd(uvd)
        error = np.sum(pts3d-pts3d_new, axis=1).mean()
        print("Conversion error is {:6.4f}".format(error))
        return error

    def depth_map_to_3d(self, depth_map: Ndarray) -> Ndarray:
        # formatting data to uvd
        h, w = np.where(depth_map != 0)
        # (u, v, 1, 1/d)
        uvd = np.concatenate([w[np.newaxis, :],
                              h[np.newaxis, :],
                              np.ones(w.shape[0])[np.newaxis, :],
                              1/depth_map[h, w][np.newaxis, :]]).T

        # call convert_from_uvd()
        return self.convert_from_uvd(uvd)

    def convert_to_depth_map(self, pc: Ndarray, transform: Optional[Ndarray] = None,
                             min_dist: FLOAT = 2.0, base: Optional[Ndarray] = None,
                             color_range: bool = True) -> Ndarray:
        uvd, _ = self.convert_to_uvd(pc, transform=transform, min_dist=min_dist)
        uv = uvd[:, :2].round().astype(np.int32)
        d = 1 / uvd[:, 3]

        if color_range:
            # available colors (RGB)
            colors = np.zeros((1024, 3), dtype=np.uint8)
            colors[:256, 0] = 255
            colors[:256, 1] = np.arange(256)
            colors[256:512, 0] = np.arange(255, -1, -1)
            colors[256:512, 1] = 255
            colors[512:768, 1] = 255
            colors[512:768, 2] = np.arange(256)
            colors[768:1024, 1] = np.arange(255, -1, -1)
            colors[768:1024, 2] = 255
            assert d.min() >= min_dist
            d = colors[np.floor(map_range(d, min_dist, np.max(d), 0, 1023)).astype(np.int32)]

        if base is None:
            base = np.zeros(tuple(self.cam.resolution)+(3,), dtype=np.uint8)
        else:
            base = base.copy()

        base[uv[:, 1], uv[:, 0]] = d

        return base

class Evaluator(object):

    def __init__(self):
        self.running_loss: FLOAT = 0
        self.n_samples_logged: INT = 0
        self.src_calib_matrix: Optional[Ndarray] = None

    def add_source_calib_matrix(self, source: Ndarray) -> None:
        assert source.shape == (4, 4), "The size of calibration matrix must be a shape of (4, 4)."
        self.src_calib_matrix = source
        return

    def compare_calib_matrix(self, target: Ndarray, source: Optional[Ndarray] = None,
                             show: bool = True) -> Tuple[Ndarray, Ndarray]:
        assert target.shape == (4, 4), "The size of calibration matrix must be a shape of (4, 4)."
        if self.src_calib_matrix is None and source is None:
            LOG.error("Source calibration matrix is not loaded.")
            raise ValueError("Source calibration matrix is not loaded.")
        elif source is None:
            source = self.src_calib_matrix

        # convert to vector: roll, pitch, yaw, x, y, z
        calib_gt = calib_np_to_phi(source)
        rot_gt = np.rad2deg(calib_gt[:3])
        trans_gt = calib_gt[3:]
        calib_op = calib_np_to_phi(target)
        rot_op = np.rad2deg(calib_op[:3])
        trans_op = calib_op[3:]

        # get absolute differences
        diff_rot = rot_gt - rot_op
        diff_trans = trans_gt - trans_op

        if show:
            print("Source calibration (GT):")
            print(source)
            print("Target calibration (assessed):")
            print(target)
            diagnose_str = "The rotation errors are: {r}\nThe translation errors are: {t}"
            print(diagnose_str.format(r=diff_rot, t=diff_trans))
        return diff_rot, diff_trans

    def calib_matrix_to_vector(self, source: Ndarray) -> Ndarray:
        assert source.shape == (4, 4), "The size of calibration matrix must be a shape of (4, 4)."
        return calib_np_to_phi(source)

if __name__ == "__main__":
    pass


from django.contrib import admin
from .models import Profile

# Register your models here.
class ProfileAdmin(admin.ModelAdmin):
    list_display = [
        'pk',
        'user',
        'phone',
        'country',
        'city',
        'аddress_1',
        'postcode',
        'other',]

admin.site.register(Profile, ProfileAdmin)


from copy import deepcopy
from torch import nn
from models.layer.residual_connection_layer import ResidualConnectionLayer

class DecoderBlock(nn.Module):
    def __init__(self, self_attention, cross_attention, position_ff, norm, dropout=0) -> None:
        super().__init__()
        self.self_attention = self_attention
        self.cross_attention = cross_attention
        self.position_ff = position_ff
        self.residuals = [ResidualConnectionLayer(deepcopy(norm), dropout) for _ in range(3)]

    def forward(self, tgt, encoder_out, tgt_mask, src_tgt_mask):
        out = tgt
        out = self.residuals[0](out, lambda out: self.self_attention(query=out, key=out, value=out, mask=tgt_mask))
        out = self.residuals[1](out, lambda out: self.self_attention(query=out, key=encoder_out, value=encoder_out, mask=src_tgt_mask))
        out = self.residuals[2](out, self.position_ff)
        return out