#!/usr/bin/python3
# -*- coding: utf-8 -*-

"""

Maxwell Macros
v0.7.0

by Sam Neurohack
from /team/laser

Launchpad set a "current path"

"""

from OSC3 import OSCServer, OSCClient, OSCMessage
import time
import numpy as np
import rtmidi
from rtmidi.midiutil import open_midiinput
from threading import Thread
from rtmidi.midiconstants import (CHANNEL_PRESSURE, CONTROLLER_CHANGE, NOTE_ON, NOTE_OFF,
                                  PITCH_BEND, POLY_PRESSURE, PROGRAM_CHANGE)

import os, json, sys, random
from datetime import datetime, timedelta
import midi3, gstt, beatstep, launchpad, bhoreal, LPD8, C4, bcr, obs

#import tkinter.filedialog
#import easygui

if os.uname()[1]=='raspberrypi':
    pass

#ip = "127.0.0.1"
mididest = 'Session 1'
djdest = 'Port'

midichannel = 1
lastcc = 0

computer = 0

current = {
    "prefixLeft": "/osc/left/X",
    "prefixRight": "/osc/right/X",
    "suffix": "/amp",
    "path": "/scaler/amt",
    "pathLeft": "/osc/left/X/curvetype",
    "pathRight": "/osc/left/X/curvetype",
    "previousmacro": -1,
    "LeftCurveType":  0,
    "lfo": 1,
    "rotator": 1,
    "translator": 1
    }

specificvalues = {

    # Sine: 0-32, Tri: 33-64, Square: 65-96, Line: 96-127
    "curvetype": {"sin": 0, "saw": 33, "squ": 95, "lin": 127},
    "freqlimit": {"1": 0, "4": 26, "16": 52, "32": 80, "127": 127},
    "amptype": {"constant": 0, "lfo1": 33, "lfo2": 95, "lfo3": 127},
    "phasemodtype": {"linear": 0,"sin": 90},
    "phaseoffsettype": {"manual": 0, "lfo1": 33, "lfo2": 95, "lfo3": 127},
    "ampoffsettype": { "manual": 0, "lfo1": 33, "lfo2": 95, "lfo3": 127},
    "inversion": {"off": 0, "on": 127},
    "colortype": {"solid": 0, "lfo": 127},
    "modtype": {"sin": 0,"linear": 127},
    "switch": {"off": 0,"on": 127},
    "operation": {"+": 0, "-": 50, "*": 127},
    "mode": {"solid": 0,"blank": 64,"lfo": 127}
    }

shortnames = {
    "/osc/left/X" : "L X",
    "/osc/left/Y" : "L Y",
    "/osc/left/Z" : "L Z",
    "/osc/right/X" : "R X",
    "/osc/right/Y" : "R Y",
    "/osc/right/Z" : "R Z",
    "/duplicator" : "Dupli",
    "/rotator/X" : "Ro X",
    "/rotator/Y" : "Ro Y",
    "/rotator/Z" : "Ro Z",
    "/rotator/X/lfo" : "Ro X",
    "/rotator/Y/lfo" : "Ro Y",
    "/rotator/Z/lfo" : "Ro Z",
    "/translator/X" : "Tr X",
    "/translator/Y" : "Tr Y",
    "/translator/Z" : "Tr Z",
    "/translator/X/lfo" : "Tr X",
    "/translator/Y/lfo" : "Tr Y",
    "/translator/Z/lfo" : "Tr Z",
    "/color" : "color",
    "/scaler" : ""
    }

# OSC Generic client
def SendOSC(ip, port, oscaddress, oscargs=''):

    oscmsg = OSCMessage()
    oscmsg.setAddress(oscaddress)
    oscmsg.append(oscargs)

    osclient = OSCClient()
    osclient.connect((ip, port))

    if gstt.debug == True:
        print("maxwellccs sending OSC message : ", oscmsg, "to", ip, ":", port)

    try:
        osclient.sendto(oscmsg, (ip, port))
        oscmsg.clearData()
        return True
    except:
        print ('Connection to', ip, 'refused : died ?')
        return False

def AllStatus(message):
    SendOSCUI('/pad/status', [message])
    SendOSCUI('/bhoreal/status', [message])
    SendOSCUI('/c4/status', [message])
    SendOSCUI('/lpd8/status', [message])
    SendOSCUI('/bcr/status', [message])
    SendOSCUI('/beatstep/status', [message])
    SendOSCUI('/status', [message])

def SendOSCUI(address, args):
    if gstt.debug >0:
        print("SendOSCUI is sending", address, args)
    SendOSC(gstt.TouchOSCIP, gstt.TouchOSCPort, address, [args])

#
# Maxwell CCs
#

def FindCC(FunctionName):

    for Maxfunction in range(len(maxwell['ccs'])):
        #print("testing", FunctionName, "vs", Maxfunction,":",maxwell['ccs'][Maxfunction]['Function'])
        if FunctionName == maxwell['ccs'][Maxfunction]['Function']:
            if gstt.debug == True:
                print(FunctionName, "is CC", Maxfunction)
            return Maxfunction

def LoadCC():
    global maxwell

    print("Maxwell CCs Functions...")

    if os.path.exists('maxwell.json'):
        #print('File maxwell.json exits')
        f=open("maxwell.json","r")

    else:
        if os.path.exists('../maxwell.json'):
            #print('File ../maxwell.json exits')
            f=open("../maxwell.json","r")

    s = f.read()
    maxwell = json.loads(s)
    #print(len(maxwell['ccs']),"Functions")
    #print("Loaded.")

# /cc cc number value
def cc(ccnumber, value, dest=mididest, midichannel =  gstt.basemidichannel):
    global lastcc

    UpdateCCs(ccnumber, value, laser = gstt.lasernumber)

    if ccnumber > 127:
        midichannel = gstt.basemidichannel + 1
        ccnumber -= 127
    #else:
    #    UpdateCCs(ccnumber, value, laser = gstt.lasernumber)

    # mixer change display in OSC UI
    if ccnumber ==90:
        SendOSCUI('/mixer/value', [value])

    gstt.ccs[gstt.lasernumber][ccnumber + 127*(midichannel-1)] = value
    lastcc = ccnumber

    if gstt.debug == True:
        print('Maxwellccs sending CC',[CONTROLLER_CHANGE+midichannel-1, ccnumber, value], dest)

    # Local Laser
    if gstt.lasernumber == 0:

        midi3.MidiMsg([CONTROLLER_CHANGE+midichannel-1, ccnumber, value], dest)
        # update CCs TouchOSC screen
        SendOSCUI('/cc/'+str(ccnumber),[value])

    else:
        SendOSC(gstt.computerIP[gstt.lasernumber], gstt.MaxwellatorPort, '/cc/'+str(ccnumber),[value])

    #SendOSCUI('/cc/'+str(ccnumber), [value])

def UpdateCCs(ccnumber, value, laser = 0):

    #print("updateCCs", ccnumber,"value", value, "laser", laser)
    bhoreal.UpdateCC(ccnumber, value, laser)
    beatstep.UpdateCC(ccnumber, value, laser)
    LPD8.UpdateCC(ccnumber, value, laser)
    launchpad.UpdateCC(ccnumber, value, laser)
    C4.UpdateCC(ccnumber, value, laser)
    bcr.UpdateCC(ccnumber, value, laser)

# increase/decrease a CC. Value can be positive or negative
def changeCC(value, path):

    MaxwellCC = FindCC(path)

    print(MaxwellCC, "CC :", FindCC(path),"was", gstt.ccs[gstt.lasernumber][MaxwellCC])
    if gstt.ccs[gstt.lasernumber][MaxwellCC] + value > 127:
        gstt.ccs[gstt.lasernumber][MaxwellCC] = 127
    if gstt.ccs[gstt.lasernumber][MaxwellCC] + value < 0:
        gstt.ccs[gstt.lasernumber][MaxwellCC] = 0
    if gstt.ccs[gstt.lasernumber][MaxwellCC] + value < 127 and gstt.ccs[gstt.lasernumber][MaxwellCC] + value >0:
        gstt.ccs[gstt.lasernumber][MaxwellCC] += value

    print("maxwellccs changeCC in maxwellccs : path =", path, "CC :", FindCC(path), "is now ", gstt.ccs[gstt.lasernumber][MaxwellCC], "for laser", gstt.lasernumber)
    cc(MaxwellCC, gstt.ccs[gstt.lasernumber][MaxwellCC] , dest ='to Maxwell 1')

def ResetCCs():

    print("Reset all requested : sending default CCs...")
    for Maxfunction in range(len(maxwell['ccs'])):

        path = maxwell['ccs'][Maxfunction]['Function']
        init = maxwell['ccs'][Maxfunction]['init']

        funcpath = path.split("/")
        func = funcpath[len(funcpath)-1]

        if func in specificvalues:
            value = specificvalues[func][init]
        else:
            value  = int(init)

        sys.stdout.write('CC : ' + str(Maxfunction) + ' value : ' + str(value) +'  \r')
        sys.stdout.flush()

        if gstt.debug == True:
            print ("RESETING", path, ": Maxfunction", Maxfunction, "with", value, " funcpath :",  funcpath, "func :",func, "init", init)

        cc(Maxfunction, value, dest="to Maxwell 1")
        time.sleep(0.01)

def SendCC(path, init):

    funcpath = path.split("/")
    func = funcpath[len(funcpath)-1]
    #Òprint(" funcpath :",  funcpath, "func :",func, "init", init)
    if func in specificvalues:
        value = specificvalues[func][init]
    else:
        value  = int(init)
    if gstt.debug == True:
        print("sending CC", FindCC(path), "with value", value)
    cc(FindCC(path),value, dest="to Maxwell 1")
    time.sleep(0.005)

# Reset one CC Mode

def resetCC():

    gstt.resetCC = (not gstt.resetCC)

    if gstt.resetCC == True:
        print("CC Reseting mode ON")
        SendOSCUI('/beatstep/' + beatstep.findMatrix("maxwellccs.resetCC", 'HueInt') + '/led', 1.0)
        AllStatus("CC resets ON")

    else:
        print("CC Reseting mode OFF")
        SendOSCUI('/beatstep/' + beatstep.findMatrix("maxwellccs.resetCC", 'HueInt') + '/led', 0.0)
        AllStatus("CC resets OFF")

def ResetUI():

    SendOSCUI('/beatstep/on', [0])
    SendOSCUI('/lpd8/on', [0])
    SendOSCUI('/bhoreal/on', [0])
    SendOSCUI('/pad/on', [0])
    SendOSCUI('/bcr/on', [0])
'''
# Reset current CC to 64 meaning no speed,...
def resetCCON(value):

    print("CC Reseting mode ON")
    SendOSCUI('/status', ["CC resets ON"])
    SendOSCUI('/bcr/status', ["CC resets ON"])
    gstt.resetCC = 0

# Reset current CC to 64 meaning no speed,...
def resetCCOFF(value):

    print("CC Reseting mode OFF")
    SendOSCUI('/status', ["CC resets OFF"])
    SendOSCUI('/bcr/status', ["CC resets OFF"])
    gstt.resetCC = -1
'''

#
# Bangs Modes
#

# Bang 0
def bang0(value):

    gstt.bang0 = (not gstt.bang0)

    if gstt.strobe == True:
        print("Bang 0 ON")
        AllStatus("Bang 0 ON")
    else:
        print("Bang 0 OFF")
        AllStatus("Bang 0 OFF")

# Bang 1
def bang1(value):

    gstt.bang1 = (not gstt.bang1)

    if gstt.strobe == True:
        print("Bang 1 ON")
        AllStatus("Bang 1 ON")
    else:
        print("Bang 1 OFF")
        AllStatus("Bang 1 OFF")

# Bang 2
def bang2(value):

    gstt.bang2 = (not gstt.bang2)

    if gstt.strobe == True:
        print("Bang 2 ON")
        AllStatus("Bang 2 ON")
    else:
        print("Bang 2 OFF")
        AllStatus("Bang 2 OFF")

# Bang 3
def bang3(value):

    gstt.bang3 = (not gstt.bang3)

    if gstt.strobe == True:
        print("Bang 3 ON")
        AllStatus("Bang 3 ON")
    else:
        print("Bang 3 OFF")
        AllStatus("Bang 3 OFF")

# bang is trigged by something !
def bangbang(value):

    print()
    print("Bang !!")
    print()
    SendOSCUI('/status', ["Bang !!"])
    SendOSCUI('/bcr/status', ["Bang !!"])
    gstt.bangbang = True

# Strobe Mode : switch to white in LFO color mode

def strobe(value):

    gstt.strobe = (not gstt.strobe)

    if gstt.strobe == True:
        print("Strobe ON")
        gstt.intoff = gstt.ccs[gstt.lasernumber][129]
        AllStatus("Strobe ON")
        cc(129, 127 , dest ='to Maxwell 1')

    else:
        print("Strobe OFF")
        gstt.ccs[gstt.lasernumber][129] = gstt.intoff
        AllStatus("Strobe OFF")
        cc(129, gstt.ccs[gstt.lasernumber][129] , dest ='to Maxwell 1')

