import time
from collections import namedtuple
from functools import lru_cache

data = '''
The first floor contains a polonium generator, a thulium generator, a thulium-compatible microchip, a promethium generator, a ruthenium generator, a ruthenium-compatible microchip, a cobalt generator, and a cobalt-compatible microchip.
The second floor contains a polonium-compatible microchip and a promethium-compatible microchip.
The third floor contains nothing relevant.
The fourth floor contains nothing relevant.
'''.strip()

data = '''
The first floor contains a hydrogen-compatible microchip and a lithium-compatible microchip.
The second floor contains a hydrogen generator.
The third floor contains a lithium generator.
The fourth floor contains nothing relevant.
'''.strip()

items = {
    'H-C': 1,
    'L-C': 1,
    'H-G': 2,
    'L-G': 3,
}

items = {
    'PL-G': 1,
    'TH-G': 1,
    'TH-C': 1,
    'PR-G': 1,
    'RU-G': 1,
    'RU-C': 1,
    'CO-G': 1,
    'CO-C': 1,
    'PL-C': 1,
    'PR-C': 1,
}

target = {
    'PL-G': 4,
    'TH-G': 4,
    'TH-C': 4,
    'PR-G': 4,
    'RU-G': 4,
    'RU-C': 4,
    'CO-G': 4,
    'CO-C': 4,
    'PL-C': 4,
    'PR-C': 4,
}

def floor_to_text(floors, cur_floor=1):
    text = []
    all_items = sum(floors, ())
    for floor_num, floor in enumerate(floors, start=1):
        if floor_num == cur_floor:
            elevator = '\x1b[;41m{}\x1b[0m'.format(floor_num)
        else:
            elevator = floor_num
        thing = ' '.join('{: ^{}}'.format((item if item in floor else '.'), len(all_items[0])) for item in all_items)
        text.insert(0, '{} {}'.format(elevator, thing))

    return '\n'.join(text)

def colorize(item, items):
    stuff_on_floor = [i for i in items if items[i] == items[item] and i != item]
    if item.endswith('-C') and stuff_on_floor and not item.replace('-C', '-G') in stuff_on_floor:
        return '\x1b[;31m{}\x1b[0m'.format(item)
    elif item.endswith('-G') and any(chip.replace('-C', '-G') not in stuff_on_floor for chip in stuff_on_floor if chip.endswith('-C')):
        return '\x1b[;31m{}\x1b[0m'.format(item)
    elif can_go_up(item, items):
        return '\x1b[;32m{}\x1b[0m'.format(item)
    else:
        return '\x1b[;33m{}\x1b[0m'.format(item)

def can_go_up(item, items):
    next_floor = items[item]+1
    stuff_on_floor = [i for i in items if items[i] == items[item]]
    stuff_on_next_floor = [i for i in items if items[i] == next_floor]
    if not stuff_on_next_floor:
        return True
    elif ((item.endswith('-C') and item.replace('-C', '-G') in stuff_on_floor)
            or item.endswith('-G') and item.replace('-G', '-C') in stuff_on_floor):
        return True
    elif item.endswith('-C') and item.replace('-C', '-G') in stuff_on_next_floor:
        return True
    return False

def fried(floors):
    for floor in floors:
        for chip in (chip for chip in floor if chip.endswith('-C')):
            if chip.replace('-C', '-G') not in floor and any(thing.endswith('-G') for thing in floor):
                return True

def to_dict(floors):
    items = {}
    for num, floor in enumerate(floors, 1):
        for item in floor:
            items[item] = num
    return items

def to_tuple(items):
    floors = []
    for i in range(4):
        floor_num = i+1
        floors.append(tuple(sorted(item for item in items if items[item] == floor_num)))
    return tuple(floors)

def possible_moves(item, floors, floor_limit=4):
    items = to_dict(floors)
    elevator = items[item]
    moves = []

    other_items = [i for i in items if i != item and items[item] == items[i]]

    next_floors = []
    if elevator < floor_limit:
        next_floors.append(elevator+1)
    if elevator > 1:
        next_floors.append(elevator-1)

    for next_floor in next_floors:
        for paired_item in other_items:
            if items[paired_item] == items[item]:
                items[paired_item] = next_floor
                items[item] = next_floor
                moves.append((next_floor, to_tuple(items)))
                items[paired_item] = elevator
                items[item] = elevator
            items[item] = next_floor
            moves.append((next_floor, to_tuple(items)))
            items[item] = elevator

    moves = [move for move in moves if not fried(move[1])]
    return moves

Move = namedtuple('Move', ('from_', 'to'))

def find_shortest(floors, elevator=1, steps=0, seen=None):
    start = (elevator, floors)
    states = {
        (elevator, floors): set(sum((possible_moves(item, floors, floor_limit=4) for item in floors[elevator-1]), []))
    }
    traversed_edges = set()
    for dest in states[(elevator, floors)]:
        states[dest] = None
        traversed_edges.add(Move(from_=(elevator, floors), to=dest))

    while any(states[state] is None for state in states):
        state = next(state for state in states if states[state] is None)
        state_floor = state[0]-1
        states[state] = states.get(state) or set()
        for item in state[1][state_floor]:
            for move in possible_moves(item, state[1], floor_limit=4):
                states[state].add(move)
                transition = Move(from_=state, to=move)
                traversed_edges.add(transition)
                states[move] = states.get(move, None)

    distances = {state:float('inf') for state in states}
    distances[start] = 0
    unvisited = set(distances)

    while unvisited:
        current = min((distances[state], state) for state in unvisited)[1]
        unvisited.remove(current)
        dist = distances[current]+1
        for neighbor in states[current]:
            distances[neighbor] = min(distances[neighbor], dist)

    for thing in distances:
        if not any(thing[1][:3]):
            print(thing)
    return distances

if __name__ == '__main__':
    target = (
        (),
        (),
        (),
        ('PL-G', 'TH-G', 'TH-C', 'PR-G', 'RU-G',
         'RU-C', 'CO-G', 'CO-C', 'PL-C', 'PR-C',
         ),
    )
    assert fried([['H-C', 'F-G'],])
    assert not fried((['H-C', 'H-G'],))
    assert fried((['H-C', 'H-G', 'F-C'],))
    assert not fried((['H-C', 'H-G', 'F-C', 'F-G'],))
    assert not fried((['H-G',  'F-G'], ['H-C', 'F-C']))

    items = {
        'H-C': 1,
        'L-C': 1,
        'H-G': 2,
        'L-G': 3,
    }

    items = {
        'PL-G': 1,
        'TH-G': 1,
        'TH-C': 1,
        'PR-G': 1,
        'RU-G': 1,
        'RU-C': 1,
        'CO-G': 1,
        'CO-C': 1,
        'PL-C': 2,
        'PR-C': 2,
    }
    target = {
        'PL-G': 4,
        'TH-G': 4,
        'TH-C': 4,
        'PR-G': 4,
        'RU-G': 4,
        'RU-C': 4,
        'CO-G': 4,
        'CO-C': 4,
        'PL-C': 4,
        'PR-C': 4,
    }

    initial_state = (
        ('H-C', 'L-C'),
        ('H-G',),
        ('L-G',),
        (),
    )
    target = (
        (), (), (),
        tuple(sorted(('H-C', 'L-C', 'H-G', 'L-G')))
    )

    assert to_dict(initial_state) == to_dict(initial_state)
    assert to_tuple(to_dict(initial_state)) == initial_state

    valid_moves_hc = [
        (2, (('L-C',),
             ('H-C', 'H-G'),
             ('L-G',),
             (),
             )
         ),
    ]
    assert possible_moves('H-C', initial_state, floor_limit=4) == valid_moves_hc
    assert possible_moves('L-C', initial_state, floor_limit=4) == []

    assert possible_moves('H-G', (('L-C',), ('H-C', 'H-G'), ('L-G',), ())) == [(3, (('L-C',), (), ('H-C', 'H-G', 'L-G'), ())),
                                                                               (3, (('L-C',), ('H-C',), ('H-G', 'L-G'), ())),
                                                                               ]

    initial_state = (
        ('H-C', 'L-C'),
        ('H-G',),
        ('L-G',),
        (),
    )
    target = (4, (
        (), (), (),
        tuple(sorted(('H-C', 'L-C', 'H-G', 'L-G')))
    ))

    print(find_shortest(initial_state, elevator=1)[target]-2)

    initial_state = (
        ('PL-G',
         'TH-G',
         'TH-C',
         'PR-G',
         'RU-G',
         'RU-C',
         'CO-G',
         'CO-C',
         ),
        ('PL-C',
         'PR-C',
         ),
         (),(),
    )
    target = (4, ((), (), (),
                  ('PL-G', 'TH-G', 'TH-C', 'PR-G', 'RU-G',
                   'RU-C', 'CO-G', 'CO-C', 'PL-C', 'PR-C',
                   )))
    print(find_shortest(initial_state, elevator=1)[target]-2)


import numpy as np
import torch
import dgl
from dgl import DGLGraph
from rdkit import Chem
from rdkit.Chem import rdMolDescriptors as rdDesc

#辅助函数
def one_of_k_encoding_unk(x, allowable_set):
    '将x与allowable_set逐个比较，相同为True， 不同为False, 都不同则认为是最后一个相同'
    if x not in allowable_set:
        x = allowable_set[-1]
    return list(map(lambda s: x == s, allowable_set))

def get_atom_features(atom):
    possible_atom = ['C', 'N', 'O', 'F', 'P', 'Cl', 'Br', 'I', 'H', 'S', 'Fe', 'Mg', 'B', 'DU'] #DU代表其他原子
    atom_features = one_of_k_encoding_unk(atom.GetSymbol(), possible_atom)
    atom_features += one_of_k_encoding_unk(atom.GetImplicitValence(), [0, 1])
    atom_features += one_of_k_encoding_unk(atom.GetNumRadicalElectrons(), [0, 1])
    atom_features += one_of_k_encoding_unk(atom.GetDegree(), [0, 1, 2, 3, 4, 5, 6])
    atom_features += one_of_k_encoding_unk(atom.GetFormalCharge(), [-1, 1])
    atom_features += one_of_k_encoding_unk(atom.GetHybridization(),
                                           [Chem.rdchem.HybridizationType.SP,
                                            Chem.rdchem.HybridizationType.SP2,
                                            Chem.rdchem.HybridizationType.SP3,
                                            Chem.rdchem.HybridizationType.SP3D])
    return np.array(atom_features)

def get_bond_features(bond):
    bond_type = bond.GetBondType()
    bond_feats = [
        bond_type == Chem.rdchem.BondType.SINGLE, bond_type == Chem.rdchem.BondType.DOUBLE,
        bond_type == Chem.rdchem.BondType.TRIPLE, bond_type == Chem.rdchem.BondType.AROMATIC,
        bond.GetIsConjugated(),
        bond.IsInRing()
    ]
    return np.array(bond_feats)

def gen_graph(molecule_smiles):
    G = DGLGraph()
    #加载smile生成mol对象
    molecule = Chem.MolFromSmiles(molecule_smiles)

    G.add_nodes(molecule.GetNumAtoms())

    node_features = []
    edge_features = []

    for i in range(molecule.GetNumAtoms()):
        atom_i = molecule.GetAtomWithIdx(i)
        atom_i_features = get_atom_features(atom_i)
        node_features.append(atom_i_features)

        for j in range(molecule.GetNumAtoms()):
            bond_ij = molecule.GetBondBetweenAtoms(i, j)
            if bond_ij is not None:
                G.add_edges(i,j)
                bond_features_ij = get_bond_features(bond_ij)
                edge_features.append(bond_features_ij)

    G.ndata['x'] = torch.from_numpy(np.array(node_features)) #dgl添加原子/节点特征
    G.edata['w'] = torch.from_numpy(np.array(edge_features)) #dgl添加键/边特征

    return G

from collections import namedtuple
from  rdkit  import  Chem
from  rdkit.Chem  import  Draw
from  rdkit.Chem.Draw  import  IPythonConsole
from  rdkit.Chem  import  rdChemReactions
import  rdkit
from rdkit.Chem import AllChem
#from PIL.Image import Image