'''
def strobeON(value):

    print("Strobe ON")
    gstt.intoff = gstt.ccs[gstt.lasernumber][129]
    SendOSCUI('/status', ["Strobe ON"])
    SendOSCUI('/bcr/status', ["Strobe ON"])
    cc(129, 127 , dest ='to Maxwell 1')

# Stop strobe mode
def strobeOFF(value):

    print("Strobe OFF")
    gstt.ccs[gstt.lasernumber][129] = gstt.intoff
    SendOSCUI('/status', ["Strobe OFF"])
    SendOSCUI('/bcr/status', ["Strobe OFF"])
    cc(129, gstt.ccs[gstt.lasernumber][129] , dest ='to Maxwell 1')
'''

def NoteOn(note,velocity, dest=mididest, laser = gstt.lasernumber):
    midi3.NoteOn(note,velocity, mididest, laser)

def NoteOff(note, dest=mididest, laser = gstt.lasernumber):
    midi3.NoteOn(note, mididest, laser)

def Send(oscaddress,oscargs=''):

    oscmsg = OSCMessage()
    oscmsg.setAddress(oscaddress)
    oscmsg.append(oscargs)

    osclient = OSCClient()
    osclient.connect((ip, port))

    print("sending OSC message : ", oscmsg, "to", ip, ":",port)
    try:
        osclient.sendto(oscmsg, (ip, port))
        oscmsg.clearData()
        return True
    except:
        print ('Connection to', ip, 'refused : died ?')
        return False

def slinearound(samples, min, max):

    samparray = [0] * samples
    linearinc = (max-min)/samples
    for ww in range(samples):
        if ww == 0:
            samparray[ww] = round(min)
        else:
            samparray[ww] = round(samparray[ww-1] + linearinc)
    #print('linear min max', min, max)
    #print ('linear',samparray)
    return samparray

# * 11.27 : to get value from 0 to 127
def lin2squrt(value):
    return round(np.sqrt(value)*11.27)

def squrt2lin(value):
    return round(np.square(value/11.27))

def curved(value):
    return round(np.sqrt(value)*11.27)
'''
def curved(value):
    return round(np.sqrt(value)*11.27)
'''

def Mixer(value):

    Send("/mixer/value", value)

def MixerLeft(value):

    if value == 127:
        Send("/mixer/value", 0)

def MixerRight(value):

    if value == 127:
        Send("/mixer/value", 127)

def MixerTempo(tempo):

    for counter in range(127):
        Send("/mixer/value", counter)

# Jog CC send : 127 to left and 1 to right
# increase or decrease current CC defined in current path
def jogLeft(value):

    path = current["pathLeft"]
    print("jog : path =",path, "CC :", FindCC(path), "value", value)
    MaxwellCC = FindCC(current["pathLeft"])
    if value == 127:
        # decrease CC
        if gstt.ccs[0][MaxwellCC] > 0:
            gstt.ccs[0][MaxwellCC] -= 1
    else:
        if gstt.ccs[0][MaxwellCC] < 127:
            gstt.ccs[0][MaxwellCC] += 1
    #print("sending", gstt.ccs[0][MaxwellCC], "to CC", MaxwellCC )
    cc(MaxwellCC, gstt.ccs[0][MaxwellCC] , dest ='to Maxwell 1')
    #RotarySpecifics(MaxwellCC, path[path.rfind("/")+1:len(path)], value)

# Jog send 127 to left and 1 to right
# increase or decrease current CC defined in current path
def jogRight(value):

    path = current["pathRight"]
    print("jog : path =",path, "CC :", FindCC(path), "value", value)
    MaxwellCC = FindCC(current["pathRight"])
    if value == 127:
        # decrease CC
        if gstt.ccs[0][MaxwellCC] > 0:
            gstt.ccs[0][MaxwellCC] -= 1
    else:
        if gstt.ccs[0][MaxwellCC] < 127:
            gstt.ccs[0][MaxwellCC] += 1
    #print("sending", gstt.ccs[0][MaxwellCC], "to CC", MaxwellCC )
    cc(MaxwellCC, gstt.ccs[0][MaxwellCC] , dest ='to Maxwell 1')
    #RotarySpecifics(MaxwellCC, path[path.rfind("/")+1:len(path)], value)

# Parameter change  : 127 : Previous type / 0 or 1 Next type
def RotarySpecifics(MaxwellCC, value):
    global maxwell

    #print("Maxwell CC :", MaxwellCC)
    #print("path :", maxwell['ccs'][MaxwellCC]['Function'])
    #print("Current :", maxwell['ccs'][MaxwellCC]['init'])
    #print("midi value :", value)
    #print("List Values", list(specificvalues))

    for count,cctype in list(enumerate(specificvalues)):
        if (cctype in maxwell['ccs'][MaxwellCC]['Function']) == True:
            specificsname = cctype
            #print(cctype, "is in :",maxwell['ccs'][MaxwellCC]['Function'])
            break

    #print("Specifics :",specificvalues[specificsname])

    elements = list(enumerate(specificvalues[specificsname]))
    #print("elements", elements)
    nextype = maxwell['ccs'][MaxwellCC]['init']
    #print('nextype', nextype)

    for count,ele in elements:

        #if  maxwell['ccs'][MaxwellCC]['init'].Find(maxwell['ccs'][MaxwellCC]['init'])
        if ele == maxwell['ccs'][MaxwellCC]['init']:
            if count > 0 and value == 127:
                nextype = elements[count-1][1]

            if count < len(elements)-1 and value < 2:
                #print("next is :",elements[count+1][1])
                nextype = elements[count+1][1]

    #print("result :", nextype, "new value :", specificvalues[specificsname][nextype], "Maxwell CC", MaxwellCC)
    maxwell['ccs'][MaxwellCC]['init'] = nextype
    cc(MaxwellCC, specificvalues[specificsname][nextype], dest ='to Maxwell 1')

# Change type : trig with only with midi value 127 on a CC event
def ButtonSpecifics127( MaxwellCC, specificsname, value):
    global maxwell

    #print("Maxwell CC :",MaxwellCC)
    #print("Current :",maxwell['ccs'][MaxwellCC]['init'])
    #print("Specifics :",specificvalues[specificsname])
    #print("midi value :", value)

    elements = list(enumerate(specificvalues[specificsname]))
    #print(elements)
    nextype = maxwell['ccs'][MaxwellCC]['init']

    for count,ele in elements:

        if ele == maxwell['ccs'][MaxwellCC]['init']:
            if count >0 and value == 127:
                nextype = elements[count-1][1]

            if count < len(elements)-1 and value < 2:
                #print("next is :",elements[count+1][1])
                nextype = elements[count+1][1]

    print("result :", nextype, "new value :", specificvalues[specificsname][nextype], "Maxwell CC", MaxwellCC)
    maxwell['ccs'][MaxwellCC]['init'] = nextype
    cc(MaxwellCC, specificvalues[specificsname][nextype], dest ='to Maxwell 1')

#
# Maxwell patchs files
#

def LoadPatchFile(filename, laser = 0):
    global patchs

    print("Load Maxwell patchs file :", filename)
    f=open("patchs/"+filename,"r")
    s = f.read()
    gstt.patchs = json.loads(s)
    SendOSCUI('/bhoreal/status', [filename])

    #print(len(gstt.patchs['pattrstorage']))
    #print(gstt.patchs['pattrstorage']['slots']['1']['data']["gen1-x::interfm-phase-offset"])
    #print("Loaded.")

def getPatchValue(patchnumber, ccnumber):

    #print("patch", patchnumber,"CC", ccnumber, ":",maxwell['ccs'][ccnumber]['maxwell'],"=",gstt.patchs['pattrstorage']['slots'][str(patchnumber)]['data'][maxwell['ccs'][ccnumber]['maxwell']])
    return int(gstt.patchs['pattrstorage']['slots'][str(patchnumber + 1)]['data'][maxwell['ccs'][ccnumber]['maxwell']][0]*127)

def runPatch(number, laser = 0):

    print()
    print("Run patch :", number, "on laser", laser,"...")

    # Patch exist ?
    if (str(number + 1) in gstt.patchs['pattrstorage']['slots']) != False:

        # Yes
        gstt.patchnumber[laser] = number
        for ccnumber in range(len(maxwell['ccs'])):

            # Update cc variable content and OSC UI for given laser
            gstt.ccs[laser][ccnumber] = getPatchValue(gstt.patchnumber[laser], ccnumber)

            SendOSCUI('/cc/'+str(ccnumber), [getPatchValue(gstt.patchnumber[laser], ccnumber)])

            # Update BCR 2000 CC if exists
            if bcr.Here != -1:
                midi3.MidiMsg([CONTROLLER_CHANGE, ccnumber, getPatchValue(gstt.patchnumber[laser], ccnumber)], "BCR2000")
        for ccnumber in range(len(maxwell['ccs'])):
            print(ccnumber," ",gstt.ccs[laser][ccnumber])

        # Update OSC UI patch number and send to Maxwell via midi
        SendOSCUI('/laser/patch/'+str(laser), [gstt.patchnumber[laser]])
        midi3.NoteOn(gstt.patchnumber[laser], 127, 'to Maxwell 1')

        #print("Laser", laser, ": current patch is now :", gstt.patchnumber[laser], 'ccs', gstt.ccs[laser])
    else:
        print("Patch doesnt exists")

def morphPatch(number, laser = 0):

    print()
    print("Morphing to patch number", number, "laser", laser,"in", gstt.morphsteps, "steps...")

    # Patch exist ?
    if (str(number + 1) in gstt.patchs['pattrstorage']['slots']) != False:

        # Yes
        gstt.patchnumber[laser] = number
        for ccnumber in range(len(maxwell['ccs'])):

            gstt.morphCCinc[ccnumber] = (getPatchValue(gstt.patchnumber[laser], ccnumber) - gstt.ccs[laser][ccnumber]) / gstt.morphsteps
            gstt.morphCC[ccnumber] = gstt.ccs[laser][ccnumber]
            print("CC", ccnumber, "was", gstt.ccs[laser][ccnumber],"will be", getPatchValue(gstt.patchnumber[laser], ccnumber), "so inced is", gstt.morphCCinc[ccnumber])

        gstt.morphing = 0

    else:
        print("Patch doesnt exists")
        gstt.morphing = -1

# Left cue button 127 = on  0 = off
def PrevPatch(value):

    print('PrevPatchVal function')
    if value == 127 and  gstt.patchnumber[0] - 1 > -1:
        cc(9, 127, dest=djdest)
        time.sleep(0.1)
        runPatch(gstt.patchnumber[0] - 1)
        #midi3.NoteOn(current['patch'], 127, 'to Maxwell 1')
        cc(9, 0, dest=djdest)

# Right cue button 127 = on  0 = off
def NextPatch(value):

    print('NextPatchVal function')
    if value == 127 and  gstt.patchnumber[0] + 1 < 41:
        cc(3, 127, dest = djdest)
        runPatch(gstt.patchnumber[0] + 1)
        time.sleep(0.1)
        cc(3, 0, dest = djdest)

#
# CCs
#

def gsteps(value, macroname):
    if value > 90  and gstt.steps -1 > -1:
        gstt.steps -= 1
    else:
        gstt.steps += 1

    #macroname = beatstep.macros[gstt.BeatstepLayers[gstt.BeatstepLayer]][beatstep.findMacros('maxwellccs.steps','Z')]["name"]
    SendOSCUI('/beatstep/'+macroname+'/value', [format(gstt.steps, "03d")])
    #print("steps",gstt.steps,beatstep.findMacros('maxwellccs.steps','Z'), '/beatstep/'+macroname+'/value')

def grate(value, macroname):
    if value > 90 and gstt.rate -1 > -1:
        gstt.rate -= 1
    else:
        gstt.rate += 1

    #macroname = beatstep.macros[gstt.BeatstepLayers[gstt.BeatstepLayer]][beatstep.findMacros('maxwellccs.rate','Z')]["name"]
    SendOSCUI('/beatstep/'+macroname+'/value', [format(gstt.rate, "03d")])
    #print("rate",gstt.rate, beatstep.findMacros('maxwellccs.rate','Z'),'/beatstep/'+macroname+'/value')

def grange(value,macroname):
    #print("grange incoming value", value, "gstt.Range", gstt.Range)
    if value > 90 and gstt.Range -1 > -1:
        gstt.Range -= 1
    if value < 30 and gstt.Range +1 < 65:
        gstt.Range += 1

    SendOSCUI('/beatstep/'+macroname+'/value', [format(gstt.Range, "03d")])
    #print("grange",gstt.Range, beatstep.findMacros('maxwellccs.grange','Z'),'/beatstep/'+macroname+'/value')

def ginhib(value,macroname):
    #print("inhib incoming value", value, "gstt.inhib", gstt.inhib)
    if value > 90 and gstt.inhib -1 >-1:
        gstt.inhib -= 1
    if value < 30 and gstt.inhib +1 < 101:
        gstt.inhib += 1

    SendOSCUI('/beatstep/'+macroname+'/value', [format(gstt.inhib, "03d")])

# lasermode ON : set duplicator number (CC 87) to 1 and append it to fixedgenes.
# lasermode OFF : remove duplicator number (CC 87) from fixedgenes.
def lasermode(value, macroname):
    #print("lasermode with",value, macroname)
    # switch to ON
    if gstt.lasermode == 0:
        gstt.lasermode = 1
        gstt.fixedgenes.append(87)
        print("Lasermode ON")
        cc(87, 0, dest ='to Maxwell 1')

    # switch to OFF
    else:
        gstt.lasermode = 1
        gstt.fixedgenes.remove(87)
        print("Lasermode OFF")

def rateselect(value, macroname):
    #print(gstt.fixedgenes)
    gstt.fixedgenes.append(gstt.randcc)
    #print(gstt.fixedgenes)

def stepsselect(value, macroname):
    #print(gstt.fixedgenes)
    gstt.fixedgenes.append(gstt.randcc)
    #print(gstt.fixedgenes)

# add a "gene" = cc in list of already selected genes
def gselect(value, macroname):
    #print("gselect", gstt.fixedgenes, gstt.randcc)
    gstt.fixedgenes.append(gstt.randcc)
    print(gstt.randcc, "was added to fixed genes list", gstt.fixedgenes)
    print()

def PlusTenLeft(value):
    if value == 127:
        changeCC(10, current["pathLeft"])

def MinusTenLeft(value):
    if value == 127:
        changeCC(-10, current["pathLeft"])

def PlusOneLeft(value):
    if value == 127:
        changeCC(1, current["pathLeft"])

def MinusOneLeft(value):
    if value == 127:
        changeCC(-1, current["pathLeft"])

def PlusTenRight(value):
    if value == 127:
        changeCC(10, current["pathRight"])

def MinusTenRight(value):
    if value == 127:
        changeCC(-10, current["pathRight"])

def PlusOneRight(value):
    if value == 127:
        changeCC(1, current["pathRight"])

def MinusOneRight(value):
    if value == 127:
        changeCC(-1, current["pathRight"])

def PlusTen(value):
    if value == 127:
        changeCC(10, current["path"])

def MinusTen(value):
    if value == 127:
        changeCC(-10, current["path"])

def PlusOne(value, path =  current["path"]):
    if value == 127:
        changeCC(1, path)

def MinusOne(value, path =  current["path"]):
    if value == 127:
        changeCC(-1, path)

def EncoderPlusOne(value, path =  current["path"]):
    if value < 50:
        changeCC(1, path)

def EncoderMinusOne(value, path =  current["path"]):
    if value > 90:
        changeCC(-1, path)

def EncoderPlusTen(value, path =  current["path"]):
    if value < 50:
        changeCC(10, path)

def EncoderMinusTen(value, path =  current["path"]):
    if value > 90:
        changeCC(-10, path)

def ChangeCurveLeft(value):

    MaxwellCC = FindCC(current["prefixLeft"] + '/curvetype')
    RotarySpecifics(MaxwellCC, value)

def ChangeFreqLimitLeft(value):

    MaxwellCC = FindCC(current["prefixLeft"] + '/freqlimit')
    RotarySpecifics(MaxwellCC, value)

def ChangeATypeLeft(value):

    MaxwellCC = FindCC(current["prefixLeft"] + '/freqlimit')
    RotarySpecifics(MaxwellCC, value)

def ChangePMTypeLeft(value):

    MaxwellCC = FindCC(current["prefixLeft"] + '/phasemodtype')
    RotarySpecifics(MaxwellCC, value)

def ChangePOTypeLeft(value):

    MaxwellCC = FindCC(current["prefixLeft"] + '/phaseoffsettype')
    RotarySpecifics(MaxwellCC, value)

def ChangeAOTypeLeft(value):

    MaxwellCC = FindCC(current["prefixLeft"] + '/ampoffsettype')
    RotarySpecifics(MaxwellCC, value)

def ChangeCurveRight(value):

    MaxwellCC = FindCC(current["prefixRight"] + '/curvetype')
    RotarySpecifics(MaxwellCC, value)

def ChangeCurveLFO(value):

    MaxwellCC = FindCC('/lfo/'+ current["lfo"] +'/curvetype')
    RotarySpecifics(MaxwellCC, value)

def ChangeCurveRot(value):

    MaxwellCC = FindCC('/rotator/'+ current["rotator"] +'/curvetype')
    RotarySpecifics(MaxwellCC, value)

def ChangeCurveTrans(value):

    MaxwellCC = FindCC('/translator/'+ current["translator"] +'/curvetype')
    RotarySpecifics(MaxwellCC, value)

#
# UI functions
#

def Laser0():

    SendOSC(gstt.myIP, 8090, '/laser/0/button', [0])
    gstt.lasernumber = 0

def Laser1():

    SendOSC(gstt.myIP, 8090, '/laser/1/button', [1])
    gstt.lasernumber = 1

def Laser2():

    SendOSC(gstt.myIP, 8090, '/laser/2/button', [2])
    gstt.lasernumber = 2

def Laser3():

    SendOSC(gstt.myIP, 8090, '/laser/3/button', [3])
    gstt.lasernumber = 3

def Beatstep():
    SendOSCUI('/beatstep', [1])

def Bhoreal():
    SendOSCUI('/bhoreal', [1])

def Launchpad():
    SendOSCUI('/pad', [1])

def Maxwell():
    SendOSCUI('/maxwell', [1])

def c4():
    SendOSCUI('/C4', [1])

def Bcr():
    SendOSCUI('/bcr', [1])

def Aurora():
    SendOSCUI('/aurora', [1])

def PPatch():

    PrevPatch(127)

def NPatch():
    NextPatch(127)

def Reload():
    LoadPatchFile(gstt.PatchFiles[0])
    launchpad.UpdateDisplay()
    bhoreal.DisplayUpdate()

def Load():

    pass
    #Tk().withdraw()
    #PatchFile =  tkinter.filedialog.askopenfilename()
    '''
    PatchFile = easygui.fileopenbox(default='patchs/*.json', filetypes=["*.json"], multiple=False)
    print(PatchFile)
    '''
    #LoadPatchFile(gstt.PatchFiles[0])

def L():

    ccnumber = FindCC('/mixer/value')
    print("Incoming Mixer L<->R : CC", ccnumber, "with value", 0)
    if gstt.lasernumber == 0:
        cc(ccnumber, 0,'to Maxwell 1')
    else:
        SendOSC(gstt.computerIP[gstt.lasernumber], gstt.MaxwellatorPort, '/mixer/value', 0)
    #beatstep.UpdateCC(ccnumber, 0)

def R():
    ccnumber = FindCC('/mixer/value')
    print("Incoming Mixer L<->R : CC", ccnumber, "with value", 127)
    if gstt.lasernumber == 0:
        cc(ccnumber, 127,'to Maxwell 1')
    else:
        SendOSC(gstt.computerIP[gstt.lasernumber], gstt.MaxwellatorPort, '/mixer/value', 127)
    #beatstep.UpdateCC(ccnumber, 127)

def Empty():

    print ("Empty")

# Beatstep previous layer
def BPLayer():

    beatstep.PLayer()

# Beatstep next layer
def BNLayer():

    beatstep.NLayer()

# LPD8 previous layer
def LPLayer():

    LPD8.PLayer()

# LPD8 Next layer
def LNLayer():

    LPD8.NLayer()

#
# Songs
#

def NSong():

    print(gstt.song + 1, len(gstt.songs))
    if gstt.song + 1< len(gstt.songs):
        gstt.song += 1
        print("New song :",gstt.songs[gstt.song])
        SendOSCUI('/song/status', [gstt.songs[gstt.song]])

def PSong():

    if gstt.song != 0:
        gstt.song -= 1
        print("New song :",gstt.songs[gstt.song])
        SendOSCUI('/song/status', [gstt.songs[gstt.song]])

# Forward incoming sequencer CC changes to local (= on midi channel 16) display i.e BCR 2000
def ELCC(ccnumber,value):

    print("ELCC forward CC", ccnumber,":", value, "to TouchOSC and BCR 2000 channel 16")

    SendOSCUI('/states/cc/'+str(ccnumber), [value])
    midi3.MidiMsg((CONTROLLER_CHANGE+15, ccnumber, int(value)), mididest = "BCR2000")

#
# BPM/Tempo network style with Ableton Link
#

prevphase = 0
effectosc =  { 0: "X", 1: "Y" , 2: "Z"}
effectside =  { 0: "left", 1: "right"}

def StartLink():

    global lnk
    import link

    print("Link ENABLED")
    lnk = link.Link(120)
    lnk.enabled = True
    lnk.startStopSyncEnabled = True
    SendOSCUI('/bpm', "120")

def RandomEvent():

    autoeffect = random.randint(0,4)
    autovalue = random.randint(0, gstt.autoval)
    automin = random.randint(25,gstt.autoval)

    # random Mixer Left/Right
    if autoeffect == 0:
        print("/mixer/value", random.randint(0, 127))
        SendCC("/mixer/value", random.randint(0, 127))

    # random blanking/points draw mode
    elif autoeffect == 1:
        print("points")
        cc(137, random.randint(43,127), dest="to Maxwell 1")

    # random oscillator frequency
    elif autoeffect == 2:
        print("/osc/"+effectside[random.randint(0,1)]+"/"+effectosc[random.randint(0,2)]+"/freq", autovalue)
        SendCC("/osc/"+effectside[random.randint(0,1)]+"/"+effectosc[random.randint(0,2)]+"/freq", autovalue)

    # random oscillator phasemod
    elif autoeffect == 3:
        print("/osc/"+effectside[random.randint(0,1)]+"/"+effectosc[random.randint(0,2)]+"/phasemod", autovalue)
        SendCC("/osc/"+effectside[random.randint(0,1)]+"/"+effectosc[random.randint(0,2)]+"/phasemod", autovalue)

    # random number of points
    elif autoeffect == 4:
        print("/points/number", automin)
        SendCC("/points/number", automin)

def BeatEvent():
    global lnk, prevphase

    if gstt.link == True:
        lnkstr = lnk.captureSessionState()
        link_time = lnk.clock().micros();
        tempo_str = '{0:.2f}'.format(lnkstr.tempo())
        beats_str = '{0:.2f}'.format(lnkstr.beatAtTime(link_time, 0))
        playing_str = str(lnkstr.isPlaying()) # always False ???
        phase = lnkstr.phaseAtTime(link_time, 4)

        '''
        phase_str = ''
        for x in range(0, 4):
          if x < phase:
            phase_str += 'X'
          else:
            phase_str += '0'
        sys.stdout.write(
          'tempo ' + tempo_str + ' | playing: ' + playing_str + ' | beats ' + beats_str
          + ' | ' + phase_str + '  \r')
        sys.stdout.flush()

        #print("Link time", link_time, "values", lnkstr.tempo(), lnkstr.beatAtTime(link_time, 0), lnkstr.isPlaying(), lnkstr.phaseAtTime(link_time, 4))
        #print("Link time", link_time, "strings", link_time, tempo_str, beats_str, playing_str, phase)

        '''

        # new beat ?
        if int(phase) != prevphase:
            prevphase = int(phase)
            '''
            sys.stdout.write("Beat "+str(beats_str) + '  \r')
            sys.stdout.flush()
            '''
            #SendOSCUI('/states/cc/'+str(ccnumber), [value])
            SendOSCUI('/beats', beats_str)
            #AllStatus("Beat "+str(beats_str))

            # one CC change if Autoanimation autolink ON
            if gstt.autolink == True:

                RandomEvent()
                RandomEvent()
                RandomEvent()

                '''
                # Really no switch in python ?

                doit = jumptable.get(autoeffect)
                doit(autovalue)

                jumptable =  {
                    0: Mapping,
                    1: PLS,
                    2: Orbits,
                    3: Dot,
                    4: Sine,
                    5: Astro,
                    6: LaserID,
                    7: Pose
                    }
                '''
# Change autoval
def autoval(val1, keyname):

    print("Autoval was :", gstt.autoval)
    # + 1
    if val1 == 1 and gstt.autoval < 125:
        gstt.autoval += 2

    # -1
    if val1 == 127 and gstt.autoval > 2:
        gstt.autoval -= 2

    print("Autoval :", gstt.autoval)
    AllStatus("Autoval : "+ str(gstt.autoval))

# Change autolink flag for auto animation on beat.
def autolink(value):

    gstt.autolink = (not gstt.autolink)
    print("AUTOLINK :", gstt.autolink)
    if gstt.autolink == True:
        AllStatus("Auto Link ON")
        SendOSCUI('/beatstep/' + beatstep.findMatrix("maxwellccs.autolink", 'HueInt') + '/led', 1.0)
    else:
        AllStatus("Auto Link OFF")
        SendOSCUI('/beatstep/' + beatstep.findMatrix("maxwellccs.autolink", 'HueInt') +'/led', 0.0)