AtomInfo = namedtuple('AtomInfo',('mapnum','reactant','reactantAtom','product','productAtom'))
def map_reacting_atoms_to_products(rxn,reactingAtoms):
    ''' figures out which atoms in the products each mapped atom in the reactants maps to '''
    res = []
    for ridx,reacting in enumerate(reactingAtoms):
        reactant = rxn.GetReactantTemplate(ridx)
        for raidx in reacting:
            mapnum = reactant.GetAtomWithIdx(raidx).GetAtomMapNum()
            foundit=False
            for pidx,product in enumerate(rxn.GetProducts()):
                for paidx,patom in enumerate(product.GetAtoms()):
                    if patom.GetAtomMapNum()==mapnum:
                        res.append(AtomInfo(mapnum,ridx,raidx,pidx,paidx))
                        foundit = True
                        break
                    if foundit:
                        break
    return res
def get_mapped_neighbors(atom):
    ''' test all mapped neighbors of a mapped atom'''
    res = {}
    amap = atom.GetAtomMapNum()
    if not amap:
        return res
    for nbr in atom.GetNeighbors():
        nmap = nbr.GetAtomMapNum()
        if nmap:
            if amap>nmap:
                res[(nmap,amap)] = (atom.GetIdx(),nbr.GetIdx())
            else:
                res[(amap,nmap)] = (nbr.GetIdx(),atom.GetIdx())
    return res

BondInfo = namedtuple('BondInfo',('product','productAtoms','productBond','status'))
def find_modifications_in_products(rxn):
    ''' returns a 2-tuple with the modified atoms and bonds from the reaction '''
    reactingAtoms = rxn.GetReactingAtoms()
    amap = map_reacting_atoms_to_products(rxn,reactingAtoms)
    res = []
    seen = set()
    # this is all driven from the list of reacting atoms:
    for _,ridx,raidx,pidx,paidx in amap:
        reactant = rxn.GetReactantTemplate(ridx)
        ratom = reactant.GetAtomWithIdx(raidx)
        product = rxn.GetProductTemplate(pidx)
        patom = product.GetAtomWithIdx(paidx)

        rnbrs = get_mapped_neighbors(ratom)
        pnbrs = get_mapped_neighbors(patom)
        for tpl in pnbrs:
            pbond = product.GetBondBetweenAtoms(*pnbrs[tpl])
            if (pidx,pbond.GetIdx()) in seen:
                continue
            seen.add((pidx,pbond.GetIdx()))
            if not tpl in rnbrs:
                # new bond in product
                res.append(BondInfo(pidx,pnbrs[tpl],pbond.GetIdx(),'New'))
            else:
                # present in both reactants and products, check to see if it changed
                rbond = reactant.GetBondBetweenAtoms(*rnbrs[tpl])
                if rbond.GetBondType()!=pbond.GetBondType():
                    res.append(BondInfo(pidx,pnbrs[tpl],pbond.GetIdx(),'Changed'))
    return amap,res

def gen_graph_with_broken_bonds(molecule_smiles, reaction, template_label):
    rxn = rdChemReactions.ReactionFromSmarts(reaction)
    rxn.Initialize()
    atms, bnds = find_modifications_in_products(rxn)

    G = DGLGraph()
    #加载smile生成mol对象
    molecule = Chem.MolFromSmiles(molecule_smiles)

    G.add_nodes(molecule.GetNumAtoms())

    node_features = []
    edge_features = []
    edge_labels = []

    for i in range(molecule.GetNumAtoms()):
        atom_i = molecule.GetAtomWithIdx(i)
        atom_i_features = get_atom_features(atom_i)
        node_features.append(atom_i_features)

        for j in range(molecule.GetNumAtoms()):
            bond_ij = molecule.GetBondBetweenAtoms(i, j)
            if bond_ij is not None:
                G.add_edges(i,j)
                bond_features_ij = get_bond_features(bond_ij)
                edge_features.append(bond_features_ij)
                l = 0
                for b in bnds:
                    if (b.productAtoms[0] == i and b.productAtoms[1] == j) or (b.productAtoms[1] == i and b.productAtoms[0] == j):
                        l = template_label + 1
                edge_labels.append(l)

    G.ndata['x'] = torch.from_numpy(np.array(node_features)) #dgl添加原子/节点特征
    G.edata['w'] = torch.from_numpy(np.array(edge_features)) #dgl添加键/边特征
    G.edata['l'] = torch.from_numpy(np.array(edge_labels)) #dgl添加键/边特征

    return G


from pyrogram import filters
from pyrogram import __version__ as pyro_vr
from pyrogram.types import InlineKeyboardButton, InlineKeyboardMarkup, Message

from .. import (Venom1, Venom2, Venom3, Venom4,
                Venom5, Venom6, Venom7, Venom8,
                Venom9, Venom10, Venom11, Venom12,
                Venom13, Venom14, Venom15, HNDLR,
                SUDO_USERS, vcbot, ALIVE_PIC, __version__)

Array = ALIVE_PIC or "https://te.legra.ph/file/045d419efd37fd56e8562.jpg"

@vcbot.on_message(filters.user(SUDO_USERS) & filters.command(["alive"], prefixes=HNDLR))
async def _Alive(_, e: Message):
    ids = 0
    try:
        if Venom1:
            ids += 1
        if Venom2:
            ids += 1
        if Venom3:
            ids += 1
        if Venom4:
            ids += 1
        if Venom5:
            ids += 1
        if Venom6:
            ids += 1
        if Venom7:
            ids += 1
        if Venom8:
            ids += 1
        if Venom9:
            ids += 1
        if Venom10:
            ids += 1
        if Venom11:
            ids += 1
        if Venom12:
            ids += 1
        if Venom13:
            ids += 1
        if Venom14:
            ids += 1
        if Venom15:
            ids += 1
        Array_msg = f"𝗧𝗥𝗢𝗡𝗜𝗫 𝗥𝗔𝗜𝗗𝗘𝗥 𝗛𝗘𝗥𝗘. 🔥 \n\n"
        Array_msg += f"◈ ━━━━━━ ◆ ━━━━━━ ◈ \n"
        Array_msg += f"► Tʀᴏɴɪx ᴠᴇʀsɪᴏɴ : `{__version__}` \n"
        Array_msg += f"► Aᴄᴛɪᴠᴇ IDs : `{ids}` \n"
        Array_msg += f"► ᴘʏʀᴏ ᴠᴇʀsɪᴏɴ : `{pyro_vr}` \n"
        Array_msg += f"◈ ━━━━━━ ◆ ━━━━━━ ◈ \n\n"
        await e.reply_photo(
        photo=Array,
        caption=Array_msg,
        reply_markup=InlineKeyboardMarkup(
                [[
                    InlineKeyboardButton(
                        "• Contact •", url="https://t.me/P_4_PEEYUSH")
                ], [
                    InlineKeyboardButton(
                        "• Repo •", url="tg://settings")
                ]],
        ),
    )
    except Exception as lol:
        Array_msg = f"𝗧𝗥𝗢𝗡𝗜𝗫 𝗥𝗔𝗜𝗗𝗘𝗥 𝗛𝗘𝗥𝗘. 🔥 \n\n"
        Array_msg += f"◈ ━━━━━━ ◆ ━━━━━━ ◈ \n"
        Array_msg += f"► Tʀᴏɴɪx ᴠᴇʀsɪᴏɴ : `{__version__}` \n"
        Array_msg += f"► ᴘʏʀᴏ ᴠᴇʀsɪᴏɴ : `{pyro_vr}` \n"
        Array_msg += f"► Sᴜᴘᴘᴏʀᴛ : [Jᴏɪɴ.](tg://settings)\n"
        Array_msg += f"◈ ━━━━━━ ◆ ━━━━━━ ◈ \n\n"
        await e.reply_photo(
        photo=Array,
        caption=Array_msg,
        reply_markup=InlineKeyboardMarkup(
            [
                [
                    InlineKeyboardButton("• Contact •", url="https://t.me/P_4_PEEYUSH"),
                ],
                [
                    InlineKeyboardButton("• Repo •", url="tg://settings"),
                ],
            ],
        ),
    )


#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Thu Jul 23 15:39:18 2020

@author: yochai_yemini
"""

import torch
import numpy as np
from collections import OrderedDict

eps=2.2204*np.exp(-16)

def stft(z, K, overlap, spec_mode='abs'):
    if spec_mode not in ['abs', 'log']:
        raise ValueError("spec_mode must be one of ")
    sub_num = 1 / (1 - overlap) - 1
    SEG_NO = np.fix(len(z) / (K * (1 - overlap))) - sub_num
    Z = np.zeros((int(K / 2 + 1), int(SEG_NO)))
    P = np.zeros((int(K / 2 + 1),int(SEG_NO)))
    for seg in np.arange(1, SEG_NO + 1):
        time_cal = np.arange((seg - 1) * K * (1 - overlap) + 1,
                             (seg - 1) * K * (1 - overlap) + K + 1) - 1
        time_cal = time_cal.astype('int')
        V = np.fft.fft(z[time_cal] * np.append(np.hanning(K - 1), 0))
        time_freq = np.arange(1, K / 2 + 1 + 1) - 1
        time_freq = time_freq.astype('int')
        P[:,int(seg-1)] = np.angle(V[time_freq])
        Z[:, int(seg - 1)] = np.abs(V[time_freq])

    if spec_mode == 'abs':
        return Z, P
    elif spec_mode == 'log':
        return np.log(Z + eps), P

def istft(A, P, synt_win):
    '''
    Returns the ISTFT of a FREQ x TIME signal
    param: A is the log-magnitude of the STFT, dimensions are FREQ x TIME.
    param: P is the phase is the phase of the STFT, dimensions are FREQ X TIME.
    param: synt_win is the synthesis window of length FREQ.
    '''
    # s_est=np.zeros((len(z),1))
    K = 512
    overlap = 0.75

    #Switch back from log-magnitude to magnitude
    A = np.exp(A)
    A[0:3] *= 0.001

    # Create the a KxTIME STFT from the (K/2+1)xTIME STFT
    A_inv, P_inv = A[::-1], P[::-1]
    A_inv, P_inv = A_inv[1:-1], P_inv[1:-1]
    P_full = np.concatenate([P, -P_inv], 0)
    A_full = np.concatenate([A, A_inv], 0)
    # A_full, P_full = A_full.numpy(), P_full.numpy()

    # Attach the phase with the magnitude
    S = A_full * np.exp(1j*P_full)
    S = np.real(np.fft.ifft(S, axis=0))
    S = synt_win * S

    SEG_NO = S.shape[1]
    long = 0.008*16000*SEG_NO + K
    long = int(long)
    s_est = np.zeros((long,1))
    for seg in np.arange(1,SEG_NO+1):
        time_cal = np.arange((seg-1)*K*(1-overlap)+1,(seg-1)*K*(1-overlap)+K+1)-1
        time_cal = time_cal.astype('int64')
        s_est[time_cal,:] = s_est[time_cal,:] + np.expand_dims(S[:, seg-1], axis=1)

    return s_est.squeeze()

def normalize_mc(z, desired_rms = 0.1, eps = 1e-4):
    '''
    Parameters
    ----------
    samples : numpy array comprising the multichannel reverberated speech
    desired_rms : desired rms after notmalisation. The default is 0.1.
                  rms is calculated over all samples and across channels.
    eps : The default is 1e-4.

    Returns
    -------
    samples : the normalised multichannel signals.

    '''
    if len(z) == 1:
        z = z/1.1/np.max(np.abs(z))
    else:
        rms = np.maximum(eps, np.sqrt(np.mean(z**2)))
        z = z * (desired_rms / rms)
    return z

def update_max(max_val, S):
    poten_max = np.max(S)
    if poten_max > max_val:
        return poten_max
    else:
        return max_val

def update_min(min_val, S):
    poten_min = np.min(S)
    if poten_min < min_val:
        return poten_min
    else:
        return min_val

def normalize_log_spec(z, max_val, min_val):
    normalized_z = (2*(z-max_val))/(max_val-min_val) + 1
    return normalized_z

def denormalize_log_spec (normelized_z, max_num = 1.0, min_num = 0.0):
    z = (normelized_z-1)*(max_num-min_num)/2 + max_num
    return z

def remove_module_str(state_dict):
    state_dict_rename = OrderedDict()
    for k, v in state_dict.items():
        name = k.replace("module.", "") # remove `module.`
        state_dict_rename[name] = v
    return state_dict_rename


#!/usr/bin/python3

# COMP3311 22T3 Assignment 2
# Print info about one movie; may need to choose

import sys
import psycopg2
import helpers

### Globals

db = None
usage = f"Usage: {sys.argv[0]} 'PartialMovieName'"

### Command-line args

if len(sys.argv) < 2:
   print(usage)
   exit(1)

# process the command-line args ...
movie = sys.argv[1].lower()
pattern = f"%{movie}%"
### Queries

qryMovieExist = """
   select m.title, m.year, m.id
   from Movies m
   where lower(title) like %s
   order by m.title, m.year
   ;