# Get BPM from tap tempo or note from a sequencer
# BPM channel 16 CC 127
def autotempo():
    global lastime

    currentime = datetime.now()
    delta = currentime - lastime
    lastime = currentime
    gstt.currentbpm = round(60/delta.total_seconds())
    print("length between notes :", delta.total_seconds(),"seconds -> bpm :", gstt.currentbpm)

    # STUPID : need to find a way to display bpm > 127
    # tell BCR 2000 the new bpm on channel 16
    if gstt.currentbpm > 127:
        midi3.MidiMsg((CONTROLLER_CHANGE+15, 127, 127), mididest = "BCR2000")
    else:
        midi3.MidiMsg((CONTROLLER_CHANGE+15, 127, gstt.currentbpm), mididest = "BCR2000")
    SendOSCUI('/bpm', [gstt.currentbpm])
    newtempo(gstt.currentbpm)

# Change current Link Tempo.
def newtempo(tempo):
    global lnk

    #print("val2", val2, "tempo", tempo)

    if gstt.link == True:
        lnk.enabled = False
        lnk.startStopSyncEnabled = False
        lnk = link.Link(tempo)
        lnk.enabled = True
        lnk.startStopSyncEnabled = True
        gstt.currentbpm = tempo
        print("New BPM", tempo)
        SendOSCUI('/bpm', tempo)
        #SendOSCUI('/bpm', [gstt.currentbpm])
        #AllStatus("New BPM : "+ str(tempo))
    else:
        print("Link is disabled")

#
def BPMAdj(val1, keyname):

    print(gstt.currentbpm)

    # + 1
    if val1 == 1:
        newtempo(gstt.currentbpm+1)

    # -1
    if val1 == 127 and gstt.currentbpm > 0:
        newtempo(gstt.currentbpm-1)

#
# OBS STUDIO scenes remote control
#

def OBSmax(x):

    if gstt.obs == True:
        print("switching to Scene MAXWELL only")
        obs.goScene("MAXWELL only")

    else:
        print("OBS is not running.")

def OBSblck(x):

    if gstt.obs == True:
        print("switching to Scene Black")
        obs.goScene("Black")
    else:
        print("OBS is not running.")

def OBStuff(x):

    if gstt.obs == True:
        print("switching to Scene Stuff maxwell")
        obs.goScene("Stuff maxwell")
    else:
        print("OBS is not running.")

def OBSlive(x):

    if gstt.obs == True:
        print("switching to Scene live")
        obs.goScene("live")
    else:
        print("OBS is not running.")

#
# Startup
#

gstt.obs = obs.isconnected()
if gstt.obs == True:
    print("OBS is running.")
    obs.Start()
else:
    print("OBS is not running !")

lastime  = datetime.now()
tempotime = lastime

LoadPatchFile(gstt.PatchFiles[gstt.lasernumber])


import Quandl
import pandas as pd
import pickle
import matplotlib.pyplot as plt
from matplotlib import style
style.use('fivethirtyeight')

def grab_initial_state_data():
    states = state_list()

    main_df = pd.DataFrame()

    for abbv in states:
        query = "FMAC/HPI_"+str(abbv)
        df = Quandl.get(query, authtoken=api_key)
        print(query)
        df = df.pct_change()
        print(df.head())
        if main_df.empty:
            main_df = df
        else:
            main_df = main_df.join(df)

    pickle_out = open('fiddy_states2.pickle','wb')
    pickle.dump(main_df, pickle_out)
    pickle_out.close()

grab_initial_state_data()

HPI_data = pd.read_pickle('fiddy_states2.pickle')

HPI_data.plot()
plt.legend().remove()
plt.show()


#!/usr/bin/env python3
import genrndtbl

class HashTable:
    def __init__(self, table):
        self.table = table
        self.size = len(table)

    def hash(self, x):
        return self.table[x % self.size]

    @staticmethod
    def Create(length, seed=None):
        t = genrndtbl.generate_random_table(length, seed)
        return HashTable(t)

if __name__ == "__main__":
    import sys
    import argparse

    parser = argparse.ArgumentParser()
    parser.add_argument("-l", "--length", type=int,
                        default=256, help="length of table")
    parser.add_argument("-r", "--random_seed", type=int, help="random seed")
    args = parser.parse_args()

    ht = HashTable.Create(args.length, args.random_seed)

    for line in sys.stdin:
        s = line.strip()
        if not s:
            break

        v = s.split(' ')

        xi = int(v[0])
        yi = int(v[1])
        xj = (int(v[0]) + 1)
        yj = (int(v[1]) + 1)

        aa = ht.hash(ht.hash(xi) + yi)
        ab = ht.hash(ht.hash(xi) + yj)
        ba = ht.hash(ht.hash(xj) + yi)
        bb = ht.hash(ht.hash(xj) + yj)
        print('{0} {1} {2} {3} {4[2]} {4[3]}'.format(aa, ab, ba, bb, v))


# -*- coding: utf-8 -*-
import time
import io
import re
from pathlib import Path

import vk_api
from vk_api.bot_longpoll import VkBotLongPoll, VkBotEventType
from vk_api.utils import get_random_id
from pytils import numeral
import requests, yaml

from functions import get_time

BASE_DIR = Path(__file__).resolve().parent
CONFIG_PATH = BASE_DIR.joinpath("config.yaml")

with open(CONFIG_PATH) as ymlFile:
    config = yaml.load(ymlFile.read(), Loader=yaml.Loader)

class Utils(object):
    def __init__(self, upload):
        self.upload = upload

    def get_photo_by_url(self, url: str) -> str:
        r = requests.get(url)
        image = io.BytesIO(r.content)

        # Отправляем фото в беседу
        response = self.upload.photo_messages(photos=image)[0]
        attachment = "photo{}_{}".format(response["owner_id"], response["id"])
        return attachment

class User(object):
    """Функции для работы с пользователем"""
    def __init__(self, vk, upload):
        self.vk = vk
        self.upload = upload

    def get_username(self, user_id: int) -> str:
        user_info = self.vk.users.get(user_ids=user_id)[0]
        username = "{} {}".format(
            user_info["first_name"],
            user_info["last_name"]
        )
        return f"[id{user_id}|{username}]"

    def get_user_last_activity(self, user_id: int, sex: int) -> str:
        last_activity = self.vk.messages.getLastActivity(
            user_id=user_id
        )
        if last_activity["online"]:
            return "Сейчас онлайн\n"
        else:
            delta = time.time() - last_activity["time"]
            if sex == 1:
                return f"Была в сети {get_time(delta)} назад\n"
            elif sex == 2:
                return f"Был в сети {get_time(delta)} назад\n"
            else:
                pass  # Attack helicopter

    def get_user_profile_photo(self) -> str:
        photo_url = self.user_info["photo_max_orig"]
        if photo_url == "https://vk.com/images/camera_400.png":
            # У пользователя нет аватарки
            return ""
        else:
            attachment = utils.get_photo_by_url(photo_url)
            return attachment

    def get_user_status(self) -> str:
        """Возвращает статус пользователя"""
        status = self.user_info["status"]
        if status != "":
            return f"Статус: {status}\n"

        return ""

    def get_user_photos(self) -> str:
        if "photos" in self.user_info["counters"]:
            count = self.user_info["counters"]["photos"]
            if count != 0:
                return f"{count} фото\n"
        return ""

    def get_user_audios(self) -> str:
        if "audios" in self.user_info["counters"]:
            count = self.user_info["counters"]["audios"]
            if count != 0:
                return "{}\n".format(
                    numeral.get_plural(count, "аудиозапись, аудиозаписи, аудиозаписей")
                )
        return ""

    def get_user_videos(self) -> str:
        if "videos" in self.user_info["counters"]:
            count = self.user_info["counters"]["videos"]
            if count != 0:
                return f"{count} видео\n"

        return ""

    def get_user_friends(self) -> str:
        if "friends" in self.user_info["counters"]:
            count = self.user_info["counters"]["friends"]
            if count != 0:
                return "{}\n".format(
                    numeral.get_plural(count, "друг, друга, друзей")
                )

        return ""

    def get_user_followers(self) -> str:
        if "followers" in self.user_info["counters"]:
            count = self.user_info["counters"]["followers"]
            if count != 0:
                return "{}\n".format(
                    numeral.get_plural(count, "подписчик, подписчика, подписчиков")
                )

        return ""

    def get_user_groups(self) -> str:
        if "groups" in self.user_info["counters"]:
            count = self.user_info["counters"]["groups"]
            if count != 0:
                return "{}\n".format(
                    numeral.get_plural(count, "группа, группы, групп")
                )

        return ""

    def get_user_info(self, user_id: int) -> dict:
        self.user_info = self.vk.users.get(
            user_ids=user_id,
            fields="sex, status, counters, photo_max_orig"
        )[0]

        message = ""
        message += self.get_username(user_id) + "\n"
        message += f"id - {user_id}\n"
        message += self.get_user_last_activity(user_id, self.user_info['sex'])
        message += self.get_user_status()

        if self.user_info["is_closed"]:
            # Профиль пользователя закрыт
            message += "Профиль закрыт ❌\n"
        else:
            # Профиль пользователя открыт
            message += "Профиль открыт ✅\n"
            message += self.get_user_photos()
            message += self.get_user_audios()
            message += self.get_user_videos()
            message += self.get_user_friends()
            message += self.get_user_followers()
            message += self.get_user_groups()

        return {
            "message": message,
            "attachment": self.get_user_profile_photo()
        }

class Group(object):
    """Функции для работы с группами"""
    def __init__(self, vk, upload):
        self.vk = vk
        self.upload = upload

    def get_group_owner(self, group_id: int) -> str:
        try:
            owner_id = self.vk.groups.getMembers(
                group_id=group_id,
                filter="managers"
            )["items"][0]["id"]
            return f"Создатель - {user.get_username(owner_id)}\n"

        except vk_api.exceptions.ApiError:
            return ""

    def get_group_info(self, group_id: int) -> dict:
        """Отправляет сообщение с информацией о группе."""
        group_info = self.vk.groups.getById(group_id=group_id)[0]

        max_size = list(group_info)[-1]
        photo_url = group_info[max_size]

        message = ""
        message += f"{group_info['name']}\n"
        message += f"id - {group_info['id']}\n"
        message += self.get_group_owner(group_id)
        message += "Аватарка"

        return {
            "message": message,
            "attachment": utils.get_photo_by_url(photo_url)
        }

class Chat(object):
    def __init__(self, vk, upload, bot):
        self.vk = vk
        self.upload = upload
        self.bot = bot

    def get_chat_photo(self, chat_info: dict) -> str:
        """
        Вовращает фото беседы если оно у неё есть,
        иначе вовращает пустую строку.
        """
        if "photo" in chat_info:
            sizes = chat_info["photo"]
            max_size = list(sizes)[-2]
            photo_url = sizes[max_size]

            return utils.get_photo_by_url(photo_url)
        else:
            return ""

    def get_chat_info(self, chat_id: int) -> dict:
        """Отправляет сообщение с информацией о беседе."""

        chat_info = self.bot.messages.getConversationsById(
            peer_ids=2000000000 + chat_id,
            group_id=config["group"]["group_id"]
        )["items"][0]["chat_settings"]

        chat_name = chat_info["title"]
        members_count = chat_info["members_count"]
        owner_id = chat_info["owner_id"]

        message = ""
        message += "Название - {chat_name}\n"
        message += "{}\n".format(numeral.get_plural(members_count, "участник, участника, участников"))
        message += f"Создатель - {user.get_username(owner_id)}\n"
        message += "Аватарка"

        return {
            "message": message,
            "attachment": self.get_chat_photo(chat_info)
        }

    def is_admin(self, user_id: int, chat_id: int) -> bool:
        """Проверяем, является ли пользователь администратором беседы"""
        chat_members = self.bot.messages.getConversationMembers(
            peer_id=2000000000 + chat_id,
            group_id=config["group"]["group_id"],
        )["items"]

        is_admin = False

        for user in chat_members:
            if user["member_id"] == user_id and "is_admin" in user and not is_admin:
                is_admin = True
                break

        return is_admin

    def ban_user(self, from_id: int, user_id: int, chat_id: int):
        if from_id == user_id:
            self.bot.messages.send(
                chat_id=chat_id,
                message="Вы не можете забанить себя",
                random_id=get_random_id()
            )

        elif not self.is_admin(from_id,chat_id):
            self.bot.messages.send(
                chat_id=chat_id,
                message="У вас недостаточно прав для использования этой команды",
                random_id=get_random_id()
            )

        elif self.is_admin(from_id, chat_id):
            self.bot.messages.removeChatUser(
                chat_id=chat_id,
                user_id=user_id
            )

            self.bot.messages.send(
                chat_id=chat_id,
                message="Пользователь {} забанен".format(
                    user.get_username(user_id=user_id)
                ),
                random_id=get_random_id()
            )