"""

#qry only with actor and actress
qryAct = """
   select p.name, pr.job, pr.id
   from Principals pr
   join People p on p.id = pr.person
   where pr.movie = %s
   order by pr.ord
   ;
"""

#qry except actor and actress
qryRole = """
   select role
   from Playsrole
   where inMovie = %s
   ;
"""

### Manipulating database
db = psycopg2.connect("dbname=ass2")
cur = db.cursor()

try:
   cur.execute(qryMovieExist, [pattern])
   movies = cur.fetchall()

   numMovie = 0
   if not movies:
      print(f"No movie matching: '{sys.argv[1]}'")
      exit(0)
   else:
      numMovie = len(movies)

   # only 1 movie match
   if numMovie == 1:
      for m in movies:
         print(f"{m[0]} ({m[1]})")
         cur.execute(qryAct, [m[2]])
         actPri = cur.fetchall()
         numRoles = len(actPri)
         for a in actPri:
            if(a[1] == "self" or a[1] == "actor" or a[1] == "actress"):
               cur.execute(qryRole, [a[2]])
               roles = cur.fetchone()
               if not roles:
                  print(f"{a[0]} plays ???")
               else:
                  print(f"{a[0]} plays {roles[0]}")
            else:
               print(f"{a[0]}: {a[1]}")
   else :
      counter = 0
      for m in movies:
         counter += 1
         print(f"{counter}. {m[0]} ({m[1]})")
         cur.execute(qryAct, [m[2]])
         actPri = cur.fetchall()
         numRoles = len(actPri)

      chooseMovie = input("Which movie? ")
      counter = 0
      for m in movies:
         counter += 1
         if (counter == int(chooseMovie)):
            print(f"{m[0]} ({m[1]})")
            cur.execute(qryAct, [m[2]])
            actPri = cur.fetchall()
            numRoles = len(actPri)
            for a in actPri:
               if(a[1] == "self" or a[1] == "actor" or a[1] == "actress"):
                  cur.execute(qryRole, [a[2]])
                  roles = cur.fetchone()
                  if not roles:
                     print(f"{a[0]} plays ???")
                  else:
                     print(f"{a[0]} plays {roles[0]}")
               else:
                  print(f"{a[0]}: {a[1]}")
         #else:
               #print("No such movie")

except Exception as err:
   print("DB error: ", err)

finally:
   if db:
      db.close()


# 入力が10**5とかになったときに100ms程度早い
import sys
read = sys.stdin.readline

def read_ints():
    return list(map(int, read().split()))

def read_a_int():
    return int(read())

def read_matrix(H):
    '''
    H is number of rows
    '''
    return [list(map(int, read().split())) for _ in range(H)]

def read_map(H):
    '''
    H is number of rows
    文字列で与えられた盤面を読み取る用
    '''
    return [read()[:-1] for _ in range(H)]

def read_col(H, n_cols):
    '''
    H is number of rows
    n_cols is number of cols

    A列、B列が与えられるようなとき
    '''
    ret = [[] for _ in range(n_cols)]
    for _ in range(H):
        tmp = list(map(int, read().split()))
        for col in range(n_cols):
            ret[col].append(tmp[col])

    return ret

from math import sqrt
N, M = read_ints()

A = read_ints()
A.sort(reverse=True)

s = 0
from itertools import combinations_with_replacement
ls = []
for a, b in combinations_with_replacement(A, 2):
    s += a + b
    ls.append(s)

ls.sort(reverse=True)
print(ls)


''''
    Author:Shuntos
    2020 Apr 14
'''

import tensorflow as tf
from tensorflow.python.platform import gfile
import numpy as np
import os
import cv2
import time
path_to_pb = "./weights/froze_xxx.pb"
classes = ["plant","negative"]
dir_path = 'imgs'

def inference(image):
    text= None
    image_size=128
    num_channels=3
    images = []
    try:
        # Resizing the image to our desired size and preprocessing will be done exactly as done during training
        image = cv2.resize(image, (image_size, image_size),0,0, cv2.INTER_LINEAR)
        images.append(image)
        images = np.array(images, dtype=np.uint8)
        images = images.astype('float32')
        images = np.multiply(images, 1.0/255.0)
        # The input to the network is of shape [None image_size image_size num_channels]. Hence we reshape.
        x_batch = images.reshape(1, image_size,image_size,num_channels)

        # Let us restore the saved model
        with tf.Session() as sess:
            print("load graph")
            with gfile.FastGFile(path_to_pb,'rb') as f:
               graph_def = tf.GraphDef()
            graph_def.ParseFromString(f.read())
            sess.graph.as_default()
            tf.import_graph_def(graph_def, name='')
            graph_nodes=[n for n in graph_def.node]
            names = []
            for t in graph_nodes:
              names.append(t.name)
            print(names)

            graph = tf.get_default_graph()
            # Now, let's get hold of the op that we can be processed to get the output.
            # In the original network y_pred is the tensor that is the prediction of the network
            y_pred = graph.get_tensor_by_name("y_pred:0")

            ## Let's feed the images to the input placeholders
            x= graph.get_tensor_by_name("x:0")

            # Creating the feed_dict that is required to be fed to calculate y_pred
            feed_dict_testing = {x: x_batch}
            result=sess.run(y_pred, feed_dict=feed_dict_testing)
            # Result is of this format [[probabiliy_of_classA probability_of_classB ....]]
            # Convert np.array to list
            a = result[0].tolist()
            text = classes[a.index(max(a))]
            print("Result",text)

    except Exception as e:
        print("Exception:",e)

    return text

if __name__ == "__main__":
    img = cv2.imread("imgs/d.jpg")
    inference(img)


import pygame
import sys
from config import *
from Point import Point

clock = pygame.time.Clock()
sc = pygame.display.set_mode((WIN_WIDTH, WIN_HEIGHT),)
isRunning = True

def pointsFabric(count, center, r, width):
    points = []
    step = r / count
    x0, y0 = center[0], center[1]
    ANGLE_VELOCITY = LINEAR_SPEED/step
    points.append(Point(cord = (x0 + step, y0),center =  (x0, y0),
                        width = width, track_radius = step,
                        angle_velocty = ANGLE_VELOCITY))

    for i in range(2, count + 1):
        print(ANGLE_VELOCITY)
        ANGLE_VELOCITY = LINEAR_SPEED/(i * step)
        points.append(Point(cord = (x0 + step * i, y0),
                            center = (x0, y0),track_radius= i * step,width= width,
                            color= WHITE, connectedPoint= points[i - 2], angle_velocty= ANGLE_VELOCITY))

    return points

points = pointsFabric(POINTS_COUNT, (WIN_HEIGHT // 2, WIN_HEIGHT // 2), RADIUS, WIDTH)
clock.tick(FPS)
while isRunning:
    sc.fill(BLACK)
    pygame.draw.circle(sc,WHITE, (WIN_HEIGHT // 2, WIN_HEIGHT // 2), WIDTH)
    for i in pygame.event.get():
        if i.type == pygame.QUIT:
            isRunning = False
            sys.exit()
    for point in points:
        point.draw(sc)

        point.move(clock.tick(FPS))

    pygame.display.flip()
    clock.tick(FPS)


# check if spark context is defined
print(sc.version)

# importing some libraries
import pandas as pd
import pyspark
from pyspark.sql import SQLContext

sqlContext = SQLContext(sc)
# stuff we'll need for text processing
# from nltk.corpus import stopwords
import re as re
from pyspark.ml.feature import CountVectorizer, IDF
# stuff we'll need for building the model

from pyspark.ml.linalg import Vector, Vectors
from pyspark.ml.clustering import LDA, LDAModel

# reading the data
data = sqlContext.read.format('text').options(header='false', inferschema='true').load(
    "/FileStore/tables/dataset_1605467")
data.printSchema()

from pyspark.ml.feature import StopWordsRemover

reviews = data.filter(data['value'].isNotNull())

# Tokenization
from pyspark.ml.feature import Tokenizer

tokenization = Tokenizer(inputCol='value', outputCol='tokens')
tokenized_df = tokenization.transform(reviews)
tokenized_df.printSchema()

# stopwords removal

from pyspark.ml.feature import StopWordsRemover

spanish_swords = StopWordsRemover.loadDefaultStopWords('spanish')
stopword_removal = StopWordsRemover(inputCol='tokens', outputCol='refined_tokens').setStopWords(spanish_swords)
refined_df = stopword_removal.transform(tokenized_df)

# Count Vectorizer
from pyspark.ml.feature import CountVectorizer

count_vec = CountVectorizer(inputCol='refined_tokens', outputCol='tf_features', vocabSize=5000, minDF=10.0)
result_cv = count_vec.fit(refined_df).transform(refined_df)

# result_cv.select(['Clothing ID','refined_tokens','raw_features']).show(4,False)
vocabArray = count_vec.fit(refined_df).vocabulary

# Tf-idf
# from pyspark.ml.feature import HashingTF,IDF

# hashing_vec=HashingTF(inputCol='refined_tokens',outputCol='tf_features')
# hashing_df=hashing_vec.transform(refined_df)
# hashing_df.select(['refined_tokens','tf_features']).show(4,False)

tf_idf_vec = IDF(inputCol='tf_features', outputCol='features')

tf_idf_df = tf_idf_vec.fit(result_cv).transform(result_cv)
# tf_idf_df.select(['user_id','tf_idf_features']).show(4,False)

# tf_idf_df.cache()
tf_idf_df.persist(storageLevel=pyspark.StorageLevel.MEMORY_AND_DISK)

from pyspark.ml.clustering import LDA, LDAModel

num_topics = 10
max_iterations = 10
lda = LDA(k=num_topics, maxIter=max_iterations)
# lda_model = lda.fit(tf_idf_df[['index','features']].rdd.map(list))

model = lda.fit(tf_idf_df)

ll = model.logLikelihood(tf_idf_df)
lp = model.logPerplexity(tf_idf_df)
print("The lower bound on the log likelihood of the entire corpus: " + str(ll))
print("The upper bound on perplexity: " + str(lp))

# Describe topics.
topics = model.describeTopics(3)
print("The topics described by their top-weighted terms:")
topics.show(truncate=False)

# Shows the result
# transformed = model.transform(tf_idf_df)
# transformed.show(truncate=False)

wordNumbers = 5
topicIndices = model.describeTopics(maxTermsPerTopic=wordNumbers)

def topic_render(topic):
    terms = topic[0]
    result = []
    for i in range(wordNumbers):
        term = vocabArray[terms[i]]
        result.append(term)
    return result

topicos = topicIndices.select('topic').collect()
# print(topicos)
tindices = topicIndices.select('termIndices').collect()
# print(tindices)

print(len(topicos))
for topic in range(len(topicos)):
    print("Topic " + str(topic) + ":")
    print(topic_render(tindices[topic]))
    '''for term in tindices[topic]:
        print(term)'''


from typing import List, Dict, Any, Tuple, Union, Optional
import math
import os
import datetime
import pickle
from enum import Enum
from copy import deepcopy
from itertools import product
from timeit import default_timer as timer

import numpy as np
import matplotlib.pyplot as plt
import cvxpy as cp

from IOData.IOData import IOData, InputRule
from IOData.IODataWith_l import IODataWith_l
from System.ErrorKinematicAcceLATI import LinearizedErrorKinematicAcceModel, LinearizedErrorKinematicAcceModelParams, KinematicAcceModelParams, KinematicAcceModel
from System.DynamicModel import DynamicModel, DynamicModelParams, DynamicModelFewOutput
from System.DynamicErrorModel import DynamicErrorModelFewOutput, DynamicErrorModelParams, DynamicErrorModelVxy
from tools.simualtion_results import Results
from tools.simple_track_generator import trackGenerator, arc, line

from SafetyFilters.DDSF import SFParams
from SafetyFilters.Indirect_Nominal_Fitting import IndirectNominalFittingFilter, IndirectNominalFittingParams
from SafetyFilters.Indirect_Nominal_FixMu import IndirectNominalFixMuFilter, IndirectNominalFixMuParams
from SafetyFilters.Indirect_Nominal_FixMu_Weighting import IndirectNominalFixMuWeightingFilter, IndirectNominalFixMuWeightingParams
from SafetyFilters.Indirect_Nominal_FixMu_Weighting_Add_Data import IndirectNominalFixMuWeightingAddDataFilter, IndirectNominalFixMuWeightingAddDataParams
from SafetyFilters.Indirect_Nominal_Stop import IndirectNominalStopFilter, IndirectNominalStopParams
from SafetyFilters.Indirect_Nominal_Zero import IndirectNominalZeroFilter, IndirectNominalZeroParams
from SafetyFilters.Indirect_Nominal_ZeroV import IndirectNominalZeroVFilter, IndirectNominalZeroVParams
from SafetyFilters.Indirect_Nominal_ZeroV_Weighting import IndirectNominalZeroVWeightingFilter, IndirectNominalZeroVWeightingParams
from SafetyFilters.Indirect_Nominal_ZeroV_Weighting_Add_Data import IndirectNominalZeroVWeightingAddDataFilter, IndirectNominalZeroVWeightingAddDataParams
from SafetyFilters.Indirect_Nominal_FixMu_AddData import IndirectNominalFixMuAddDataFilter, IndirectNominalFixMuAddDataParams
from SafetyFilters.Indirect_Nominal_FixMu_AddDataLateral import IndirectNominalFixMuAddDataLateralFilter, IndirectNominalFixMuAddDataLateralParams
from SafetyFilters.Direct_Nominal_Zero import DirectNominalZeroFilter, DirectNominalZeroParams
from SafetyFilters.TrackFilter import SafetyFilterForTrack
from SafetyFilters.TrackFilter_AddData import SafetyFilterForTrackAddData

from simulators.simulation_settings import SafetyFilterTypes, TrackFilterTypes, SimulationInputRule, ModelType
from typing import Mapping

FewOutPutErrorSystem = Union[LinearizedErrorKinematicAcceModel, DynamicErrorModelFewOutput]

def oval_track(gen: trackGenerator, t: float, track_start_point: np.ndarray) -> trackGenerator:
    next = gen.straight(track_start_point, 3*t)
    next = gen.left_turn(next, t, np.pi)
    next = gen.straight(next, 6*t)
    next = gen.left_turn(next, t, np.pi)
    next = gen.straight(next, 3*t)
    return gen

def larger_oval_track(gen: trackGenerator, t: float, track_start_point: np.ndarray) -> trackGenerator:
    next = gen.straight(track_start_point, 3*t)
    next = gen.left_turn(next, 3*t, np.pi)
    next = gen.straight(next, 6*t)
    next = gen.left_turn(next, 3*t, np.pi)
    next = gen.straight(next, 3*t)
    return gen

def round_track(gen: trackGenerator, t: float, track_start_point: np.ndarray) -> trackGenerator:
    next = gen.left_turn(track_start_point, t, 2*np.pi)
    return gen

track_func = oval_track

class TrackSimulator:
    """Simulator for a certain track"""
    # parameters for simulation
    # parameters for kinematic model
    l_r = 0.052
    l_f = 0.038
    # additional parameters for dynamic model
    m = 0.181
    Iz = 0.000505
    Df = 0.65
    Cf = 1.5
    Bf = 5.2
    Dr = 1.0
    Cr = 1.45
    Br = 8.5
    Cm1 = 0.98028992
    Cm2 = 0.01814131
    Cd = 0.02750696
    Croll = 0.08518052
    # control and output constraints
    a_min, a_max = -1.1, 5.5
    delta_max = 0.4
    d_a_max = 27.62
    d_delta_max = 40
    v_x_min, v_x_max = 0.05, 3.2
    v_y_min, v_y_max = -2.0, 2.0
    mu_min, mu_max = -0.5*math.pi, 0.5*math.pi

    # track parameters
    cur = 1/0.3
    track_width = 0.5
    track_start = np.array([0,0,0]) # [x_p0, y_p0, Psi_0]
    # steady state speed
    v_0 = 1
    # track density and generator
    density = 300
    track_generator = trackGenerator(density, track_width)
    track_fun_name = 'round_track'

    # noise level for online observation
    n_x = 0.001
    n_y = 0.001
    n_psi = 0.01
    n_v = 0.005
    # noise level for dataset
    n_e_lat_max = 0.002
    n_mu_max = 0.01
    n_v_max = 0.005
    n_l_max = 0.002

    use_saved_data = False # whether to use saved dataset
    save_data = True # whether to save dataset
    # maximum control inputs for collecting dataset
    a_d_max = 3
    delta_d_max = 0.2
    t_data = 10 # time horizon for dataset, in seconds
    data_model_type = ModelType.DYNAMIC_FEW_OUTPUT # model type used for collecting dataset
    data_input_rule = InputRule.RANDOM_2_WITH_MEAN # input rule used for collecting dataset

    # random seed for generating noise
    random_seed = 0
    # model type used for simulation
    simulate_model_type = ModelType.DYNAMIC_FEW_OUTPUT
    # intial state of system
    global_initial_state = np.array([0.0, 0.0, 0.0, 0.5, 0.0, 0.0]) # [x_p0, y_p0, yaw_0, v_x_0, v_y_0, yaw_rate_0], will be transformed if necessary
    simulation_input_type = SimulationInputRule.SINE_WAVE
    # magnitute of control inputs for simulation
    a_sim = 6
    delta_sim = 0.12*math.pi
    # simulation time
    t_sim = 1
    num_predicted_traj = 5 # number of predicted trajectories to be saved
    first_save_time = 0.6 # time when the first predicted trajectory is saved
    num_prediction_buffer = 50 # save the previous preidcted trajectory and proposed inputs, for later uasge
    buffer_y_value: List[Tuple[float,List[np.ndarray]]] = [] #[[time step, List of state],]
    buffer_sigma_value: List[Tuple[float,List[np.ndarray]]] = []
    buffer_u_value: List[Tuple[float,List[np.ndarray]]] = []
    buffer_i_seg: List[Tuple[float,int]] = []
    buffer_global_state: List[Tuple[float,np.ndarray]] = []
    out_of_track: bool = False # whether the car is out of track, we only want to see what happens at the first steps!
    save_predicted_traj_regular = True # whether to save predicted trajectory
    stop_after_out_of_track = False # whether to stop simulation after the car is out of track
    save_dataset_after = True # whether to save dataset after simulation

    # filter parameters, those are important also for simulation
    filter_model_type = ModelType.KINEMATIC
    track_filter_type = TrackFilterTypes.SINGLE_SEGMENT
    filter_type_list = [SafetyFilterTypes.INDIRECT_FITTING_TERMINAL]
    # sampling time
    Ts = 1/100
    L = 50 # prediction horizon
    lag = 15 # steps used for initial condition of filter
    steps = 4 # number of inputs applied to the system for each filtering
    slack = 1e-1 # slack variable for determining the segment index

    # variables to be used for generating filters
    systems: List[LinearizedErrorKinematicAcceModel]
    filter_params: List
    io_data_dict: Mapping[float, IODataWith_l]
    io_data_dict_stored: Mapping[float, IODataWith_l] = {} # stored data, not modified after created

    def get_filter_params(self, cur: float, filter_type: SafetyFilterTypes, **kwargs):
        half_track_width = self.track_width/2
        epsilon = max(self.n_e_lat_max, self.n_mu_max, self.n_v_max)
        if filter_type == SafetyFilterTypes.INDIRECT_FITTING_TERMINAL:
            lam_sig = 50000
            c = [[0.3, 0.1, 0.05, 0.01], [0.3, 0.1, 0.05, 0.01]]
            R = np.matrix('1 0; 0 1')
            lam_sig = kwargs.get('lam_sig', lam_sig)
            c = kwargs.get('c', c)
            R = kwargs.get('R', R)
            epsilon = kwargs.get('epsilon', epsilon)
            filter_params = IndirectNominalFittingParams(
                L=self.L, lag=self.lag, R=R, lam_sig=lam_sig, epsilon=None,
                c=[
                    [c_i*half_track_width for c_i in c[0]],
                    [c_i*self.mu_max for c_i in c[1]],
                ],
                sf_params=SFParams(steps=self.steps, verbose=False, solver=cp.MOSEK,
                            solver_args={'mosek_params':{
                                'MSK_DPAR_INTPNT_CO_TOL_PFEAS':1e-4,
                                'MSK_DPAR_INTPNT_CO_TOL_DFEAS':1e-4,
                                'MSK_DPAR_INTPNT_CO_TOL_REL_GAP':5e-4,
                                'MSK_DPAR_INTPNT_CO_TOL_INFEAS':1e-5,
                                'MSK_IPAR_INTPNT_MAX_ITERATIONS': 30000}}),
            )
        elif filter_type == SafetyFilterTypes.INDIRECT_FIX_MU:
            lam_sig = 50000
            c = [[0.3, 0.1, 0.05, 0.01], [0.3, 0.1, 0.05, 0.01]]
            R = np.matrix('1 0; 0 1')
            lam_sig = kwargs.get('lam_sig', lam_sig)
            c = kwargs.get('c', c)
            R = kwargs.get('R', R)
            epsilon = kwargs.get('epsilon', epsilon)

            filter_params = IndirectNominalFixMuParams(
                L=self.L, lag=self.lag, R=R, lam_sig=lam_sig, epsilon=None,
                c=[
                    [c_i*half_track_width for c_i in c[0]],
                    [c_i*self.mu_max for c_i in c[1]],
                ],
                sf_params=SFParams(steps=self.steps, verbose=False, solver=cp.MOSEK,
                                    solver_args={'mosek_params':{
                                        'MSK_DPAR_INTPNT_CO_TOL_PFEAS':1e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_DFEAS':1e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_REL_GAP':1e-3,
                                        'MSK_DPAR_INTPNT_CO_TOL_INFEAS':1e-5,
                                        'MSK_IPAR_INTPNT_MAX_ITERATIONS': 30000}})
            )
        elif filter_type == SafetyFilterTypes.INDIRECT_FIX_MU_WEIGHTING:
            lam_sig = 50000
            c = [[0.3, 0.1, 0.05, 0.01], [0.3, 0.1, 0.05, 0.01]]
            R = np.matrix('1 0; 0 1')
            lam_sig = kwargs.get('lam_sig', lam_sig)
            c = kwargs.get('c', c)
            R = kwargs.get('R', R)
            epsilon = kwargs.get('epsilon', epsilon)
            c_max_list = [half_track_width, self.mu_max, (self.v_x_max-self.v_x_min)/2, (self.v_y_max-self.v_y_min)/2]

            filter_params = IndirectNominalFixMuWeightingParams(
                L=self.L, lag=self.lag, R=R, lam_sig=lam_sig, epsilon=None,
                c=[
                    [c_i*c_max for c_i in c_sub] for c_max,c_sub in zip(c_max_list, c)
                ],
                sf_params=SFParams(steps=self.steps, verbose=False, solver=cp.MOSEK,
                                    solver_args={'mosek_params':{
                                        'MSK_DPAR_INTPNT_CO_TOL_PFEAS':1e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_DFEAS':1e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_REL_GAP':1e-3,
                                        'MSK_DPAR_INTPNT_CO_TOL_INFEAS':1e-5,
                                        'MSK_IPAR_INTPNT_MAX_ITERATIONS': 30000}})
            )
        elif filter_type == SafetyFilterTypes.INDIRECT_FIX_MU_WEIGHTING_ADD_DATA:
            lam_sig = 50000
            c = [[0.3, 0.1, 0.05, 0.01], [0.3, 0.1, 0.05, 0.01]]
            R = np.matrix('1 0; 0 1')
            lam_sig = kwargs.get('lam_sig', lam_sig)
            c = kwargs.get('c', c)
            R = kwargs.get('R', R)
            epsilon = kwargs.get('epsilon', epsilon)
            t_new_data = kwargs.get('t_new_data', 6.0)
            c_max_list = [half_track_width, self.mu_max, (self.v_x_max-self.v_x_min)/2, (self.v_y_max-self.v_y_min)/2]

            filter_params = IndirectNominalFixMuWeightingAddDataParams(
                L=self.L, lag=self.lag, R=R, lam_sig=lam_sig, epsilon=None,
                t_new_data=t_new_data,
                c=[
                    [c_i*c_max for c_i in c_sub] for c_max,c_sub in zip(c_max_list, c)
                ],
                sf_params=SFParams(steps=self.steps, verbose=False, solver=cp.MOSEK,
                                    solver_args={'mosek_params':{
                                        'MSK_DPAR_INTPNT_CO_TOL_PFEAS':1e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_DFEAS':1e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_REL_GAP':1e-3,
                                        'MSK_DPAR_INTPNT_CO_TOL_INFEAS':1e-5,
                                        'MSK_IPAR_INTPNT_MAX_ITERATIONS': 30000}})
            )
        elif filter_type == SafetyFilterTypes.INDIRECT_ZERO_V:
            lam_sig = 50000
            c = [[0.3, 0.1, 0.05, 0.01], [0.3, 0.1, 0.05, 0.01]]
            R = np.matrix('1 0; 0 1')
            lam_sig = kwargs.get('lam_sig', lam_sig)
            c = kwargs.get('c', c)
            R = kwargs.get('R', R)
            epsilon = kwargs.get('epsilon', epsilon)

            filter_params = IndirectNominalZeroVParams(
                L=self.L, lag=self.lag, R=R, lam_sig=lam_sig, epsilon=None,
                c=[
                    [c_i*half_track_width for c_i in c[0]],
                    [c_i*self.mu_max for c_i in c[1]],
                ],
                sf_params=SFParams(steps=self.steps, verbose=False, solver=cp.MOSEK,
                                    solver_args={'mosek_params':{
                                        'MSK_DPAR_INTPNT_CO_TOL_PFEAS':1e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_DFEAS':1e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_REL_GAP':1e-3,
                                        'MSK_DPAR_INTPNT_CO_TOL_INFEAS':1e-5,
                                        'MSK_IPAR_INTPNT_MAX_ITERATIONS': 30000}})
            )
        elif filter_type == SafetyFilterTypes.INDIRECT_ZERO_V_WEIGHTING:
            lam_sig = 50000
            c = [[0.3, 0.1, 0.05, 0.01], [0.3, 0.1, 0.05, 0.01]]
            R = np.matrix('1 0; 0 1')
            lam_sig = kwargs.get('lam_sig', lam_sig)
            c = kwargs.get('c', c)
            R = kwargs.get('R', R)
            epsilon = kwargs.get('epsilon', epsilon)
            t_new_data = kwargs.get('t_new_data', 6.0)
            c_max_list = [half_track_width, self.mu_max, (self.v_x_max-self.v_x_min)/2, (self.v_y_max-self.v_y_min)/2]

            filter_params = IndirectNominalZeroVWeightingParams(
                L=self.L, lag=self.lag, R=R, lam_sig=lam_sig, epsilon=None,
                c=[
                    [c_i*c_max for c_i in c_sub] for c_max,c_sub in zip(c_max_list, c)
                ],
                sf_params=SFParams(steps=self.steps, verbose=False, solver=cp.MOSEK,
                                    solver_args={'mosek_params':{
                                        'MSK_DPAR_INTPNT_CO_TOL_PFEAS':1e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_DFEAS':1e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_REL_GAP':1e-3,
                                        'MSK_DPAR_INTPNT_CO_TOL_INFEAS':1e-5,
                                        'MSK_IPAR_INTPNT_MAX_ITERATIONS': 30000}})
            )
        elif filter_type == SafetyFilterTypes.INDIRECT_ZERO_V_WEIGHTING_ADD_DATA:
            lam_sig = 50000
            c = [[0.3, 0.1, 0.05, 0.01], [0.3, 0.1, 0.05, 0.01]]
            R = np.matrix('1 0; 0 1')
            lam_sig = kwargs.get('lam_sig', lam_sig)
            c = kwargs.get('c', c)
            R = kwargs.get('R', R)
            epsilon = kwargs.get('epsilon', epsilon)
            t_new_data = kwargs.get('t_new_data', 6.0)
            c_max_list = [half_track_width, self.mu_max, (self.v_x_max-self.v_x_min)/2, (self.v_y_max-self.v_y_min)/2]

            filter_params = IndirectNominalZeroVWeightingAddDataParams(
                L=self.L, lag=self.lag, R=R, lam_sig=lam_sig, epsilon=None,
                t_new_data=t_new_data,
                c=[
                    [c_i*c_max for c_i in c_sub] for c_max,c_sub in zip(c_max_list, c)
                ],
                sf_params=SFParams(steps=self.steps, verbose=False, solver=cp.MOSEK,
                                    solver_args={'mosek_params':{
                                        'MSK_DPAR_INTPNT_CO_TOL_PFEAS':1e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_DFEAS':1e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_REL_GAP':1e-3,
                                        'MSK_DPAR_INTPNT_CO_TOL_INFEAS':1e-5,
                                        'MSK_IPAR_INTPNT_MAX_ITERATIONS': 30000}})
            )
        elif filter_type == SafetyFilterTypes.INDIRECT_ZERO:
            lam_sig = 50000
            c = [[0.3, 0.1, 0.05, 0.01], [0.3, 0.1, 0.05, 0.01]]
            R = np.matrix('1 0; 0 1')
            lam_sig = kwargs.get('lam_sig', lam_sig)
            c = kwargs.get('c', c)
            R = kwargs.get('R', R)
            epsilon = kwargs.get('epsilon', epsilon)

            filter_params = IndirectNominalZeroParams(
                L=self.L, lag=self.lag, R=R, lam_sig=lam_sig, epsilon=None,
                c=[
                    [c_i*half_track_width for c_i in c[0]],
                    [c_i*self.mu_max for c_i in c[1]],
                ],
                sf_params=SFParams(steps=self.steps, verbose=False, solver=cp.MOSEK,
                                    solver_args={'mosek_params':{
                                        'MSK_DPAR_INTPNT_CO_TOL_PFEAS':1e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_DFEAS':1e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_REL_GAP':1e-3,
                                        'MSK_DPAR_INTPNT_CO_TOL_INFEAS':1e-5,
                                        'MSK_IPAR_INTPNT_MAX_ITERATIONS': 30000}})
            )
        elif filter_type == SafetyFilterTypes.INDIRECT_STOP:
            lam_sig = 50000
            c = [[0.3, 0.1, 0.05, 0.01], [0.3, 0.1, 0.05, 0.01]]
            R = np.matrix('1 0; 0 1')
            lam_sig = kwargs.get('lam_sig', lam_sig)
            c = kwargs.get('c', c)
            R = kwargs.get('R', R)
            epsilon = kwargs.get('epsilon', epsilon)

            filter_params = IndirectNominalStopParams(
                L=self.L, lag=self.lag, R=R, lam_sig=lam_sig, epsilon=None,
                c=[
                    [c_i*half_track_width for c_i in c[0]],
                    [c_i*self.mu_max for c_i in c[1]],
                ],
                sf_params=SFParams(steps=self.steps, verbose=False, solver=cp.MOSEK,
                                    solver_args={'mosek_params':{
                                        'MSK_DPAR_INTPNT_CO_TOL_PFEAS':1e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_DFEAS':1e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_REL_GAP':1e-3,
                                        'MSK_DPAR_INTPNT_CO_TOL_INFEAS':1e-5,
                                        'MSK_IPAR_INTPNT_MAX_ITERATIONS': 30000}})
            )
        elif filter_type == SafetyFilterTypes.INDIRECT_FIX_MU_ADD_DATA:
            lam_sig = 50000
            c = [[0.3, 0.1, 0.05, 0.01], [0.3, 0.1, 0.05, 0.01]]
            R = np.matrix('1 0; 0 1')
            lam_sig = kwargs.get('lam_sig', lam_sig)
            c = kwargs.get('c', c)
            R = kwargs.get('R', R)
            epsilon = kwargs.get('epsilon', epsilon)

            filter_params = IndirectNominalFixMuAddDataParams(
                L=self.L, lag=self.lag, R=R, lam_sig=lam_sig, epsilon=None,
                c=[
                    [c_i*half_track_width for c_i in c[0]],
                    [c_i*self.mu_max for c_i in c[1]],
                ],
                sf_params=SFParams(steps=self.steps, verbose=False, solver=cp.MOSEK,
                                    solver_args={'mosek_params':{
                                        'MSK_DPAR_INTPNT_CO_TOL_PFEAS':1e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_DFEAS':1e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_REL_GAP':1e-3,
                                        'MSK_DPAR_INTPNT_CO_TOL_INFEAS':1e-5,
                                        'MSK_IPAR_INTPNT_MAX_ITERATIONS': 30000}})
            )
        elif filter_type == SafetyFilterTypes.INDIRECT_FIX_MU_ADD_DATA_LATERAL:
            lam_sig = 50000
            c = [[0.3, 0.1, 0.05, 0.01], [0.3, 0.1, 0.05, 0.01]]
            R = np.matrix('1 0; 0 1')
            lam_sig = kwargs.get('lam_sig', lam_sig)
            c = kwargs.get('c', c)
            R = kwargs.get('R', R)
            epsilon = kwargs.get('epsilon', epsilon)

            filter_params = IndirectNominalFixMuAddDataLateralParams(
                L=self.L, lag=self.lag, R=R, lam_sig=lam_sig, epsilon=None,
                c=[
                    [c_i*half_track_width for c_i in c[0]],
                    [c_i*self.mu_max for c_i in c[1]],
                ],
                sf_params=SFParams(steps=self.steps, verbose=False, solver=cp.MOSEK,
                                    solver_args={'mosek_params':{
                                        'MSK_DPAR_INTPNT_CO_TOL_PFEAS':1e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_DFEAS':1e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_REL_GAP':1e-3,
                                        'MSK_DPAR_INTPNT_CO_TOL_INFEAS':1e-5,
                                        'MSK_IPAR_INTPNT_MAX_ITERATIONS': 30000}})
            )
        elif filter_type == SafetyFilterTypes.DIRECT_ZERO_TERMINAL:
            lam_sig = 1500
            lam_alph = 1500/epsilon
            c = [[0.35, 0.1, 0.1, 0.05], [0.4, 0.1, 0.1, 0.05]]
            R = np.matrix('1 0; 0 1')
            lam_sig = kwargs.get('lam_sig', lam_sig)
            lam_alph = kwargs.get('lam_alph', lam_alph)
            c = kwargs.get('c', c)
            R = kwargs.get('R', R)
            epsilon = kwargs.get('epsilon', epsilon)
            filter_params = DirectNominalZeroParams(
                L=self.L, lag=self.lag, R=R, lam_alph=lam_alph, lam_sig=lam_sig, epsilon=epsilon,
                c=[
                    [c_i*half_track_width for c_i in c[0]],
                    [c_i*self.mu_max for c_i in c[1]],
                ],
                sf_params=SFParams(steps=self.steps, verbose=True, solver=cp.MOSEK,
                                   solver_args={'mosek_params':{
                                        'MSK_DPAR_INTPNT_CO_TOL_PFEAS':3e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_DFEAS':3e-5,
                                        'MSK_DPAR_INTPNT_CO_TOL_INFEAS':3e-6,
                                        'MSK_DPAR_INTPNT_CO_TOL_REL_GAP':1e-4}})
            )
        return filter_params

    def get_single_filter(self,
                          cur: float, filter_type: SafetyFilterTypes,
                          system, io_data, **kwargs):
        params = self.get_filter_params(cur, filter_type, **kwargs)
        FilterClass = {SafetyFilterTypes.INDIRECT_FITTING_TERMINAL: IndirectNominalFittingFilter,
                        SafetyFilterTypes.INDIRECT_FIX_MU: IndirectNominalFixMuFilter,
                        SafetyFilterTypes.INDIRECT_FIX_MU_WEIGHTING: IndirectNominalFixMuWeightingFilter,
                        SafetyFilterTypes.INDIRECT_FIX_MU_WEIGHTING_ADD_DATA: IndirectNominalFixMuWeightingAddDataFilter,
                        SafetyFilterTypes.INDIRECT_ZERO_V: IndirectNominalZeroVFilter,
                        SafetyFilterTypes.INDIRECT_ZERO_V_WEIGHTING: IndirectNominalZeroVWeightingFilter,
                        SafetyFilterTypes.INDIRECT_ZERO_V_WEIGHTING_ADD_DATA: IndirectNominalZeroVWeightingAddDataFilter,
                        SafetyFilterTypes.INDIRECT_ZERO: IndirectNominalZeroFilter,
                        SafetyFilterTypes.INDIRECT_STOP: IndirectNominalStopFilter,
                        SafetyFilterTypes.INDIRECT_FIX_MU_ADD_DATA: IndirectNominalFixMuAddDataFilter,
                        SafetyFilterTypes.INDIRECT_FIX_MU_ADD_DATA_LATERAL: IndirectNominalFixMuAddDataLateralFilter,
                        SafetyFilterTypes.DIRECT_ZERO_TERMINAL: DirectNominalZeroFilter,
                        }.get(filter_type, IndirectNominalFittingFilter)
        return FilterClass(system, io_data, params)

    def simulate_with_dataset_update(self,
                                     random_seed: int,
                                     track_filter_type: TrackFilterTypes,
                                     filter_type: SafetyFilterTypes,
                                     filter_params: Dict[str, Any],
                                     simulation_input_types: List[SimulationInputRule],
                                     max_run_turns: int = 10,
                                     ) -> List[Results]:
        # setup needed options
        self.save_dataset_after = True
        self.stop_after_out_of_track = True

        self.noise_list_dict = {}
        self.random_seed = random_seed
        self.noise_list_dict[random_seed] = self.get_noise()
        self.track_filter_type = track_filter_type
        self.filter_type_list = [filter_type] # reserve for using different filter types for segments
        self.simulation_input_type = simulation_input_types[0]

        results_list: List[Results] = []

        print("Simulating for the first time")
        self.get_utilities_for_simualtion(random_seed, **filter_params)
        print(f"\nSimulating with input rule {simulation_input_types[0]}")
        print(f"And with first dataset list {[data.length for data in self.filter._safety_filters[0]._io_data_list]}")
        result = self.simulate_once(random_seed, **filter_params)
        results_list.append(result)
        i = 1
        for input_rule in simulation_input_types[1:]:
            self.out_of_track = False
            print(f"\nSimulating with input rule {input_rule}")
            self.simulation_input_type = input_rule
            self.get_utilities_for_simualtion(random_seed, **filter_params)
            self.load_datasets(results_list[-1].saved_dataset_name)
            print(f"And with first dataset list {[data.length for data in self.filter._safety_filters[0]._io_data_list]}")
            result = self.simulate_once(random_seed, **filter_params)
            results_list.append(result)
            i += 1
        all_good = True # good for all simulation input rules
        for result in results_list[-len(simulation_input_types):]:
            if len(result.violating_time_steps) != 0:
                all_good = False
                break
        while not all_good and len(results_list) < max_run_turns:
        # while len(results_list) < max_run_turns:
            print("\nContinue to simulate since violated constraints last time")
            for input_rule in simulation_input_types:
                print(f"\nSimulating with input rule {input_rule}")
                self.out_of_track = False
                self.simulation_input_type = input_rule
                self.get_utilities_for_simualtion(random_seed, **filter_params)
                self.load_datasets(results_list[-1].saved_dataset_name)
                print(f"And with first dataset list {[data.length for data in self.filter._safety_filters[0]._io_data_list]}")
                result = self.simulate_once(random_seed, **filter_params)
                results_list.append(result)
            all_good = True # good for all simulation input rules
            for result in results_list[-len(simulation_input_types):]:
                if len(result.violating_time_steps) != 0:
                    all_good = False
                    break

        return results_list

    def simulate_multi(self,
                       random_seends: List[float],
                       track_filter_types: List[TrackFilterTypes],
                       filter_type_lists: List[SafetyFilterTypes],
                       filter_params: Dict[SafetyFilterTypes, List[Dict[str, Any]]],
                       simualtion_input_types: List[SimulationInputRule],) -> Dict[Any, List[Results]]:
        dict_results = {}
        self.noise_list_dict = {}
        for random_seed in random_seends:
            self.random_seed = random_seed
            self.noise_list_dict[random_seed] = self.get_noise()
        for random_seed, track_filter_type, filter_type, simulation_input_type in product(random_seends, track_filter_types, filter_type_lists, simualtion_input_types):
            self.track_filter_type = track_filter_type
            self.filter_type_list = [filter_type] # reserve for using different filter types for segments
            self.simulation_input_type = simulation_input_type
            dict_results[(random_seed, track_filter_type, filter_type, simulation_input_type)] = []
            print(f"\n \n Simulating random_seed={random_seed}, track_filter_type={track_filter_type}, filter_type={filter_type}, simulation_input_type={simulation_input_type}")
            for filter_param in filter_params.get(filter_type, [{}]):
                self.get_utilities_for_simualtion(random_seed, **filter_param)
                results = self.simulate_once(random_seed, **filter_param)
                dict_results[(random_seed, track_filter_type, filter_type, simulation_input_type)].append(results)

        self.dict_results = dict_results
        return dict_results

    def get_utilities_for_simualtion(self, random_seed: int, **kwargs):
        self.random_seed = random_seed
        # store parameters, after single simualtion they should be restored
        Ts, L, steps, lag, slack = self.Ts, self.L, self.steps, self.lag, self.slack

        self.set_params_from_dict(**kwargs)

        self.track_func = {
            'oval_track': oval_track,
            'round_track': round_track,
            'large_oval_track': larger_oval_track,
        }.get(self.track_fun_name, oval_track)
        self.track_generator = self.propogate_track_gen()

        self.systems = self.get_system_list(system_type=self.filter_model_type)

        if self.io_data_dict_stored:
            self.io_data_dict = deepcopy(self.io_data_dict_stored)
        else:
            self.io_data_dict_stored = self.get_io_data_dic()
            self.io_data_dict = deepcopy(self.io_data_dict_stored)

        if random_seed not in self.noise_list_dict.keys():
            print(f"Noise with random_seed={random_seed} not generated, generating it online")
            self.noise_list = self.get_noise()
        else:
            self.noise_list = self.noise_list_dict[random_seed]

        self.filter = self.get_filter(**kwargs)
        self.Ts, self.L, self.steps, self.lag, self.slack = Ts, L, steps, lag, slack

    def simulate_once(self, random_seed: int, **kwargs) -> Results:

        self.random_seed = random_seed
        # store parameters, after single simualtion they should be restored
        Ts, L, steps, lag, slack = self.Ts, self.L, self.steps, self.lag, self.slack

        self.set_params_from_dict(**kwargs)

        if self.filter is None:
            self.Ts, self.L, self.steps, self.lag, self.slack = Ts, L, steps, lag, slack
            return None

        results = Results(self.Ts)
        # stores the segment index for each time step
        self.segment_index_list = []

        # initialize global model
        if self.simulate_model_type == ModelType.KINEMATIC:
            kinematic_model_params = KinematicAcceModelParams(self.l_r, self.l_f, self.m, self.Ts)
            v_0 = math.sqrt(self.global_initial_state[3]**2 + self.global_initial_state[4]**2)
            initial_state = np.hstack((self.global_initial_state[0:3],np.array([v_0])))
            crs_model = KinematicAcceModel(kinematic_model_params, initial_state)
        elif self.simulate_model_type == ModelType.DYNAMIC:
            initial_state = self.global_initial_state
            dynamic_model_params = DynamicModelParams(
                self.Ts, self.l_f, self.l_r, self.m, self.Iz, self.Bf, self.Cf, self.Df, self.Br, self.Cr, self.Dr, self.Croll, self.Cm1, self.Cm2, self.Cd,
            )
            crs_model = DynamicModel(dynamic_model_params, self.global_initial_state)
        elif self.simulate_model_type == ModelType.DYNAMIC_FEW_OUTPUT:
            initial_state = self.global_initial_state
            dynamic_model_params = DynamicModelParams(
                self.Ts, self.l_f, self.l_r, self.m, self.Iz, self.Bf, self.Cf, self.Df, self.Br, self.Cr, self.Dr, self.Croll, self.Cm1, self.Cm2, self.Cd,
            )
            crs_model = DynamicModelFewOutput(dynamic_model_params, self.global_initial_state)
            v_0 = math.sqrt(self.global_initial_state[3]**2 + self.global_initial_state[4]**2)
            # initial state for safety filter, here it should be few output model
            initial_state = np.hstack((self.global_initial_state[0:3],np.array([v_0])))

        # initial states for the safety filter. If initial state is non-zero, careful with consistency!
        initial_states_filter: List[np.ndarray] = [initial_state] * self.lag
        initial_inputs_filter: List[np.ndarray] = [np.matrix([[0],[0]])] * self.lag

        # prepare log and block number for simulation
        N = int(self.t_sim/self.Ts)
        N_blocks = int(N/self.steps)
        first_block_to_save = int(self.first_save_time/self.Ts/self.steps)
        N_slices = int((N_blocks-first_block_to_save)/self.num_predicted_traj) # save predicted trajectory every N_slices blocks

        for i_block in range(N_blocks):
            t_block = i_block*self.steps*self.Ts
            # get objective inputs
            u_obj = self.get_input_obj(t_block, crs_model.state)

            # try solving the optimization problem
            try:
                start = timer()
                # update initial conditions for filter
                for u_init, y_init, n_init in zip(initial_inputs_filter[-self.lag:], initial_states_filter[-self.lag:], self.noise_list[i_block*self.steps:i_block*self.steps+self.lag]):
                    self.filter.add_point(u_init, y_init + n_init)

                # apply the safety filter
                u_i, status_i, opt_i = self.filter.filter(u_obj)
                end = timer()
            except Exception as e:
                print(f"Exception {e} raised during optimization, returning partial results")
                if self.save_dataset_after:
                    file_name = self.save_datasets()
                    results.saved_dataset_name = file_name
                results.end_simulation()

                # reset parameters
                self.Ts, self.L, self.steps, self.lag, self.slack = Ts, L, steps, lag, slack
                return results
            results.add_calculation_time(end-start)
            results.add_opt_value(opt_i)
            i_seg = self.filter._i
            results.add_sigma_value(self.filter._safety_filters[i_seg]._sigma.value)
            self.update_predicted_buffer(self.Ts*self.steps*i_block, self.filter_type_list[i_seg], self.filter._safety_filters[i_seg], i_seg, deepcopy(crs_model.state))

            # save predicted trajectory every N_slices blocks
            if (i_block-first_block_to_save)%N_slices == 0 and self.save_predicted_traj_regular:
                # get predicted outputs
                predicted_traj, predicted_with_slack = self.get_predicted_outputs_from_buffer(-1)
                i_seg_buffer = self.buffer_i_seg[-1][1]
                global_initial_state = self.buffer_global_state[-1][1]
                # get and save real output when the proposed inputs are applied
                real_output_list = self.get_real_trajectory_from_proposed_buffer(-1, i_seg_buffer, global_initial_state)
                results.add_predicted_error_slice(i_block*self.steps*self.Ts, predicted_traj)
                results.add_predicted_error_slack_slice(i_block*self.steps*self.Ts, predicted_with_slack)
                results.add_error_slice(i_block*self.steps*self.Ts, real_output_list)

            # save and update system state, for `step` times
            for j in range(self.steps):
                i_steps = i_block*self.steps + j
                # save system states and inputs first
                # global kinematic model state
                global_state = deepcopy(crs_model.state)
                # error system state
                self.systems[i_seg].set_kinematic_model_state(global_state, -self.slack)
                error_dynamics_state = deepcopy(self.systems[i_seg].state)
                global_state[2] = global_state[2] * 180 / np.pi # from rad to deg
                error_dynamics_state[1] = error_dynamics_state[1] * 180 / np.pi # from rad to deg
                u_obj_t = u_obj[j*self.systems[i_seg].m:(j+1)*self.systems[i_seg].m]
                u_t = u_i[j*self.systems[i_seg].m:(j+1)*self.systems[i_seg].m]

                # if constraint not satisfied, save predicted values at previous steps
                A = self.systems[i_seg].A_y
                b = self.systems[i_seg].b_y
                x = np.matrix(self.systems[i_seg].state).T
                x = x[:self.filter._safety_filters[i_seg]._p]
                x[2] = x[2] - self.v_0 # from actual velocity to deviation from steady state
                if not np.all(A*x <= b) and not self.out_of_track: # first time constraint not satisfied
                    self.out_of_track = True
                    results.violating_time_steps.append(self.Ts*(i_block*self.steps+j))
                    index_to_save_list = [-1, -5, -10]
                    print(f"Constraint not satisfied at time {self.Ts*(i_block*self.steps+j)}.")
                    for index_to_save in index_to_save_list:
                        results.mark_time_steps.append((i_block+1+index_to_save)*self.steps-1)
                        # save predicted trajectory at previous steps
                        predicted_traj, predicted_with_slack = self.get_predicted_outputs_from_buffer(index_to_save)
                        i_seg_buffer = self.buffer_i_seg[index_to_save][1]
                        global_initial_state = self.buffer_global_state[index_to_save][1]
                        # get and save real output when the proposed inputs are applied
                        real_output_list = self.get_real_trajectory_from_proposed_buffer(index_to_save, i_seg_buffer, global_initial_state)
                        results.add_predicted_error_slice(self.buffer_u_value[index_to_save][0], predicted_traj)
                        results.add_predicted_error_slack_slice(self.buffer_u_value[index_to_save][0], predicted_with_slack)
                        results.add_error_slice(self.buffer_u_value[index_to_save][0], real_output_list)
                        results._proposed_input_slices.append(self.buffer_u_value[index_to_save])
                    if self.save_dataset_after:
                        file_name = self.save_datasets()
                        results.saved_dataset_name = file_name
                    if self.stop_after_out_of_track:
                        print(f"Stop simulation after constraint not satisfied!")
                        results.end_simulation()

                        # reset parameters
                        self.Ts, self.L, self.steps, self.lag, self.slack = Ts, L, steps, lag, slack
                        return results
                elif np.all(A*x <= b) and self.out_of_track:
                    print(f"Constraint satisfied again at time {self.Ts*(i_block*self.steps+j)}.")
                    self.out_of_track = False

                # try step global system
                try:
                    crs_model.step(np.array(u_t).flatten())
                except RuntimeError as e:
                    print(f"Exception {e} raised during system simulation, returning partial results")
                    if self.save_dataset_after:
                        file_name = self.save_datasets()
                        results.saved_dataset_name = file_name
                    # calculate root mean square intervention and calcualtion time
                    results.end_simulation()

                    # reset parameters
                    self.Ts, self.L, self.steps, self.lag, self.slack = Ts, L, steps, lag, slack
                    return results

                # save states and inputs for filter
                initial_inputs_filter.append(u_t)
                initial_states_filter.append(crs_model.state)

                # save to results and trajectory, for initial condition of filter later
                results.add_point(u_obj_t, u_t,
                                  global_state, self.noise_list[i_steps+self.lag],
                                  error_dynamics_state, np.zeros(error_dynamics_state.shape),
                                  i_seg,)

        # calculate root mean square intervention and calcualtion time
        if self.save_dataset_after:
            file_name = self.save_datasets()
            results.saved_dataset_name = file_name
        results.end_simulation()
        if self.filter_type_list[0] in SafetyFilterTypes.output_hankel_matrix_types:
            results.H_uy, results.H_y_future = self.filter._safety_filters[0].get_datasets_hankel_matrix()

        # reset parameters
        self.Ts, self.L, self.steps, self.lag, self.slack = Ts, L, steps, lag, slack
        return results

    def save_datasets(self) -> Optional[str]:
        """Save the list of current datasets to a pickle file"""
        time = f"{datetime.datetime.now():%m-%d-%H-%M-%S}"
        file_name = time + "-" + self.track_fun_name + "-" + self.filter_type_list[0].name + ".pkl"
        io_data_list_list = []
        for single_filter in self.filter._safety_filters:
            io_data_list_list.append(single_filter._io_data_list)
        with open(os.path.join(os.getcwd(), 'datasets', file_name), 'wb') as f:
            pickle.dump(io_data_list_list, f)
        return file_name

    def load_datasets(self, file_name: str) -> bool:
        """
        Load the list of current datasets from a pickle file
        Directly replace current datasets in the filter
        """
        with open(os.path.join(os.getcwd(), 'datasets', file_name), 'rb') as read_file:
                io_data_list_list: List[List[IODataWith_l]] = pickle.load(read_file)
        if len(io_data_list_list) != len(self.filter._safety_filters):
            raise RuntimeError(f"Number of filters and io_data lists are different, cannot load datasets")
        for single_filter, io_data_list in zip(self.filter._safety_filters, io_data_list_list):
            single_filter._io_data_list = io_data_list

    def get_real_trajectory_from_proposed_buffer(self, i: int, i_seg: int, global_initial_state: np.ndarray) -> List[np.ndarray]:
        """
        get list of outputs if the proposed inputs are applied, with index i
        """
        sys_for_output = self.get_system(cur=self.systems[i_seg].cur, start_point=self.systems[i_seg].segment_start, system_type=self.simulate_model_type)
        sys_for_output.set_kinematic_model_state(global_initial_state)
        real_output_list = []
        u_proposed_list = self.buffer_u_value[i][1]
        for u in u_proposed_list:
            try:
                u = np.matrix(np.reshape(u, (sys_for_output.m, 1)))
                y, e_lin, n = sys_for_output.step_lin(u)
                y = np.array(y + e_lin).flatten()
                y[1] = y[1] * 180 / np.pi # from rad to deg
                y[2] = y[2] + self.v_0 # from deviation from steady state to actual velocity
                real_output_list.append(y)
            except RuntimeError as e:
                print(e, "during simulation of real system, returning partial results")
                break
        return real_output_list

    def get_predicted_outputs_from_buffer(self, i: int) -> Tuple[List[np.ndarray], List[np.ndarray]]:
        """
        get list of predicted outputs from the buffer, with index i
        return Tuple[list of predicted trajectory, list of predicted trajectory with slack]
        """
        predicted_traj = self.buffer_y_value[i][1]
        predicted_with_slack = [y-s for y,s in zip(self.buffer_y_value[i][1], self.buffer_sigma_value[i][1])]
        # transforms for readability
        for y in predicted_traj:
            y[1] = y[1] * 180 / np.pi # from rad to deg
            y[2] = y[2] + self.v_0 # from deviation from steady state to actual velocity
        for y in predicted_with_slack:
            y[1] = y[1] * 180 / np.pi # from rad to deg
            y[2] = y[2] + self.v_0 # from deviation from steady state to actual velocity
        return predicted_traj, predicted_with_slack

    def separate_proposed_u(self, m: int, filter_type: SafetyFilterTypes, u: np.ndarray) -> List[np.ndarray]:
        """Separate a large array containing proposed inputs into a list of inputs
        m: size of input
        filter_type: type of filter"""
        if filter_type in SafetyFilterTypes.direct_types:
            return np.split(u[m*self.lag:], self.L)
        else:
            return np.split(u, self.L)

    def separate_predicted_value(self, p: int, filter_type: SafetyFilterTypes, y: np.ndarray) -> List[np.ndarray]:
        """Separate a large array containing predicted trajectory into a list of outputs
        m: size of input
        filter_type: type of filter"""
        if filter_type in SafetyFilterTypes.direct_types:
            return np.split(y[self.lag*p:], self.L)
        else:
            return np.split(y, self.L)

    def update_predicted_buffer(self, t: float, filter_type: SafetyFilterTypes, safety_filter, i_seg: int, global_state: np.ndarray) -> None:
        """
        update the buffer used to store predicted trajectory and proposed inputs
        params are variables from the filter, will be separated according to the type of filter
        """
        u_value = safety_filter._u.value
        y_value = safety_filter._y.value
        sigma_value = safety_filter._sigma.value

        if len(self.buffer_y_value) >= self.num_prediction_buffer:
            self.buffer_y_value.pop(0)
            self.buffer_u_value.pop(0)
            self.buffer_sigma_value.pop(0)
            self.buffer_i_seg.pop(0)
            self.buffer_global_state.pop(0)

        self.buffer_y_value.append( (t, self.separate_predicted_value(safety_filter._p, filter_type, y_value)) )
        self.buffer_sigma_value.append( (t, self.separate_predicted_value(safety_filter._p, filter_type, sigma_value)) )
        self.buffer_u_value.append( (t, self.separate_proposed_u(safety_filter._m, filter_type, u_value)) )
        self.buffer_i_seg.append( (t, i_seg) )
        self.buffer_global_state.append( (t, global_state) )

    def propogate_track_gen(self) -> trackGenerator:
        self.track_generator = trackGenerator(self.density, self.track_width)
        # track_func = {
        #     'oval_track': oval_track,
        #     'round_track': round_track,
        # }.get(self.track_fun_name, oval_track)
        self.track_func(self.track_generator, t=1/self.cur, track_start_point=self.track_start)
        self.track_generator.populatePointsAndArcLength()
        return self.track_generator

    def get_input_obj(self, t: float, global_state: np.ndarray) -> np.matrix:
        """get next `steps` inputs

        :param t: simulation time
        :param xi_t: extended state of system
        :return: proposed u_obj for next `steps`
        """
        u_obj = np.matrix(np.ndarray((0,1)))
        throttle_sim = self.a_sim * self.m
        for j in range(self.steps):
            t_j = t + j*self.Ts

            if self.simulation_input_type == SimulationInputRule.SINE_WAVE:
                u_obj_k = np.matrix([[throttle_sim*math.sin(t_j*math.pi)],[self.delta_sim*math.sin(3*t_j*math.pi)]])
            elif self.simulation_input_type == SimulationInputRule.MAX_THROTTLE_SINE_STEER:
                u_obj_k = np.matrix([[throttle_sim],[self.delta_sim*math.sin(t_j*math.pi)]])
            elif self.simulation_input_type == SimulationInputRule.MAX_THROTTLE:
                u_obj_k = np.matrix([[throttle_sim],[0]])
            elif self.simulation_input_type == SimulationInputRule.SINE_WITH_MEAN:
                u_obj_k = np.matrix([[0.8*throttle_sim + throttle_sim*math.sin(t_j*math.pi)],[self.delta_sim*math.sin(t_j*math.pi)]])
            u_obj = np.vstack( (u_obj, u_obj_k) )
        return u_obj

    def get_system_list(self, system_type: ModelType = ModelType.KINEMATIC) -> List[FewOutPutErrorSystem]:
        system_list = []
        for segment in self.track_generator.chainOfSegments:
            if isinstance(segment, line):
                psi_0 = math.atan2(segment.y1-segment.y0, segment.x1-segment.x0, )
                start_point = np.array([segment.x0, segment.y0, psi_0])
                system_list.append(self.get_system(cur=0, start_point=start_point, system_type=system_type))
            elif isinstance(segment, arc):
                unit_from_center = np.array([np.cos(segment.theta_s), np.sin(segment.theta_s)])
                x_p0 = segment.x0 + unit_from_center[0]*segment.radius
                y_p0 = segment.y0 + unit_from_center[1]*segment.radius
                if segment.theta_f > segment.theta_s: # left hand turn
                    cur = segment.curvature
                    psi_0 = segment.theta_s + 0.5*math.pi
                else: # right hand turn
                    cur = -segment.curvature
                    psi_0 = segment.theta_s - 0.5*math.pi
                system_list.append(self.get_system(cur=cur, start_point=np.array([x_p0, y_p0, psi_0]), system_type=system_type))
        return system_list

    def get_system(self, cur: float, start_point: np.ndarray, system_type: ModelType = ModelType.KINEMATIC) -> FewOutPutErrorSystem:
        """get Linearized system for given curvature and start point

        start_point: array of the form [x, y, psi]
        """
        half_track_width = self.track_width/2
        if system_type == ModelType.KINEMATIC:
            system_params = LinearizedErrorKinematicAcceModelParams(
                    KinematicAcceModelParams(l_r=self.l_r,l_f=self.l_f,m=self.m,Ts=self.Ts),
                    cur = cur,
                    state_s0 = start_point,
                    v_0 = self.v_0,
                    A_u = np.matrix('1 0; 0 1; -1 0; 0 -1'),
                    b_u = np.matrix([[self.a_max*self.m],[self.delta_max],[-self.a_min*self.m],[self.delta_max]]),
                    A_y = np.matrix('1 0 0; 0 1 0; 0 0 1; -1 0 0; 0 -1 0; 0 0 -1'),
                    b_y = np.matrix([[half_track_width],[self.mu_max],[self.v_x_max-self.v_0],[half_track_width],[-self.mu_min], [-self.v_x_min+self.v_0]]),
                    A_n = np.matrix('1 0 0; 0 1 0; 0 0 1; -1 0 0; 0 -1 0; 0 0 -1'),
                    b_n = np.matrix([[self.n_e_lat_max],[self.n_mu_max],[self.n_v_max],[self.n_e_lat_max],[self.n_mu_max],[self.n_v_max]]),
                )
            # for this system, the global initial state does not matter
            return LinearizedErrorKinematicAcceModel(params=system_params, state_0=np.array([0,0,0,0]))
        elif system_type == ModelType.DYNAMIC_FEW_OUTPUT:
            system_params = DynamicErrorModelParams(
                    DynamicModelParams(Ts=self.Ts, l_f=self.l_f, l_r=self.l_r, m=self.m, Iz=self.Iz, Bf=self.Bf, Cf=self.Cf, Df=self.Df, Br=self.Br, Cr=self.Cr, Dr=self.Dr, Croll=self.Croll, Cm1=self.Cm1, Cm2=self.Cm2, Cd=self.Cd),
                    cur = cur,
                    segment_start = start_point,
                    v_0 = self.v_0,
                    A_u = np.matrix('1 0; 0 1; -1 0; 0 -1'),
                    b_u = np.matrix([[self.a_max*self.m],[self.delta_max],[-self.a_min*self.m],[self.delta_max]]),
                    A_y = np.matrix('1 0 0; 0 1 0; 0 0 1; -1 0 0; 0 -1 0; 0 0 -1'),
                    b_y = np.matrix([[half_track_width],[self.mu_max],[self.v_x_max-self.v_0],[half_track_width],[-self.mu_min], [-self.v_x_min+self.v_0]]),
                    A_n = np.matrix('1 0 0; 0 1 0; 0 0 1; -1 0 0; 0 -1 0; 0 0 -1'),
                    b_n = np.matrix([[self.n_e_lat_max],[self.n_mu_max],[self.n_v_max],[self.n_e_lat_max],[self.n_mu_max],[self.n_v_max]]),
            )
            # for this system, the global initial state does not matter
            return DynamicErrorModelFewOutput(params=system_params, initial_state=np.array([0,0,0,0,0,0]))
        elif system_type == ModelType.DYNAMIC:
            system_params = DynamicErrorModelParams(
                    DynamicModelParams(Ts=self.Ts, l_f=self.l_f, l_r=self.l_r, m=self.m, Iz=self.Iz, Bf=self.Bf, Cf=self.Cf, Df=self.Df, Br=self.Br, Cr=self.Cr, Dr=self.Dr, Croll=self.Croll, Cm1=self.Cm1, Cm2=self.Cm2, Cd=self.Cd),
                    cur = cur,
                    segment_start = start_point,
                    v_0 = self.v_0,
                    A_u = np.matrix('1 0; 0 1; -1 0; 0 -1'),
                    b_u = np.matrix([[self.a_max*self.m],[self.delta_max],[-self.a_min*self.m],[self.delta_max]]),
                    A_y = np.matrix('1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1; -1 0 0 0; 0 -1 0 0; 0 0 -1 0; 0 0 0 -1'),
                    b_y = np.matrix([[half_track_width],[self.mu_max],[self.v_x_max-self.v_0], [self.v_y_max], [half_track_width],[-self.mu_min],[-self.v_x_min+self.v_0],[-self.v_y_min]]),
                    A_n = np.matrix('1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1; -1 0 0 0; 0 -1 0 0; 0 0 -1 0; 0 0 0 -1'),
                    b_n = np.matrix([[self.n_e_lat_max],[self.n_mu_max],[self.n_v_max/1.2],[self.n_v_max/1.2],[self.n_e_lat_max],[self.n_mu_max],[self.n_v_max/1.2],[self.n_v_max/1.2]]),
            )
            # for this system, the global initial state does not matter
            return DynamicErrorModelVxy(params=system_params, initial_state=np.array([0,0,0,0,0,0]))

    def get_io_data_dic(self) -> Dict[float, IODataWith_l]:
        if self.use_saved_data: # use saved data
            with open(os.path.join(os.getcwd(), 'datasets', f'io_datas_{self.t_data}_{self.Ts}.pkl'), 'rb') as read_file:
                io_data_dict: Dict[float, IODataWith_l] = pickle.load(read_file)
        else: # generate new data
            io_data_dict = {}
            np.random.seed(self.random_seed)
            for segment in self.track_generator.chainOfSegments:
                if isinstance(segment, line):
                    cur = 0
                elif segment.theta_f < segment.theta_s: # right hand turn
                    cur = -segment.curvature
                elif segment.theta_f > segment.theta_s: # left hand turn
                    cur = segment.curvature
                sys = self.get_system(cur=cur, start_point=np.array([0,0,0]), system_type=self.data_model_type)
                zero_input = sys.get_zero_input()
                zero_state = sys.get_zero_state()
                sys.set_error_state(zero_state)
                length = int(self.t_data/self.Ts)
                u_0_max = self.a_d_max*self.m
                io_data = IODataWith_l(
                            self.L+self.lag, sys=sys, input_rule=self.data_input_rule, mean_input=np.matrix(zero_input).transpose(), length=length,
                            A_u_d=np.matrix('1 0; 0 1; -1 0; 0 -1'), b_u_d=np.matrix([[u_0_max],[self.delta_d_max],[u_0_max],[self.delta_d_max]]),
                            d_u_max=np.matrix([[self.a_d_max*self.m],[self.delta_d_max]]),
                            n_l = self.n_l_max, lag = self.lag, L = self.L, N_l = int(length/2), K_l = 5)
                io_data_dict[cur] = io_data
                if self.save_data:
                    with open(os.path.join(os.getcwd(), 'datasets', f'io_datas_{self.t_data}_{self.Ts}.pkl'), 'wb') as file:
                        pickle.dump(io_data_dict, file)
        return io_data_dict

    def get_filter(self, **kwargs):
        if self.track_filter_type == TrackFilterTypes.SINGLE_SEGMENT:
            filter_list = []
            num_segments = len(self.track_generator.chainOfSegments)
            num_given_filter_types = len(self.filter_type_list)
            if num_given_filter_types < num_segments:
                self.filter_type_list = self.filter_type_list + [self.filter_type_list[-1]] * (num_segments - num_given_filter_types)
            for segment, filter_type, system in zip(
                self.track_generator.chainOfSegments, self.filter_type_list, self.systems
                ):
                if filter_type not in SafetyFilterTypes.feedin_xi_types:
                    print(f"Warning: filter type {filter_type} is not supported for track filter type {self.track_filter_type}.")
                    return None
                if isinstance(segment, line):
                    filter_list.append(self.get_single_filter(0, filter_type, system, deepcopy(self.io_data_dict[0]), **kwargs))
                elif isinstance(segment, arc):
                    if segment.theta_f > segment.theta_s: # left hand turn
                        cur = segment.curvature
                    elif segment.theta_f < segment.theta_s: # right hand turn
                        cur = -segment.curvature
                    filter_list.append(self.get_single_filter(0, filter_type, system, deepcopy(self.io_data_dict[cur]), **kwargs))
            self.filter_list = filter_list
            filter = SafetyFilterForTrack(
                track_fun=self.track_func,
                t = 1/self.cur,
                track_start = self.track_start,
                density = self.density,
                filter_list=filter_list,
                # io_datas=self.io_data_dict,
                systems=self.systems,
                slack=self.slack,
                position_0=self.global_initial_state
            )
        elif self.track_filter_type == TrackFilterTypes.SINGLE_SEGMENT_ADD_DATA:
            filter_list = []
            num_segments = len(self.track_generator.chainOfSegments)
            num_given_filter_types = len(self.filter_type_list)
            if num_given_filter_types < num_segments:
                self.filter_type_list = self.filter_type_list + [self.filter_type_list[-1]] * (num_segments - num_given_filter_types)
            for segment, filter_type, system in zip(
                self.track_generator.chainOfSegments, self.filter_type_list, self.systems
                ):
                if filter_type not in SafetyFilterTypes.feedin_list_state_types:
                    print(f"Warning: filter type {filter_type} is not supported for track filter type {self.track_filter_type}.")
                    return None
                if isinstance(segment, line):
                    filter_list.append(self.get_single_filter(0, filter_type, system, deepcopy(self.io_data_dict[0]), **kwargs))
                elif isinstance(segment, arc):
                    if segment.theta_f > segment.theta_s: # left hand turn
                        cur = segment.curvature
                    elif segment.theta_f < segment.theta_s: # right hand turn
                        cur = -segment.curvature
                    filter_list.append(self.get_single_filter(0, filter_type, system, deepcopy(self.io_data_dict[cur]), **kwargs))
            self.filter_list = filter_list
            filter = SafetyFilterForTrackAddData(
                track_fun=self.track_func,
                t = 1/self.cur,
                track_start = self.track_start,
                density = self.density,
                filter_list=filter_list,
                # io_datas=self.io_data_dict,
                systems=self.systems,
                slack=self.slack,
                position_0=self.global_initial_state
            )

        return filter

    def get_noise(self) -> List[np.ndarray]:
        length = self.lag + int(self.t_sim/self.Ts)
        np.random.seed(self.random_seed)
        if self.simulate_model_type in ModelType.few_output_models:
            return [np.array([
                                self.n_x*(2*np.random.rand()-1),
                                self.n_y*(2*np.random.rand()-1),
                                self.n_psi*(2*np.random.rand()-1),
                                self.n_v*(2*np.random.rand()-1),
                            ]) for _ in range(length)]
        else:
            return [np.array([
                                self.n_x*(2*np.random.rand()-1),
                                self.n_y*(2*np.random.rand()-1),
                                self.n_psi*(2*np.random.rand()-1),
                                self.n_v*(2*np.random.rand()-1)/1.2,
                                self.n_v*(2*np.random.rand()-1)/1.2,
                                0,
                            ]) for _ in range(length)]

    def set_params_from_dict(self, **kwargs) -> None:
        """
        Get parameters based on default values and kwargs \\
        return (Ts, L, steps, lag, slack)
        """
        self.Ts = kwargs.get('Ts', self.Ts)
        self.L = kwargs.get('L', self.L)
        self.steps = kwargs.get('steps', self.steps)
        self.lag = kwargs.get('lag', self.lag)
        self.slack = kwargs.get('slack', self.slack)