class Bot(object):
    def __init__(self, bot, longpoll):
        self.bot = bot
        self.longpoll = longpoll

    def _get_help(self, chat_id: int):
        message = "Список команд\n" \
                  "!помощь\n" \
                  "!инфо беседа\n" \
                  "!инфо @(Имя пользователя) или id(id пользователя)\n" \
                  "!инфо @(Название группы) или @public(id группы)\n\n" \
                  "Доступно только администраторам\n" \
                  "!бан @(Имя пользователя)"

        self.bot.messages.send(
            chat_id=chat_id,
            message=message,
            random_id=get_random_id()
        )

    def check_message(self, received_message, chat_id):
        if re.match("!инфо", received_message):
            if received_message == "!инфо беседа":
                data = chat.get_chat_info(chat_id=chat_id)
                self.bot.messages.send(
                    chat_id=chat_id,
                    message=data["message"],
                    attachment=data["attachment"],
                    random_id=get_random_id()
                )
            elif re.match("!инфо id", received_message):
                user_id = int(received_message.split("id")[1].split("|")[0])
                data = user.get_user_info(user_id)
                self.bot.messages.send(
                    chat_id=chat_id,
                    message=data["message"],
                    attachment=data["attachment"],
                    random_id=get_random_id()
                )
            elif re.match("!инфо club", received_message):
                group_id = int(received_message.split("club")[1].split("|")[0])
                data = group.get_group_info(group_id)
                self.bot.messages.send(
                    chat_id=chat_id,
                    message=data["message"],
                    attachment=data["attachment"],
                    random_id=get_random_id()
                )
            else:
                self.bot.messages.send(
                    chat_id=chat_id,
                    message="Такой команды не существует",
                    random_id=get_random_id()
                )

        elif re.match("!бан id", received_message):
            user_id = int(received_message.split("id")[1].split("|")[0])
            chat.ban_user(
                from_id=self.from_id,
                user_id=user_id,
                chat_id=chat_id
            )
        elif received_message == "!помощь":
            self._get_help(chat_id)

    def listen(self):
        print("Начинаю мониторинг сообщений...")

        while True:
            try:
                """Отслеживаем каждое событие в беседе."""
                for event in self.longpoll.listen():
                    if event.type == VkBotEventType.MESSAGE_NEW and event.from_chat and event.message.get("text") != "":
                        received_message = event.message.get("text").lower().replace("[", "").replace("]", "")
                        chat_id = event.chat_id
                        self.from_id = event.message.get("from_id")
                        self.check_message(received_message, chat_id)
            except (requests.exceptions.ReadTimeout, requests.exceptions.ConnectionError) as e:
                print(e)

if __name__ == "__main__":
    # Авторизируем бота
    authorize = vk_api.VkApi(token=config["group"]["group_token"])
    longpoll = VkBotLongPoll(
        authorize,
        group_id=config["group"]["group_id"]
    )
    upload = vk_api.VkUpload(authorize)
    bot = authorize.get_api()

    # Авторизируем пользователя
    vk_session = vk_api.VkApi(
        token=config["user"]["user_token"]
    )
    vk = vk_session.get_api()

    vkbot = Bot(
        longpoll=longpoll,
        bot=bot
    )

    utils = Utils(
        upload=upload
    )

    user = User(
        vk=vk,
        upload=upload
    )

    group = Group(
        vk=vk,
        upload=upload
    )

    chat = Chat(
        vk=vk,
        upload=upload,
        bot=bot
    )

    vkbot.listen()  # Запускаем мониторинг бесед


import numpy as np
from pbal.msg import   (SlidingStateStamped,
                        FrictionParamsStamped,
                        ControlCommandStamped,
                        QPDebugStamped,
                        PivotSlidingCommandedFlagStamped,
                        PolygonContactStateStamped,
                        TorqueConeBoundaryFlagStamped,
                        TorqueConeBoundaryTestStamped,
                        TorqueBoundsStamped,
                        GeneralizedPositionsStamped)

from geometry_msgs.msg import   (PoseStamped,
                                TransformStamped,
                                Pose2D,
                                WrenchStamped,
                                PointStamped)

def list2pose_stamped(pose, frame_id="world"):
    msg = PoseStamped()
    msg.header.frame_id = frame_id
    msg.pose.position.x = pose[0]
    msg.pose.position.y = pose[1]
    msg.pose.position.z = pose[2]
    msg.pose.orientation.x = pose[3]
    msg.pose.orientation.y = pose[4]
    msg.pose.orientation.z = pose[5]
    msg.pose.orientation.w = pose[6]
    return msg

def pose_stamped2list(msg):
    return [float(msg.pose.position.x),
            float(msg.pose.position.y),
            float(msg.pose.position.z),
            float(msg.pose.orientation.x),
            float(msg.pose.orientation.y),
            float(msg.pose.orientation.z),
            float(msg.pose.orientation.w)]

def list2transform_stamped(transform, header_frame_id = 'base', child_frame_id = 'hand_estimate'):
    msg = TransformStamped()
    msg.header.frame_id = header_frame_id
    msg.child_frame_id = child_frame_id
    msg.transform.translation.x = transform[0]
    msg.transform.translation.y = transform[1]
    msg.transform.translation.z = transform[2]
    msg.transform.rotation.x = transform[3]
    msg.transform.rotation.y = transform[4]
    msg.transform.rotation.z = transform[5]
    msg.transform.rotation.w = transform[6]
    return msg

def transform_stamped2list(msg):
    return [float(msg.transform.translation.x),
            float(msg.transform.translation.y),
            float(msg.transform.translation.z),
            float(msg.transform.rotation.x),
            float(msg.transform.rotation.y),
            float(msg.transform.rotation.z),
            float(msg.transform.rotation.w)]

def list2wrench_stamped(wrench, frame_id='base'):
    msg = WrenchStamped()
    msg.header.frame_id = frame_id
    msg.wrench.force.x = wrench[0]
    msg.wrench.force.y = wrench[1]
    msg.wrench.force.z = wrench[2]
    msg.wrench.torque.x = wrench[3]
    msg.wrench.torque.y = wrench[4]
    msg.wrench.torque.z = wrench[5]
    return msg

def wrench_stamped2list(msg):
    return [float(msg.wrench.force.x),
            float(msg.wrench.force.y),
            float(msg.wrench.force.z),
            float(msg.wrench.torque.x),
            float(msg.wrench.torque.y),
            float(msg.wrench.torque.z)]

def wrenchstamped_2FT(msg):
    force = [float(msg.wrench.force.x),
             float(msg.wrench.force.y),
             float(msg.wrench.force.z)]

    torque = [float(msg.wrench.torque.x),
              float(msg.wrench.torque.y),
              float(msg.wrench.torque.z)]

    return force, torque

def list2point_stamped(xyz):
    msg = PointStamped()
    msg.point.x = xyz[0]
    msg.point.y = xyz[1]
    msg.point.z = xyz[2]
    return msg

def point_stamped2list(msg):
    return [float(msg.point.x), float(msg.point.y), float(msg.point.z)]

def list2pose_twod(pose):
    msg = Pose2D()
    msg.x = pose[0]
    msg.y = pose[1]
    msg.theta = pose[2]
    return msg

def pose_twod2list(msg):
    return [float(msg.x), float(msg.y), float(msg.theta)]

def quat2list(quat):
    return [float(quat.x), float(quat.y), float(quat.z), float(quat.w)]

def unit_pose_stamped():
    return list2pose_stamped([0.0,0.0,0.0,0.0,0.0,0.0,1.0])

def generate_torque_cone_boundary_flag_stamped(torque_boundary_flag):
    torque_boundary_flag_message = TorqueConeBoundaryFlagStamped()
    torque_boundary_flag_message.boundary_flag = torque_boundary_flag
    return torque_boundary_flag_message

def parse_torque_cone_boundary_flag_stamped(torque_boundary_flag_message):
    return torque_boundary_flag_message.boundary_flag

def generate_torque_cone_boundary_test_stamped(torque_boundary_boolean):
    torque_boundary_boolean_message = TorqueConeBoundaryTestStamped()
    torque_boundary_boolean_message.boundary_test = torque_boundary_boolean
    return torque_boundary_boolean_message

def parse_torque_cone_boundary_test_stamped(torque_boundary_boolean_message):
    return torque_boundary_boolean_message.boundary_test

def generate_pivot_sliding_commanded_flag(pivot_sliding_commanded_flag):
    pivot_sliding_commanded_flag_message = PivotSlidingCommandedFlagStamped()
    pivot_sliding_commanded_flag_message.command_flag = pivot_sliding_commanded_flag
    return pivot_sliding_commanded_flag_message

def parse_pivot_sliding_commanded_flag(pivot_sliding_commanded_flag_message):
    return pivot_sliding_commanded_flag_message.command_flag

def generate_torque_bounds_stamped(torque_bounds):
    torque_bounds_message = TorqueBoundsStamped()
    torque_bounds_message.torque_bounds = torque_bounds
    return torque_bounds_message

def parse_torque_bounds_stamped(torque_bounds_message):
    return torque_bounds_message.torque_bounds

def generate_generalized_positions_stamped(generalized_positions):
    generalized_positions_message = GeneralizedPositionsStamped()
    generalized_positions_message.generalized_positions = generalized_positions
    return generalized_positions_message

def parse_generalized_positions_stamped(generalized_positions_message):
    return generalized_positions_message.generalized_positions

def generate_polygon_contact_state_stamped(vertex_array,contact_indices,mgl_cos_theta_list=None,mgl_sin_theta_list=None,
                                            hand_contact_indices = None, ground_contact_indices = None, wall_contact_indices =  None,
                                            wall_flag = None):
    polygon_contact_state_message = PolygonContactStateStamped()

    l = len(vertex_array[0])

    polygon_contact_state_message.polygon_contact_state.x_coords = list(vertex_array[0])
    polygon_contact_state_message.polygon_contact_state.y_coords = list(vertex_array[1])
    polygon_contact_state_message.polygon_contact_state.z_coords = list(vertex_array[2])

    polygon_contact_state_message.polygon_contact_state.contact_indices = list(contact_indices)

    if mgl_cos_theta_list is None:
        mgl_cos_theta_list = [0.0]*l
        mgl_sin_theta_list = [0.0]*l

    polygon_contact_state_message.polygon_contact_state.mgl_costheta_list = mgl_cos_theta_list
    polygon_contact_state_message.polygon_contact_state.mgl_sintheta_list = mgl_sin_theta_list

    if hand_contact_indices is None:
        hand_contact_indices = []

    polygon_contact_state_message.polygon_contact_state.hand_contact_indices = hand_contact_indices

    if ground_contact_indices is None:
        ground_contact_indices = []

    polygon_contact_state_message.polygon_contact_state.ground_contact_indices = ground_contact_indices

    if wall_contact_indices is None:
        wall_contact_indices = []

    polygon_contact_state_message.polygon_contact_state.wall_contact_indices = wall_contact_indices

    if wall_flag is None:
        wall_flag = -1

    polygon_contact_state_message.polygon_contact_state.wall_flag = wall_flag

    return polygon_contact_state_message

def parse_polygon_contact_state_stamped(polygon_contact_state_message):
    x_coords = list(polygon_contact_state_message.polygon_contact_state.x_coords)
    y_coords = list(polygon_contact_state_message.polygon_contact_state.y_coords)
    z_coords = list(polygon_contact_state_message.polygon_contact_state.z_coords)

    mgl_cos_theta_list = list(polygon_contact_state_message.polygon_contact_state.mgl_costheta_list)
    mgl_sin_theta_list = list(polygon_contact_state_message.polygon_contact_state.mgl_sintheta_list)

    vertex_array = np.array([x_coords,y_coords,z_coords])

    contact_indices = list(polygon_contact_state_message.polygon_contact_state.contact_indices)

    hand_contact_indices = list(polygon_contact_state_message.polygon_contact_state.hand_contact_indices)
    ground_contact_indices = list(polygon_contact_state_message.polygon_contact_state.ground_contact_indices)
    wall_contact_indices = list(polygon_contact_state_message.polygon_contact_state.wall_contact_indices)

    wall_flag = polygon_contact_state_message.polygon_contact_state.wall_flag

    polygon_contact_state_dict = {}
    polygon_contact_state_dict['vertex_array'] = vertex_array
    polygon_contact_state_dict['contact_indices'] = contact_indices
    polygon_contact_state_dict['hand_contact_indices'] = hand_contact_indices
    polygon_contact_state_dict['ground_contact_indices'] = ground_contact_indices
    polygon_contact_state_dict['wall_contact_indices'] = wall_contact_indices
    polygon_contact_state_dict['mgl_cos_theta_array'] = np.array(mgl_cos_theta_list)
    polygon_contact_state_dict['mgl_sin_theta_array'] = np.array(mgl_sin_theta_list)
    polygon_contact_state_dict['wall_flag'] = wall_flag

    return polygon_contact_state_dict

def sliding_dict_to_sliding_stamped(sliding_dict):

    sliding_msg = SlidingStateStamped()

    sliding_msg.sliding_state.pivot_sliding_left = sliding_dict["pslf"]     # True if pivot sliding left
    sliding_msg.sliding_state.pivot_sliding_right  = sliding_dict["psrf"]   # True if pivot sliding right
    sliding_msg.sliding_state.pivot_sliding = sliding_dict["psf"]           # pivot_sliding_left || pivot_sliding_right

    sliding_msg.sliding_state.robot_sliding_left = sliding_dict["cslf"]     # True if pivot sliding left
    sliding_msg.sliding_state.robot_sliding_right = sliding_dict["csrf"]    # True if pivot sliding right
    sliding_msg.sliding_state.robot_sliding = sliding_dict["csf"]        # robot_sliding_left || robot_sliding_right

    return sliding_msg

def sliding_stamped_to_sliding_dict(sliding_msg):

    sliding_dict = {}

    sliding_dict["pslf"] = sliding_msg.sliding_state.pivot_sliding_left
    sliding_dict["psrf"] = sliding_msg.sliding_state.pivot_sliding_right
    sliding_dict["psf"] = sliding_msg.sliding_state.pivot_sliding

    sliding_dict["cslf"] = sliding_msg.sliding_state.robot_sliding_left
    sliding_dict["csrf"] = sliding_msg.sliding_state.robot_sliding_right
    sliding_dict["csf"] = sliding_msg.sliding_state.robot_sliding

    return sliding_dict

def friction_dict_to_friction_stamped(friction_dict):

    friction_msg = FrictionParamsStamped()

    # ground contact LHS
    friction_msg.friction_params.A_pivot_left = [
        item for row in friction_dict['ael'] for item in row]
    friction_msg.friction_params.b_pivot_left = friction_dict['bel']

    # ground contact RHS
    friction_msg.friction_params.A_pivot_right = [
        item for row in friction_dict['aer'] for item in row]
    friction_msg.friction_params.b_pivot_right = friction_dict['ber']

    # ground contact bools
    friction_msg.friction_params.use_env_left = friction_dict['elu']
    friction_msg.friction_params.use_env_right = friction_dict['eru']

    # hand contact LHS
    friction_msg.friction_params.A_robot_left = friction_dict['acl']
    friction_msg.friction_params.b_robot_left = friction_dict['bcl']

    # hand contact RHS
    friction_msg.friction_params.A_robot_right = friction_dict['acr']
    friction_msg.friction_params.b_robot_right = friction_dict['bcr']

    # hand contact bool
    friction_msg.friction_params.use_robot = friction_dict['cu']

    return friction_msg

def friction_stamped_to_friction_dict(friction_msg):

    friction_dict = {}

    # ground contact LHS
    friction_dict['ael'] = np.reshape(np.array(
        friction_msg.friction_params.A_pivot_left), newshape=(-1, 3)).tolist()
    friction_dict['bel'] = list(friction_msg.friction_params.b_pivot_left)

    # ground contact RHS

    friction_dict['aer'] = np.reshape(np.array(
        friction_msg.friction_params.A_pivot_right), newshape=(-1, 3)).tolist()
    friction_dict['ber'] = list(friction_msg.friction_params.b_pivot_right)

    # ground contact bools
    friction_dict['elu'] = friction_msg.friction_params.use_env_left
    friction_dict['eru'] = friction_msg.friction_params.use_env_right

    # hand contact LHS
    friction_dict['acl'] = list(friction_msg.friction_params.A_robot_left)
    friction_dict['bcl'] = friction_msg.friction_params.b_robot_left

    # hand contact RHS
    friction_dict['acr'] = list(friction_msg.friction_params.A_robot_right)
    friction_dict['bcr'] = friction_msg.friction_params.b_robot_right

    # hand contact bool
    friction_dict['cu'] = friction_msg.friction_params.use_robot

    return friction_dict

def command_stamped_to_command_dict(command_msg):

    command_dict = {}

    command_dict['name']            = command_msg.control_command.name
    command_dict['command_flag']    = command_msg.control_command.command_flag
    command_dict['mode']            = command_msg.control_command.mode

    # relative commands
    command_dict['delta_theta']     = command_msg.control_command.delta_theta
    command_dict['delta_s_pivot']   = command_msg.control_command.delta_s_pivot
    command_dict['delta_s_hand']    = command_msg.control_command.delta_s_hand

    command_dict['delta_theta_relative']     = command_msg.control_command.delta_theta_relative
    command_dict['delta_theta_object']       = command_msg.control_command.delta_theta_object

    # absolute commands
    command_dict['theta']           = command_msg.control_command.theta
    command_dict['s_pivot']         = command_msg.control_command.s_pivot
    command_dict['s_hand']          = command_msg.control_command.s_hand

    command_dict['theta_relative']     = command_msg.control_command.theta_relative
    command_dict['theta_object']       = command_msg.control_command.theta_object

    #additional commands for jogging
    command_dict['delta_x']     = command_msg.control_command.delta_x
    command_dict['delta_y']     = command_msg.control_command.delta_y
    command_dict['delta_normal']     = command_msg.control_command.delta_normal
    command_dict['delta_tangential']     = command_msg.control_command.delta_tangential

    return command_dict

def command_dict_to_command_stamped(command_dict):

    command_msg = ControlCommandStamped()

    if command_dict is None:
        return command_msg

    if 'name' in command_dict:
        command_msg.control_command.name = command_dict['name']

    if 'command_flag' in command_dict:
        command_msg.control_command.command_flag = command_dict['command_flag']

    if 'mode' in command_dict:
        command_msg.control_command.mode = command_dict['mode']

    if   'delta_theta' in command_dict:
        command_msg.control_command.delta_theta = command_dict['delta_theta']
    elif 'theta' in command_dict:
        command_msg.control_command.theta = command_dict['theta']

    if   'delta_theta_relative' in command_dict:
        command_msg.control_command.delta_theta_relative = command_dict['delta_theta_relative']
    elif 'theta_relative' in command_dict:
        command_msg.control_command.theta_relative = command_dict['theta_relative']

    if   'delta_theta_object' in command_dict:
        command_msg.control_command.delta_theta_object = command_dict['delta_theta_object']
    elif 'theta_object' in command_dict:
        command_msg.control_command.theta_object = command_dict['theta_object']

    if   'delta_s_pivot' in command_dict:
        command_msg.control_command.delta_s_pivot = command_dict['delta_s_pivot']
    elif 's_pivot' in command_dict:
        command_msg.control_command.s_pivot = command_dict['s_pivot']

    if   'delta_s_hand' in command_dict:
        command_msg.control_command.delta_s_hand = command_dict['delta_s_hand']
    elif 's_hand' in command_dict:
        command_msg.control_command.s_hand = command_dict['s_hand']

    if   'delta_x' in command_dict:
        command_msg.control_command.delta_x = command_dict['delta_x']

    if   'delta_y' in command_dict:
        command_msg.control_command.delta_y = command_dict['delta_y']

    if   'delta_normal' in command_dict:
        command_msg.control_command.delta_normal = command_dict['delta_normal']

    if   'delta_tangential' in command_dict:
        command_msg.control_command.delta_tangential = command_dict['delta_tangential']

    return command_msg

def qp_debug_dict_to_qp_debug_stamped(qp_debug_dict):

    qp_debug_msg = QPDebugStamped()

    # primitive info
    qp_debug_msg.qp_debug.name = qp_debug_dict['name']
    qp_debug_msg.qp_debug.mode = qp_debug_dict['mode']

    # break up error dict by keys/vals
    error_dict_keys = qp_debug_dict['error_dict'].keys()
    qp_debug_msg.qp_debug.error_dict_keys = ','.join(error_dict_keys)
    qp_debug_msg.qp_debug.error_dict_vals = [
        qp_debug_dict['error_dict'][key] for key in error_dict_keys]

    qp_debug_msg.qp_debug.snewrb  = qp_debug_dict['snewrb']

    # QP costs
    qp_debug_msg.qp_debug.quadratic_cost_term = [
        item for row in qp_debug_dict['quadratic_cost_term'] for item in row]
    qp_debug_msg.qp_debug.linear_cost_term = qp_debug_dict['linear_cost_term']
    qp_debug_msg.qp_debug.proj_vec_list = [
        item for row in qp_debug_dict['proj_vec_list'] for item in row]
    qp_debug_msg.qp_debug.error_list = qp_debug_dict['error_list']

    # QP constraints
    qp_debug_msg.qp_debug.label_list_cnstr = ','.join(qp_debug_dict['label_list_cnstr'])
    qp_debug_msg.qp_debug.constraint_normals = [
        item for row in qp_debug_dict['constraint_normals'] for item in row]
    qp_debug_msg.qp_debug.constraint_offsets = qp_debug_dict['constraint_offsets']
    qp_debug_msg.qp_debug.measured_wrench = qp_debug_dict['measured_wrench']
    qp_debug_msg.qp_debug.slacks = qp_debug_dict['slacks']

    # Solution
    qp_debug_msg.qp_debug.solve_time = qp_debug_dict['solve_time']
    qp_debug_msg.qp_debug.delta_wrench = qp_debug_dict['delta_wrench']
    qp_debug_msg.qp_debug.delta_wrench_unconstrained = qp_debug_dict[
        'delta_wrench_unconstrained']

    return qp_debug_msg

def qp_debug_stamped_to_qp_debug_dict(qp_debug_msg):

    qp_dict = {}

    # primitive info
    qp_dict['name'] = qp_debug_msg.qp_debug.name
    qp_dict['mode'] = qp_debug_msg.qp_debug.mode

    # join dict back together
    error_dict_keys = qp_debug_msg.qp_debug.error_dict_keys.split(',')
    error_dict_vals = qp_debug_msg.qp_debug.error_dict_vals

    qp_dict['error_dict'] = {key : val
        for key, val in zip(error_dict_keys, error_dict_vals)}

    # QP costs
    qp_dict['quadratic_cost_term'] = np.reshape(np.array(
         qp_debug_msg.qp_debug.quadratic_cost_term), newshape=(-1, 3)).tolist()
    qp_dict['linear_cost_term'] = qp_debug_msg.qp_debug.linear_cost_term
    qp_dict['proj_vec_list'] = np.reshape(np.array(
         qp_debug_msg.qp_debug.proj_vec_list), newshape=(-1, 3)).tolist()
    qp_dict['error_list'] = qp_debug_msg.qp_debug.error_list

    # QP constraints
    qp_dict['label_list_cnstr'] = qp_debug_msg.qp_debug.label_list_cnstr.split(',')
    qp_dict['constraint_normals'] = np.reshape(np.array(
         qp_debug_msg.qp_debug.constraint_normals), newshape=(-1, 3)).tolist()
    qp_dict['constraint_offsets'] = qp_debug_msg.qp_debug.constraint_offsets
    qp_dict['measured_wrench'] = qp_debug_msg.qp_debug.measured_wrench
    qp_dict['slacks'] = qp_debug_msg.qp_debug.slacks

    # Solution
    qp_dict['solve_time'] = qp_debug_msg.qp_debug.solve_time
    qp_dict['delta_wrench'] = qp_debug_msg.qp_debug.delta_wrench
    qp_dict['delta_wrench_unconstrained'] = qp_debug_msg.qp_debug.delta_wrench_unconstrained

    return qp_dict

def ground_truth_stamped_to_ground_truth_dict(ground_truth_msg):

    ground_truth_dict = {}

    # robot kinematics
    ground_truth_dict['tht_h'] = ground_truth_msg.ground_truth.hand_angle
    ground_truth_dict['hp'] = ground_truth_msg.ground_truth.hand_pose
    ground_truth_dict['sq'] = ground_truth_msg.ground_truth.hand_sliding

    # object kinematics
    ground_truth_dict['tht_o'] = ground_truth_msg.ground_truth.object_angle
    ground_truth_dict['op'] = ground_truth_msg.ground_truth.object_pose
    ground_truth_dict['sg'] = ground_truth_msg.ground_truth.object_sliding
    ground_truth_dict['pivs'] = np.reshape(np.array(
        ground_truth_msg.ground_truth.pivot_locations),
        newshape=(-1, 2)).tolist()

    # ground
    ground_truth_dict['tht_g'] = ground_truth_msg.ground_truth.ground_angle

    # perception
    ground_truth_dict['sn'] = ground_truth_msg.ground_truth.shape_name
    ground_truth_dict['atid'] = ground_truth_msg.ground_truth.apriltag_id
    ground_truth_dict['od'] = ground_truth_msg.ground_truth.object_dectected

    return ground_truth_dict

def ground_truth_dict_to_ground_truth_stamped(ground_truth_dict):

    ground_truth_msg = GroundTruthStamped()

    # robot kinematics
    ground_truth_msg.ground_truth.hand_angle = ground_truth_dict['tht_h']
    ground_truth_msg.ground_truth.hand_pose = ground_truth_dict['hp']
    ground_truth_msg.ground_truth.hand_sliding = ground_truth_dict['sq']

    # object kinematics
    ground_truth_msg.ground_truth.object_angle = ground_truth_dict['tht_o']
    ground_truth_msg.ground_truth.object_pose = ground_truth_dict['op']
    ground_truth_msg.ground_truth.object_sliding = ground_truth_dict['sg']
    ground_truth_msg.ground_truth.pivot_locations = [
        item for row in ground_truth_dict['pivs'] for item in row]

    # ground
    ground_truth_msg.ground_truth.ground_angle = ground_truth_dict['tht_g']

    # perception
    ground_truth_msg.ground_truth.shape_name = ground_truth_dict['sn']
    ground_truth_msg.ground_truth.apriltag_id = ground_truth_dict['atid']
    ground_truth_msg.ground_truth.object_dectected = ground_truth_dict['od']

    return ground_truth_msg

def parse_apriltag_detection_array(apriltag_message):

    if len(apriltag_message.detections)>0:
        detection_dict = {}
        for i in range(len(apriltag_message.detections)):

            msg = apriltag_message.detections[i]

            detection_dict[msg.id[0]] = {
            'id': msg.id[0],
            'size': msg.size[0],
            'position': [msg.pose.pose.pose.position.x, msg.pose.pose.pose.position.y,
                msg.pose.pose.pose.position.z],
            'orientation': [msg.pose.pose.pose.orientation.x, msg.pose.pose.pose.orientation.y,
                msg.pose.pose.pose.orientation.z, msg.pose.pose.pose.orientation.w]
            }
        return detection_dict
    else:
        return None

def parse_camera_info(camera_info_message):
    camera_matrix = np.reshape(camera_info_message.P, (3, 4))
    return camera_matrix.tolist()


# Quest Pack 4 (1.0) by Windyplains

from header_game_menus import *
from header_parties import *
from header_items import *
from header_mission_templates import *
from header_music import *
from header_terrain_types import *

from module_constants import *

####################################################################################################################
#  (menu-id, menu-flags, menu_text, mesh-name, [<operations>], [<options>]),
#
#   Each game menu is a tuple that contains the following fields:
#
#  1) Game-menu id (string): used for referencing game-menus in other files.
#     The prefix menu_ is automatically added before each game-menu-id
#
#  2) Game-menu flags (int). See header_game_menus.py for a list of available flags.
#     You can also specify menu text color here, with the menu_text_color macro
#  3) Game-menu text (string).
#  4) mesh-name (string). Not currently used. Must be the string "none"
#  5) Operations block (list). A list of operations. See header_operations.py for reference.
#     The operations block is executed when the game menu is activated.
#  6) List of Menu options (List).
#     Each menu-option record is a tuple containing the following fields:
#   6.1) Menu-option-id (string) used for referencing game-menus in other files.
#        The prefix mno_ is automatically added before each menu-option.
#   6.2) Conditions block (list). This must be a valid operation block. See header_operations.py for reference.
#        The conditions are executed for each menu option to decide whether the option will be shown to the player or not.
#   6.3) Menu-option text (string).
#   6.4) Consequences block (list). This must be a valid operation block. See header_operations.py for reference.
#        The consequences are executed for the menu option that has been selected by the player.
#
#
# Note: The first Menu is the initial character creation menu.
####################################################################################################################

game_menus = [
	##### QUEST - ODVAL ACCEPT THE CHALLENGE #####
	("qp4_odval_challenge",0,
		"Trial of Arms^^You and {s14} have returned to the village of {s13} to stand against {s14}'s accusers in combat.  The two of you have discussed the best plan available for dealing with the three men and believe you have a good chance at success, but failure may cost {reg3?her:him} more than simply shame.  In the hay field to the left you can see a large crowd gathered.^^OBJECTIVE:^You and {s14} must face {reg3?her:his} accusers in a contest of martial arms.  {s15}^^WARNING:^You may want to ensure {s14}'s equipment is up to this challenge.",
		"none",
		[
			(check_quest_active, "qst_odval_accept_the_challenge"),
			(main_party_has_troop, NPC_Odval),
			(quest_slot_eq, "qst_odval_accept_the_challenge", slot_quest_current_state, qp4_odval_accept_the_challenge_challenge_accepted),
			(quest_get_slot, reg1, "qst_odval_accept_the_challenge", slot_quest_giver_center),
			(str_store_party_name, s13, reg1),
			(str_store_troop_name, s14, NPC_Odval),
			(troop_get_type, reg3, NPC_Odval),
			(try_begin),
				(eq, "$quest_reactions", QUEST_REACTIONS_HIGH),
				(str_store_string, s15, "@{s14} must survive this contest."),
			(else_try),
				(str_clear, s15),
			(try_end),
			# Name the village elder.
			# (party_get_slot, ":troop_village_elder", qp4_odval_home_town, slot_town_elder),
			# (str_store_troop_name, s21, ":troop_village_elder"),
		],
		[
			("odval_challenge_yes", [], "Agree to begin the match.",
				[
					(call_script, "script_common_quest_change_state", "qst_odval_redemption", qp4_odval_accept_the_challenge_challenge_begun),
					(party_get_slot, ":village_scene", "$current_town", slot_castle_exterior), # qp4_odval_home_town
					(modify_visitors_at_site, ":village_scene"),
					(reset_visitors),
					(set_jump_mission, "mt_odval_challenge"),
					(jump_to_scene, ":village_scene"),
					(change_screen_mission),
				]),

			("odval_challenge_wait", [], "Come back later.",
				[
					(jump_to_menu, "mnu_village"),
				]),

			("odval_challenge_no", [], "Forget this whole affair.",
				[
					(quest_set_slot, "qst_odval_accept_the_challenge", slot_quest_current_state, qp4_odval_accept_the_challenge_challenge_lost),
					(quest_set_slot, "qst_odval_return_to_tulbuk", slot_quest_current_state, qp4_odval_return_to_tulbuk_refused_challenge),
					(call_script, "script_qp4_quest_odval_accept_the_challenge", floris_quest_update),
					(call_script, "script_qp4_quest_odval_accept_the_challenge", floris_quest_fail),
					(jump_to_menu, "mnu_village"),
				]),
		]),

	##### QUEST - ODVAL SAVING FACE #####
	("qp4_odval_saving_face",0,
		"Duel in {s13}^^You and {s14} have returned to the village of {s13} to face one another in combat in order to prove {s14}'s ability as a warrior.  You can see that the villagers have gathered near the hay field again along with {s21}.^^OBJECTIVE:^Help clear {s14}'s reputation through a contest of arms.",
		"none",
		[
			(check_quest_active, "qst_odval_saving_face"),
			(main_party_has_troop, NPC_Odval),
			(quest_slot_eq, "qst_odval_saving_face", slot_quest_current_state, qp4_odval_saving_face_challenge_accepted),
			(quest_get_slot, reg1, "qst_odval_saving_face", slot_quest_giver_center),
			# Ensure a couple of days have lapsed.
			(store_current_hours, ":hours"),
			(neg|quest_slot_ge, "qst_odval_saving_face", slot_quest_target_amount, ":hours"),
			(str_store_party_name, s13, reg1),
			(str_store_troop_name, s14, NPC_Odval),
			(troop_get_type, reg3, NPC_Odval),
			# Name the village elder.
			(party_get_slot, ":troop_village_elder", qp4_odval_home_town, slot_town_elder),
			(str_store_troop_name, s21, ":troop_village_elder"),
		],
		[
			("odval_saving_yes", [], "Agree to begin the match.",
				[
					(call_script, "script_common_quest_change_state", "qst_odval_saving_face", qp4_odval_saving_face_challenge_begun),
					(troop_set_health, "trp_player", 100),
					(troop_set_health, NPC_Odval, 100),
					(party_get_slot, ":village_scene", qp4_odval_home_town, slot_castle_exterior),
					(modify_visitors_at_site, ":village_scene"),
					(reset_visitors),
					(set_jump_mission, "mt_odval_challenge"),
					(jump_to_scene, ":village_scene"),
					(change_screen_mission),
				]),

			("odval_saving_wait", [], "Come back later.",
				[
					(jump_to_menu, "mnu_village"),
				]),

			("odval_saving_no", [], "Forget this whole affair.",
				[
					(quest_set_slot, "qst_odval_saving_face", slot_quest_current_state, qp4_odval_saving_face_inactive),
					(call_script, "script_qp4_quest_odval_saving_face", floris_quest_update),
					(call_script, "script_qp4_quest_odval_saving_face", floris_quest_fail),
					(try_begin),
						(ge, "$quest_reactions", QUEST_REACTIONS_HIGH), # 3 successful quests required.
						(call_script, "script_qp4_quest_odval_redemption", floris_quest_fail),
					(else_try),
						(call_script, "script_qp4_quest_odval_redemption", floris_quest_succeed), # 2 of 2 required quests passed.
					(try_end),
					(jump_to_menu, "mnu_village"),
				]),
		]),

	##### QUEST - EDWYN FIRST KNIGHT #####
	("qp4_edwyn_knight_1",0,
		"You, {s14} and a few other companions sneak up to the edge of the bandit's encampment looking for any signs of the rogue knight.  Your patience pays off as a man in plate matching {s14}'s description of Sir Tenry comes into view giving orders to those around him.",
		"none",
		[
			(str_store_party_name, s13, reg1),
			(str_store_troop_name, s14, NPC_Edwyn),
			(troop_get_type, reg3, NPC_Edwyn),
		],
		[
			("edwyn_first_yes", [], "Begin the Assault...",
				[
					(call_script, "script_common_quest_change_state", "qst_edwyn_first_knight", qp4_edwyn_first_entered_lair),
					(party_set_slot, "$g_encountered_party", slot_party_ai_substate, 1),
					#(party_get_template_id, ":template", "$g_encountered_party"),
					(assign, "$g_enemy_party", "$g_encountered_party"),

					(try_begin),
					  # (eq, ":template", "pt_sea_raider_lair"),
					  # (assign, ":bandit_troop", "trp_sea_raider"),
					  # (assign, ":scene_to_use", "scn_lair_sea_raiders"),
					# (else_try),
					  # (eq, ":template", "pt_forest_bandit_lair"),
					  # (assign, ":bandit_troop", "trp_forest_bandit"),
					  # (assign, ":scene_to_use", "scn_lair_forest_bandits"),
					# (else_try),
					  # (eq, ":template", "pt_desert_bandit_lair"),
					  # (assign, ":bandit_troop", "trp_desert_bandit"),
					  # (assign, ":scene_to_use", "scn_lair_desert_bandits"),
					# (else_try),
					  # (eq, ":template", "pt_mountain_bandit_lair"),
					  # (assign, ":bandit_troop", "trp_mountain_bandit"),
					  # (assign, ":scene_to_use", "scn_lair_mountain_bandits"),
					# (else_try),
					  # (eq, ":template", "pt_taiga_bandit_lair"),
					  # (assign, ":bandit_troop", "trp_taiga_bandit"),
					  # (assign, ":scene_to_use", "scn_lair_taiga_bandits"),
					# (else_try),
					  # (eq, ":template", "pt_steppe_bandit_lair"),
					  (assign, ":bandit_troop", "trp_steppe_bandit"), # "trp_bandit_n_steppe"),
					  (assign, ":scene_to_use", "scn_lair_steppe_bandits"),
					(try_end),

					(modify_visitors_at_site,":scene_to_use"),
					(reset_visitors),

					(store_character_level, ":player_level", "trp_player"),
					(store_add, ":number_of_bandits_will_be_spawned_at_each_period", 5, ":player_level"),
					(val_div, ":number_of_bandits_will_be_spawned_at_each_period", 3),

					(try_for_range, ":unused", 0, ":number_of_bandits_will_be_spawned_at_each_period"),
					  (store_random_in_range, ":random_entry_point", 2, 11),
					  (set_visitor, ":random_entry_point", ":bandit_troop", 1),
					(try_end),

					(set_visitor, 5, qp4_actor_named_knight, 1), # Add Sir Tenry to the scene.
					(troop_set_name, qp4_actor_named_knight, "@Sir Tenry"),

					(party_clear, "p_temp_casualties"),

					(set_party_battle_mode),
					(set_battle_advantage, 0),
					(assign, "$g_battle_result", 0),
					(set_jump_mission,"mt_bandit_lair"),

					(jump_to_scene, ":scene_to_use"),
					(change_screen_mission),
				]),

			("edwyn_first_no", [], "Come back later.",
				[
					(jump_to_menu, "mnu_bandit_lair"),
				]),
		]),

	##### QUEST - EDWYN SECOND KNIGHT #####
	("qp4_edwyn_knight_2",0,
		"Skirmish in the Streets of {s17}^^You and {s14} keep an eye out for the knight as you pass through the streets of {s17}, but do not see any immediate sign of Sir Henric.  Possibly a more detailed search will yield better results.^^OBJECTIVE:^ Slay Sir Henric and escape.^^WARNING:^ You will want to have at least a few companions (level 10+) in your party prior to attempting this.",
		"none",
		[
			(str_store_troop_name, s14, NPC_Edwyn),
			(str_store_party_name, s17, "$current_town"),
			(set_background_mesh, "mesh_pic_town1"),
		],
		[
			("edwyn_find_henric_yes", [], "Take a walk around the streets.",
				[
					(try_begin),
					   (party_get_slot, ":town_scene", "$current_town", slot_town_center),
					   (modify_visitors_at_site, ":town_scene"),
					   (reset_visitors),
					   (assign, "$g_mt_mode", tcm_default),
					   (store_faction_of_party, ":town_faction","$current_town"),

					   (try_begin),
						 (neq, ":town_faction", "fac_player_supporters_faction"),
						 (faction_get_slot, ":troop_prison_guard", "$g_encountered_party_faction", slot_faction_prison_guard_troop),
						 #(faction_get_slot, ":troop_castle_guard", "$g_encountered_party_faction", slot_faction_castle_guard_troop),
						 (faction_get_slot, ":tier_2_troop", ":town_faction", slot_faction_tier_2_troop),
						 (faction_get_slot, ":tier_3_troop", ":town_faction", slot_faction_tier_3_troop),
					   (else_try),
						 (party_get_slot, ":town_original_faction", "$current_town", slot_center_original_faction),
						 (faction_get_slot, ":troop_prison_guard", ":town_original_faction", slot_faction_prison_guard_troop),
						 #(faction_get_slot, ":troop_castle_guard", ":town_original_faction", slot_faction_castle_guard_troop),
						 (faction_get_slot, ":tier_2_troop", ":town_original_faction", slot_faction_tier_2_troop),
						 (faction_get_slot, ":tier_3_troop", ":town_original_faction", slot_faction_tier_3_troop),
					   (try_end),
					   (set_visitor, 24, ":troop_prison_guard"),

					   (try_begin),
						 (gt,":tier_2_troop", 0),
						 (assign,reg0,":tier_3_troop"),
						 (assign,reg1,":tier_3_troop"),
						 (assign,reg2,":tier_2_troop"),
						 (assign,reg3,":tier_2_troop"),
					   (try_end),
					   (shuffle_range,0,4),

					   (party_get_slot, ":spawned_troop", "$current_town", slot_town_armorer),
					   (set_visitor, 9, ":spawned_troop"),
					   (party_get_slot, ":spawned_troop", "$current_town", slot_town_weaponsmith),
					   (set_visitor, 10, ":spawned_troop"),
					   (party_get_slot, ":spawned_troop", "$current_town", slot_town_elder),
					   (set_visitor, 11, ":spawned_troop"),
					   (party_get_slot, ":spawned_troop", "$current_town", slot_town_horse_merchant),
					   (set_visitor, 12, ":spawned_troop"),
					   (call_script, "script_init_town_walkers"),
					   (set_jump_mission,"mt_edwyn_town_fight"),
					   (assign, ":override_state", af_override_horse),
					   (try_begin),
						 (eq, "$sneaked_into_town", 1), #setup disguise
						 (assign, ":override_state", af_override_all),
					   (try_end),
					   (mission_tpl_entry_set_override_flags, "mt_town_center", 0, ":override_state"),
					   (mission_tpl_entry_set_override_flags, "mt_town_center", 2, ":override_state"),
					   (mission_tpl_entry_set_override_flags, "mt_town_center", 3, ":override_state"),
					   (mission_tpl_entry_set_override_flags, "mt_town_center", 4, ":override_state"),
					   (mission_tpl_entry_set_override_flags, "mt_town_center", 5, ":override_state"),
					   (mission_tpl_entry_set_override_flags, "mt_town_center", 6, ":override_state"),
					   (mission_tpl_entry_set_override_flags, "mt_town_center", 7, ":override_state"),
					   (try_begin),
						 (eq, "$town_entered", 0),
						 (assign, "$town_entered", 1),
						 (eq, "$town_nighttime", 0),
						 (set_jump_entry, 1),
					   (try_end),
					   (jump_to_scene, ":town_scene"),
					   (change_screen_mission),
					 (try_end),
				]),

			("edwyn_find_henric_no", [], "Come back to look later.", [(jump_to_menu, "mnu_town"),]),
		]),

	##### QUEST - EDWYN THIRD KNIGHT #####
	("qp4_edwyn_third_knight",0,
		"Battle of {s13}^^You and {s14} have returned to the village of {s13} to take Sir Gerrin's forces by surprise and see him slain.  Ahead you can hear the sounds of a battle being waged.^^OBJECTIVE:^Slay Sir Gerrin and protect the villagers.^^WARNING:^You will want to have a reasonable sized host of at least thirty men to face Sir Gerrin's forces.",
		"none",
		[
			(str_store_party_name, s13, "$current_town"),
			(str_store_troop_name, s14, NPC_Edwyn),
			(troop_get_type, reg3, NPC_Edwyn),
			(set_background_mesh, "mesh_pic_villageriot"),
		],
		[
			("edwyn_third_yes", [], "Attack Sir Gerrin's forces.",
				[
					# Fill the player friend party.
					(call_script, "script_party_copy", "p_collective_friends", "p_main_party"),
					(call_script, "script_party_copy", "p_collective_friends_backup", "p_collective_friends"),
					# Fill the enemy friend party.
					(party_clear, "p_collective_enemy"),
					(party_add_members, "p_collective_enemy", qp4_actor_named_knight, 1),
					(party_add_members, "p_collective_enemy", qp4_actor_knight, 2),
					(party_add_members, "p_collective_enemy", qp4_actor_knight_support, 12),
					(assign, "$g_enemy_party", "p_collective_enemy"),

					(party_get_slot, ":scene_to_use", "$current_town", slot_castle_exterior),
					(modify_visitors_at_site,":scene_to_use"),
					(reset_visitors),
					(set_visitors, 0, qp4_actor_knight, 2),
					(set_visitors, 0, qp4_actor_knight_support, 12),
					(set_visitors, 0, qp4_actor_named_knight, 1),
					(troop_set_name, qp4_actor_named_knight, "@Sir Gerrin"),
					(party_get_slot, ":village_elder_troop", "$current_town",slot_town_elder),
					(set_visitor, 11, ":village_elder_troop"),

					##Floris MTT begin
					#(troop_get_slot,":mercenary_farmer", "$troop_trees", slot_mercenary_farmer),
					(store_faction_of_party, ":faction_no", "$current_town"),
					(faction_get_slot, ":culture", ":faction_no",  slot_faction_culture),
					(faction_get_slot, ":walker", ":culture",  slot_faction_village_walker_male_troop),
					(set_visitors, 2, ":walker", qp4_villagers_present_for_fight),
					##Floris MTT end
					(set_party_battle_mode),
					(set_battle_advantage, 0),
					(assign, "$g_battle_result", 0),
					(set_jump_mission,"mt_edwyn_village_fight"),
					(jump_to_scene, ":scene_to_use"),
					# (assign, "$g_next_menu", "mnu_village"),
					# (jump_to_menu, "mnu_battle_debrief"),
					# (assign, "$g_mt_mode", vba_normal),
					(change_screen_mission),
				]),

			("edwyn_third_no", [], "Come back later.",
				[
					(jump_to_menu, "mnu_village"),
				]),
		]),

 ]

from util_common import *
from util_wrappers import *

def modmerge_game_menus(orig_game_menus, check_duplicates = False):
    if( not check_duplicates ):
        orig_game_menus.extend(game_menus) # Use this only if there are no replacements (i.e. no duplicated item names)
    else:
    # Use the following loop to replace existing entries with same id
        for i in range (0,len(game_menus)-1):
          find_index = find_object(orig_game_menus, game_menus[i][0]); # find_object is from header_common.py
          if( find_index == -1 ):
            orig_game_menus.append(game_menus[i])
          else:
            orig_game_menus[find_index] = game_menus[i]

	# QUEST: ODVAL_ACCEPT_THE_CHALLENGE (village)
    find_index = find_object(orig_game_menus, "village")
    orig_game_menus[find_index][5].insert(3,
            ("qp4_odval_contest_1",[
			(party_slot_eq, "$current_town", slot_village_infested_by_bandits, 0),
	        (party_slot_eq, "$current_town", slot_village_state, svs_normal),
			(check_quest_active, "qst_odval_accept_the_challenge"),
			(quest_slot_eq, "qst_odval_accept_the_challenge", slot_quest_current_state, qp4_odval_accept_the_challenge_challenge_accepted),
			(quest_slot_eq, "qst_odval_accept_the_challenge", slot_quest_target_amount, 0),
			(eq, qp4_odval_home_town, "$current_town"),
			(str_store_party_name, s13, qp4_odval_home_town),],"Return to {s13} for contest. (Story Quest)", [(jump_to_menu, "mnu_qp4_odval_challenge"),]),
          )

	# QUEST: ODVAL_SAVING_FACE (village)
    find_index = find_object(orig_game_menus, "village")
    orig_game_menus[find_index][5].insert(3,
            ("qp4_odval_contest_2",[
			(party_slot_eq, "$current_town", slot_village_infested_by_bandits, 0),
	        (party_slot_eq, "$current_town", slot_village_state, svs_normal),
			(check_quest_active, "qst_odval_saving_face"),
			(quest_slot_eq, "qst_odval_saving_face", slot_quest_current_state, qp4_odval_saving_face_challenge_accepted),
			(neg|quest_slot_ge, "qst_odval_saving_face", slot_quest_target_amount, 4),
			(eq, qp4_odval_home_town, "$current_town"),
			(str_store_troop_name, s14, NPC_Odval),],"Return to fight {s14}. (Story Quest)", [(jump_to_menu, "mnu_qp4_odval_saving_face"),]),
          )

	# QUEST: EDWYN_FIRST_KNIGHT (bandit lair)
    find_index = find_object(orig_game_menus, "bandit_lair")
    orig_game_menus[find_index][5].insert(1,
            ("qp4_edwyn_knight_1",
			[
				(check_quest_active, "qst_edwyn_first_knight"),
				(main_party_has_troop, NPC_Edwyn),
				(quest_slot_eq, "qst_edwyn_first_knight", slot_quest_current_state, qp4_edwyn_first_found_lair_on_map),
				(quest_slot_eq, "qst_edwyn_first_knight", slot_quest_stage_2_trigger_chance, "$g_encountered_party"),
			],"Investigate the Lair. (Story Quest)", [(jump_to_menu, "mnu_qp4_edwyn_knight_1"),]),
          )

	# QUEST: EDWYN_SECOND_KNIGHT (town)
    find_index = find_object(orig_game_menus, "town")
    orig_game_menus[find_index][5].insert(9,
            ("qp4_edwyn_knight_2",
			[
				(check_quest_active, "qst_edwyn_second_knight"),
				(main_party_has_troop, NPC_Edwyn),
				(quest_slot_eq, "qst_edwyn_second_knight", slot_quest_current_state, qp4_edwyn_second_arrived_in_town),
				(quest_slot_eq, "qst_edwyn_second_knight", slot_quest_stage_2_trigger_chance, "$current_town"),
			],"Search for Sir Henric in the streets. (Story Quest)", [(jump_to_menu, "mnu_qp4_edwyn_knight_2"),]),
          )

	# QUEST: EDWYN_THIRD_KNIGHT (village)
    find_index = find_object(orig_game_menus, "village")
    orig_game_menus[find_index][5].insert(3,
            ("qp4_edwyn_knight_3",[
				(party_slot_eq, "$current_town", slot_village_infested_by_bandits, 0),
				(party_slot_eq, "$current_town", slot_village_state, svs_normal),
				(check_quest_active, "qst_edwyn_third_knight"),
				(quest_slot_eq, "qst_edwyn_third_knight", slot_quest_current_state, qp4_edwyn_third_planning_to_kill_knight),
				(quest_slot_eq, "qst_edwyn_third_knight", slot_quest_stage_2_trigger_chance, "$current_town"),
			],"Attack Sir Gerrin's forces. (Story Quest)", [(jump_to_menu, "mnu_qp4_edwyn_third_knight"),]),
          )

# Used by modmerger framework version >= 200 to merge stuff
def modmerge(var_set):
    try:
        var_name_1 = "game_menus"
        orig_game_menus = var_set[var_name_1]
        modmerge_game_menus(orig_game_menus)
        # find_i = list_find_first_match_i( orig_game_menus, "town_trade_assessment" )
        # codeblock = GameMenuWrapper(orig_game_menus[find_i]).GetOpBlock()
        # line_i = codeblock.GetLength() - 2
        # codeblock.InsertBefore(line_i, hook_assessment_quest_initiation)
    except KeyError:
        errstring = "Variable set does not contain expected variable: \"%s\"." % var_name_1
        raise ValueError(errstring)

Source - Quest Utilities/Quest Pack 4 (Companions)/questpack4_game_menus.py